From a9f34b00546f7f869879da04e2d6792d6d0c3f49 Mon Sep 17 00:00:00 2001 From: Alexander Klein Date: Thu, 25 Nov 2021 15:01:20 +0100 Subject: [PATCH] added debug class for time measurment --- src/config.h | 9 +++--- src/debugTimes.cpp | 11 +++++++ src/debugTimes.h | 18 ++++++++++++ src/driveModi/manualControl.cpp | 14 +++++++-- src/driveModi/manualControl.h | 1 + src/main.cpp | 52 ++++++++++++--------------------- src/moveControl.cpp | 4 +-- 7 files changed, 67 insertions(+), 42 deletions(-) create mode 100644 src/debugTimes.cpp create mode 100644 src/debugTimes.h diff --git a/src/config.h b/src/config.h index 46c965a..dc75ef9 100644 --- a/src/config.h +++ b/src/config.h @@ -1,3 +1,4 @@ +// Global config file // The front is where the boards are @@ -48,10 +49,10 @@ //MoveControl config #define RUN_MOVE_CONTROL_DELAY 10 #define ACCELERATE_STEPS 1 -#define PID_LEFT_P 40 +#define PID_LEFT_P 85 #define PID_LEFT_I 0 #define PID_LEFT_D 0 -#define PID_RIGHT_P 40 +#define PID_RIGHT_P 75 #define PID_RIGHT_I 0 #define PID_RIGHT_D 0 #define PID_OUT_MIN -100 @@ -60,8 +61,8 @@ //ManualControl config #define RUN_MANUALCONTROL_DELAY 10 // normaly 10 -#define MANUALCONTROL_MAX_SPEED 1.0 -#define MANUALCONTROL_MAX_ROTATION 1.0 +#define MANUALCONTROL_MAX_SPEED 1.0 // m/s +#define MANUALCONTROL_MAX_ROTATION 7.0 // rad/s //MQTT global config #define MQTT_BUFFER_SITE 128 diff --git a/src/debugTimes.cpp b/src/debugTimes.cpp new file mode 100644 index 0000000..ffa189b --- /dev/null +++ b/src/debugTimes.cpp @@ -0,0 +1,11 @@ +#include "debugTimes.h" + +DebugTimes::DebugTimes() { + this->startTime = millis(); +} + +void DebugTimes::stop(const char* name, uint16_t minTime) { + uint64_t time = millis() - this->startTime; + if (time > minTime) + Serial.printf("%s needs %llu ms\n", name, time); +} \ No newline at end of file diff --git a/src/debugTimes.h b/src/debugTimes.h new file mode 100644 index 0000000..9aca030 --- /dev/null +++ b/src/debugTimes.h @@ -0,0 +1,18 @@ +#ifndef DEBUG_TIMES_H +#define DEBUG_TIMES_H + +#include +#include + +#include + +class DebugTimes { + public: + DebugTimes(); + void stop(const char* name, uint16_t minTime = 0); + + private: + uint64_t startTime; +}; + +#endif //DEBUG_TIMES_H \ No newline at end of file diff --git a/src/driveModi/manualControl.cpp b/src/driveModi/manualControl.cpp index f5478a6..df44411 100644 --- a/src/driveModi/manualControl.cpp +++ b/src/driveModi/manualControl.cpp @@ -15,6 +15,8 @@ void ManualControl::init(MoveControl *moveControl, MotorControl *motorControlLef } void ManualControl::runManualControl() { + DebugTimes runManualDebuginFunction; + //Cancel if delay is not reached if (millis() - this->last_millis < RUN_MANUALCONTROL_DELAY) { return; @@ -27,6 +29,8 @@ void ManualControl::runManualControl() { } this->last_millis = millis(); + runManualDebuginFunction.stop("Ober teil von runManualControl", 100); + //Change controlMode if (this->controlMode == ControMode::halt && Ps3.data.button.l3) { this->reset(); @@ -51,9 +55,15 @@ void ManualControl::runManualControl() { this->moveControl->setDrivingStatus(DrivingStatus::stop); break; - case ControMode::joystick: + case ControMode::joystick: { + DebugTimes driveWithJoyDebug; this->driveWithControllerJoystick(); + driveWithJoyDebug.stop("driveWithControllerJoystick", 100); + + DebugTimes moveControllDebug; this->moveControl->runMoveControl(); + moveControllDebug.stop("moveControl", 100); + } break; case ControMode::trigger: @@ -78,7 +88,7 @@ void ManualControl::driveWithControllerJoystick() { // debug->sendMsg(Loglevel::debug, str); // Kommt 1 raus bei vollausschlag value_per_step = MANUALCONTROL_MAX_ROTATION * 2 / 256; - this->moveControl->setRotationspeed(x * value_per_step); + this->moveControl->setRotationspeed(-x * value_per_step); } void ManualControl::changeDriveMode(ControMode drive_mode) { diff --git a/src/driveModi/manualControl.h b/src/driveModi/manualControl.h index 83fb157..89feff8 100644 --- a/src/driveModi/manualControl.h +++ b/src/driveModi/manualControl.h @@ -6,6 +6,7 @@ #include "moveControl.h" #include "hardware/motorControl.h" #include "debugMqtt.h" +#include "debugTimes.h" enum ControMode {halt, joystick, trigger}; class ManualControl { diff --git a/src/main.cpp b/src/main.cpp index b20e962..7d06c72 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -15,6 +15,7 @@ #include "moveControl.h" #include "route.h" #include "debugMqtt.h" +#include "debugTimes.h" void callbackControllerAction(); void callbackControllerConnect(); @@ -22,22 +23,6 @@ void controllerPrintBattery(); bool reconnectMqtt(); -// TODO: This is a temporary function for testing -void checkTimes(bool set, uint16_t maxTime,const char *name) { - static uint64_t lastMillis = 0; - if (set) { - lastMillis = millis(); - } else { - uint64_t pastTime = millis() - lastMillis; - if (pastTime > maxTime) { - Serial.print("Function: "); - Serial.print(name); - Serial.print("-Time: "); - Serial.println(pastTime); - } - } -} - MotorControl left_motor; MotorControl right_motor; Speedometer speedometer_left; @@ -90,6 +75,7 @@ void setup() { // mqtt_client.setServer(mqtt_server, MQTT_PORT); mqtt_client.setServer(MQTT_SERVER, MQTT_PORT); + mqtt_client.setSocketTimeout(1); reconnectMqtt(); //FIXME: make it from config.h @@ -120,15 +106,14 @@ void setup() { consolControl.init(&moveController); //Test checkTimes function - checkTimes(true, 0, ""); + DebugTimes testDebugTimes; delay(2); - checkTimes(false, 1, "Test checkTimes"); - - //Mqtt set down Sockettimeout - mqtt_client.setSocketTimeout(1); + testDebugTimes.stop("Test checkTimes"); } void loop() { + + if (!mqtt_client.connected()) { long now = millis(); if (now - lastReconnectAttempt > MQTT_TIME_RECONNECT) { @@ -140,31 +125,30 @@ void loop() { } } else { // Client connected - checkTimes(true, 0, ""); + DebugTimes mqttClientLoop; mqtt_client.loop(); - checkTimes(false, 100, "mqttloop"); + mqttClientLoop.stop("mqttloop", 100); } - checkTimes(true, 0, ""); + DebugTimes runMotorLeft; left_motor.runMotorControl(); - checkTimes(false, 100, "left_motor"); - checkTimes(true, 0, ""); + runMotorLeft.stop("left_motor", 100); + + DebugTimes runMotorRight; right_motor.runMotorControl(); - checkTimes(false, 100, "right_motor"); - checkTimes(true, 0, ""); + runMotorRight.stop("right_motor", 100); + speedometer_left.runSpeedometer(); - checkTimes(false, 100, "left_speed"); - checkTimes(true, 0, ""); speedometer_right.runSpeedometer(); - checkTimes(false, 100, "right_speed"); // TODO: Auskommentiert weil macht vielleicht komische Sachen // route.runRoute(); switch (driveMode) { - case manualControl_e: - checkTimes(true, 0, ""); + case manualControl_e: { + DebugTimes manControlDebug; manualControl.runManualControl(); - checkTimes(false, 100, "manualControl run"); + manControlDebug.stop("manualControl run", 100); + } break; case captureRoute_e: diff --git a/src/moveControl.cpp b/src/moveControl.cpp index 61f755e..22d8c8c 100644 --- a/src/moveControl.cpp +++ b/src/moveControl.cpp @@ -51,7 +51,7 @@ void MoveControl::runMoveControl() { static uint32_t functioncalls = 0; functioncalls++; - if (functioncalls % 20 == 0) { + if (functioncalls % 50 == 0) { char str[128]; 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, @@ -73,7 +73,7 @@ void MoveControl::setSpeed(double speed) { } void MoveControl::setRotationspeed(double speed) { - if (speed < 0.2 && speed > -0.2) + if (speed < 0.7 && speed > -0.7) this->rotation_speed = 0; else this->rotation_speed = speed;