EmaMaker
/
SPQR-Team-2019-REVAMPED
1
1
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

drive: only drive when the PID is calculated 

no need to recalibrate the pid
pull/1/head
EmaMaker 2021-05-10 20:37:13 +02:00
parent 714b00ee25
commit 15ba2991a1
2 changed files with 42 additions and 42 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/main.cpp
View File

@ -52,7 +52,7 @@ void loop() {
striker->play(striker_condition); striker->play(striker_condition);
keeper->play(keeper_condition); keeper->play(keeper_condition);
// testmenu->testMenu(); testmenu->testMenu();
// Last thing to do: movement and update status vector // Last thing to do: movement and update status vector
drive->drivePrepared(); drive->drivePrepared();

82
src/motors_movement/drivecontroller.cpp
View File

@ -96,54 +96,54 @@ void DriveController::drive(int dir, int speed, int tilt){
input = delta; input = delta;
setpoint = tilt; setpoint = tilt;
pid->Compute(); if(pid->Compute()){
pidfactor = -output;
speed1 += pidfactor;
speed2 += pidfactor;
speed3 += pidfactor;
speed4 += pidfactor;
pidfactor = -output; // Find the maximum speed and scale all of them for the maximum to be 255
speed1 += pidfactor; float maxVel = 0;
speed2 += pidfactor; maxVel = max(abs(speed1), maxVel);
speed3 += pidfactor; maxVel = max(abs(speed2), maxVel);
speed4 += pidfactor; maxVel = max(abs(speed3), maxVel);
maxVel = max(abs(speed4), maxVel);
// Find the maximum speed and scale all of them for the maximum to be 255 if(maxVel > 255){
float maxVel = 0; // Ratio to 255
maxVel = max(abs(speed1), maxVel); float ratio = maxVel/255;
maxVel = max(abs(speed2), maxVel);
maxVel = max(abs(speed3), maxVel);
maxVel = max(abs(speed4), maxVel);
if(maxVel > 255){ // //Scale all the velocities
// Ratio to 255 speed1 /= ratio;
float ratio = maxVel/255; speed2 /= ratio;
speed3 /= ratio;
speed4 /= ratio;
// //Scale all the velocities // DEBUG.print(speed1);
speed1 /= ratio; // DEBUG.print(" | ");
speed2 /= ratio; // DEBUG.print(speed2);
speed3 /= ratio; // DEBUG.print(" | ");
speed4 /= ratio; // DEBUG.print(speed3);
// DEBUG.print(" | ");
// DEBUG.print(speed4);
// DEBUG.print(" | ");
// DEBUG.println(maxVel);
}
// DEBUG.print(speed1); speed1 = constrain(speed1, -255, 255);
// DEBUG.print(" | "); speed2 = constrain(speed2, -255, 255);
// DEBUG.print(speed2); speed3 = constrain(speed3, -255, 255);
// DEBUG.print(" | "); speed4 = constrain(speed4, -255, 255);
// DEBUG.print(speed3);
// DEBUG.print(" | "); m1->drive((int) speed1);
// DEBUG.print(speed4); m2->drive((int) speed2);
// DEBUG.print(" | "); m3->drive((int) speed3);
// DEBUG.println(maxVel); m4->drive((int) speed4);
oldSpeed = speed;
} }
speed1 = constrain(speed1, -255, 255);
speed2 = constrain(speed2, -255, 255);
speed3 = constrain(speed3, -255, 255);
speed4 = constrain(speed4, -255, 255);
m1->drive((int) speed1);
m2->drive((int) speed2);
m3->drive((int) speed3);
m4->drive((int) speed4);
oldSpeed = speed;
CURRENT_DATA_WRITE.dir = dir; CURRENT_DATA_WRITE.dir = dir;
CURRENT_DATA_WRITE.speed = speed; CURRENT_DATA_WRITE.speed = speed;
CURRENT_DATA_WRITE.tilt = tilt; CURRENT_DATA_WRITE.tilt = tilt;