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);
keeper->play(keeper_condition);
// testmenu->testMenu();
testmenu->testMenu();
// Last thing to do: movement and update status vector
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;
setpoint = tilt;
pid->Compute();
if(pid->Compute()){
pidfactor = -output;
speed1 += pidfactor;
speed2 += pidfactor;
speed3 += pidfactor;
speed4 += pidfactor;
pidfactor = -output;
speed1 += pidfactor;
speed2 += pidfactor;
speed3 += pidfactor;
speed4 += pidfactor;
// Find the maximum speed and scale all of them for the maximum to be 255
float maxVel = 0;
maxVel = max(abs(speed1), maxVel);
maxVel = max(abs(speed2), maxVel);
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
float maxVel = 0;
maxVel = max(abs(speed1), maxVel);
maxVel = max(abs(speed2), maxVel);
maxVel = max(abs(speed3), maxVel);
maxVel = max(abs(speed4), maxVel);
if(maxVel > 255){
// Ratio to 255
float ratio = maxVel/255;
if(maxVel > 255){
// Ratio to 255
float ratio = maxVel/255;
// //Scale all the velocities
speed1 /= ratio;
speed2 /= ratio;
speed3 /= ratio;
speed4 /= ratio;
// //Scale all the velocities
speed1 /= ratio;
speed2 /= ratio;
speed3 /= ratio;
speed4 /= ratio;
// DEBUG.print(speed1);
// DEBUG.print(" | ");
// DEBUG.print(speed2);
// DEBUG.print(" | ");
// DEBUG.print(speed3);
// DEBUG.print(" | ");
// DEBUG.print(speed4);
// DEBUG.print(" | ");
// DEBUG.println(maxVel);
}
// DEBUG.print(speed1);
// DEBUG.print(" | ");
// DEBUG.print(speed2);
// DEBUG.print(" | ");
// DEBUG.print(speed3);
// DEBUG.print(" | ");
// DEBUG.print(speed4);
// DEBUG.print(" | ");
// DEBUG.println(maxVel);
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;
}
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.speed = speed;
CURRENT_DATA_WRITE.tilt = tilt;