added debug class for time measurment

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

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);
+}

src/debugTimes.h

@@ -0,0 +1,18 @@
+#ifndef DEBUG_TIMES_H
+#define DEBUG_TIMES_H
+
+#include <list>
+#include <cstdint>
+
+#include <Arduino.h>
+
+class DebugTimes {
+    public:
+        DebugTimes();
+        void stop(const char* name, uint16_t minTime = 0);
+
+    private:
+        uint64_t startTime;
+};
+
+#endif //DEBUG_TIMES_H

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) {

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 {

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:

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;