checking Times in moveControl

src/moveControl.cpp

@@ -44,10 +44,14 @@ void MoveControl::runMoveControl() {
     this->updateWheelSpeed();
     this->calcWheelSpeed();
 
+    DebugTimes pidTimes;
     this->right_pid->Compute();
     this->left_pid->Compute();
+    pidTimes.stop("PID Calulate", 100);
 
+    DebugTimes regMotorTime;
     this->regulateMotors();
+    regMotorTime.stop("regulateMotors", 100);
 
     static uint32_t functioncalls = 0;
     functioncalls++;
@@ -56,7 +60,9 @@ void MoveControl::runMoveControl() {
         sprintf(str, "x_speed: %3.2f, rotation_speed: %3.2f, left: %3.2f, right: %3.2f",
             this->x_speed, this->rotation_speed, this->wheelspeed_left,
             this->wheelspeed_right);
+        DebugTimes debugMsgTime;
         debug->sendMsg(Loglevel::debug, str);
+        debugMsgTime.stop("debug Msg", 100);
     }
 }