From 732298b7db9c461750e947ed9d00c7ff6f5264b6 Mon Sep 17 00:00:00 2001 From: EmaMaker Date: Mon, 11 Jan 2021 15:47:18 +0100 Subject: [PATCH] movement: correct pid formula and motors angles --- src/motors_movement/drivecontroller.cpp | 4 ++-- src/sensors/sensors.cpp | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/motors_movement/drivecontroller.cpp b/src/motors_movement/drivecontroller.cpp index 45df50e..e1a25bb 100644 --- a/src/motors_movement/drivecontroller.cpp +++ b/src/motors_movement/drivecontroller.cpp @@ -85,8 +85,8 @@ void DriveController::drive(int dir, int speed, int tilt){ speed1 = ((vx * sins[m1->angle] ) + (vy * cosins[m1->angle] )); speed2 = ((vx * sins[m2->angle]) + (vy * cosins[m2->angle])); - speed3 = -(speed1); - speed4 = -(speed2); + speed3 = ((vx * sins[m3->angle]) + (vy * cosins[m3->angle])); + speed4 = ((vx * sins[m4->angle]) + (vy * cosins[m4->angle])); // Calculate position error relative to the 0 delta = CURRENT_DATA_READ.IMUAngle; diff --git a/src/sensors/sensors.cpp b/src/sensors/sensors.cpp index a89c522..88a52a0 100644 --- a/src/sensors/sensors.cpp +++ b/src/sensors/sensors.cpp @@ -9,8 +9,8 @@ void initSensors(){ pinMode(LED_Y, OUTPUT); pinMode(LED_G, OUTPUT); - drive = new DriveController(new Motor(12, 11, 4, 55), new Motor(25, 24, 5, 135), new Motor(27, 26, 2, 225), new Motor(29, 28, 3, 305)); - //drive = new DriveController(new Motor(12, 11, 2, 45),new Motor(25, 24, 5, 135), new Motor(27, 26, 6, 225), new Motor(21, 22, 23, 315)); + drive = new DriveController(new Motor(11, 12, 4, 55), new Motor(24, 25, 5, 135), new Motor(26, 27, 2, 225), new Motor(28, 29, 3, 305)); + //drive = new DriveController(new Motor(12, 11, 2, 45),new Motor(25, 24, 5, 135), new Motor(27, 26, 6, 225), new Motor(21, 22, 23, 315)); compass = new DataSourceBNO055(); ball = new DataSourceBall(&Serial2, 57600); //ball = new DataSourceBall(&Serial4, 57600);