a lot of bullshit

src/moveControl.cpp

@@ -1,9 +1,11 @@
 #include "moveControl.h"
 
-#include <Arduino.h>
-
 MoveControl::MoveControl() {
-    this->debug = new DebugMqtt("MoveControl");
+    this->left_motor = new MotorControl;
+    this->right_motor = new MotorControl;
+    this->left_speedometer = new Speedometer;
+    this->right_speedometer = new Speedometer;
+
     this->left_pid = new PID(   &this->wheelspeed_left,
                                 &this->left_pid_out,
                                 &this->wheelspeed_left_target,
@@ -21,49 +23,42 @@ MoveControl::MoveControl() {
 
     this->right_pid->SetOutputLimits(PID_OUT_MIN, PID_OUT_MAX);
     this->right_pid->SetSampleTime(PID_SAMPLETIME);
-    this->right_pid->SetMode(AUTOMATIC);                                
+    this->right_pid->SetMode(AUTOMATIC); 
+
+    this->left_motor->init(M_PWM_1, PWM_CHANNEL_M1, M_DIR_11, M_DIR_12);
+    this->right_motor->init(M_PWM_2, PWM_CHANNEL_M2, M_DIR_21, M_DIR_22);
+
+    this->left_speedometer->init(M_ENCODE_1A, M_ENCODE_1B, WHEEL_DIAMETER, ENC_STEPS);
+    this->right_speedometer->init(M_ENCODE_2A, M_ENCODE_2B, WHEEL_DIAMETER, ENC_STEPS);                               
 }
 
-void MoveControl::init(MotorControl *left_motor, MotorControl *right_motor,
-                        Speedometer *left_speedometer, Speedometer *right_speedometer) {
-    debug->sendMsg(Loglevel::info, "Init...");
-    this->left_motor = left_motor;
-    this->right_motor = right_motor;
-    this->left_speedometer = left_speedometer;
-    this->right_speedometer = right_speedometer;
-    debug->sendMsg(Loglevel::info, "Init finished!");
+MoveControl::~MoveControl() {
+    this->left_motor->emergencyStop();
+    this->right_motor->emergencyStop();
+}
+
+void MoveControl::loop() {
+    this->left_motor->loop();
+    this->right_motor->loop();
+    this->left_speedometer->loop();
+    this->right_speedometer->loop();
+
+    static uint64_t last_millis = 0;
+    if (millis() - last_millis < delay) 
+        return;
+
+    this->runMoveControl();
+    last_millis = millis();
 }
 
 void MoveControl::runMoveControl() {
-        static uint64_t last_millis = 0;
-    if (millis() - last_millis < RUN_MOVE_CONTROL_DELAY) {
-        return;
-    }
-    last_millis = millis();
+    this->updateCurrentWheelSpeed();
+    this->calcTargetWheelSpeed();
 
-    this->updateWheelSpeed();
-    this->calcWheelSpeed();
-
-    DebugTimes pidTimes;
     this->right_pid->Compute();
     this->left_pid->Compute();
-    pidTimes.stopConsol("PID Calulate", 100);
 
-    DebugTimes regMotorTime;
     this->regulateMotors();
-    regMotorTime.stopConsol("regulateMotors", 100);
-
-    static uint32_t functioncalls = 0;
-    functioncalls++;
-    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,
-            this->wheelspeed_right);
-        DebugTimes debugMsgTime;
-        debug->sendMsg(Loglevel::debug, str);
-        debugMsgTime.stopConsol("debug Msg", 100);
-    }
 }
 
 void MoveControl::setDrivingStatus(DrivingStatus status) {
@@ -75,7 +70,6 @@ void MoveControl::setSpeed(double speed) {
         this->x_speed = 0;
     else
         this->x_speed = speed;
-    // Serial.printf("New speed: %f in moveControll.cpp \n", speed);
 }
 
 void MoveControl::setRotationspeed(double speed) {
@@ -83,7 +77,6 @@ void MoveControl::setRotationspeed(double speed) {
         this->rotation_speed = 0;
     else
         this->rotation_speed = speed;
-    
 }
 
 void MoveControl::setPidTunings(uint8_t side, double p, double i, double d) {
@@ -94,10 +87,9 @@ void MoveControl::setPidTunings(uint8_t side, double p, double i, double d) {
         selectedPID = this->right_pid;
 
     selectedPID->SetTunings(p, i, d);
-    
 }
 
-void MoveControl::calcWheelSpeed() {
+void MoveControl::calcTargetWheelSpeed() {
     /* original formula:
        (1 / r) / 1  b \ / x \ = / Xl \
                \ 1 -b / \ T /   \ Xr /  */
@@ -109,8 +101,6 @@ void MoveControl::calcWheelSpeed() {
 
     this->wheelspeed_right_target = (A * this->x_speed + B * this->rotation_speed) * (WHEEL_DIAMETER / 2);
     this->wheelspeed_left_target  = (A * this->x_speed + (-B) * this->rotation_speed) * (WHEEL_DIAMETER / 2);
-    //debug->writeToInflux("moveControl", "wheelspeed_right_target", this->wheelspeed_right_target);
-    //debug->writeToInflux("moveControl", "wheelspeed_left_target", this->wheelspeed_left_target);
 }
 
 void MoveControl::regulateMotors() {
@@ -132,7 +122,7 @@ void MoveControl::regulateMotors() {
     }
 }
 
-void MoveControl::updateWheelSpeed() {
+void MoveControl::updateCurrentWheelSpeed() {
     this->wheelspeed_left = this->left_speedometer->getSpeed();
     this->wheelspeed_right = this->right_speedometer->getSpeed();
 }