drive: only drive when the PID is calculated

no need to recalibrate the pid

src/main.cpp

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

src/motors_movement/drivecontroller.cpp

@@ -96,8 +96,7 @@ void DriveController::drive(int dir, int speed, int tilt){
     input = delta;
     setpoint = tilt;
     
-    pid->Compute();
-
+    if(pid->Compute()){
         pidfactor = -output;
         speed1 += pidfactor;
         speed2 += pidfactor;
@@ -143,6 +142,7 @@ void DriveController::drive(int dir, int speed, int tilt){
         m4->drive((int) speed4);
 
         oldSpeed = speed;
+    }
 
     CURRENT_DATA_WRITE.dir = dir;
     CURRENT_DATA_WRITE.speed = speed;