diff --git a/src/driveModi/manualControl.cpp b/src/driveModi/manualControl.cpp index df44411..ec6debc 100644 --- a/src/driveModi/manualControl.cpp +++ b/src/driveModi/manualControl.cpp @@ -56,13 +56,13 @@ void ManualControl::runManualControl() { break; case ControMode::joystick: { - DebugTimes driveWithJoyDebug; + // DebugTimes driveWithJoyDebug; this->driveWithControllerJoystick(); - driveWithJoyDebug.stop("driveWithControllerJoystick", 100); + // driveWithJoyDebug.stop("driveWithControllerJoystick", 100); - DebugTimes moveControllDebug; + // DebugTimes moveControllDebug; this->moveControl->runMoveControl(); - moveControllDebug.stop("moveControl", 100); + // moveControllDebug.stop("moveControl", 100); } break; diff --git a/src/moveControl.cpp b/src/moveControl.cpp index 22d8c8c..aa58239 100644 --- a/src/moveControl.cpp +++ b/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); } } diff --git a/src/moveControl.h b/src/moveControl.h index c608315..5b02f71 100644 --- a/src/moveControl.h +++ b/src/moveControl.h @@ -8,6 +8,7 @@ #include "hardware/speedometer.h" #include "config.h" #include "debugMqtt.h" +#include "debugTimes.h" enum DrivingStatus {stop, drive};