EmaMaker
/
SPQR-Team1-2020
1
1
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

movement: correct pid formula and motors angles

docs
EmaMaker 2021-01-11 15:47:18 +01:00
parent 62821341b0
commit 732298b7db
2 changed files with 4 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/motors_movement/drivecontroller.cpp
View File

@ -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;

2
src/sensors/sensors.cpp
View File

@ -9,7 +9,7 @@ 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(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);