improved moveControl

src/main.cpp

@@ -10,7 +10,6 @@
 #include <DabbleESP32.h>
 
 void gamepadInput();
-int8_t modSpeed(int8_t dir, int8_t speed);
 
 MotorControl left_motor;
 MotorControl right_motor;
@@ -34,88 +33,43 @@ void setup() {
 }
 
 void loop() {
-  if (millis() - last_millis > 700) {
+  if (millis() - last_millis > 400) {
     gamepadInput();
     // left_motor.toString();
+    // Serial.printf("speed: %f, ", speedometer_right.getSpeed());
     // right_motor.toString();
     // Serial.printf("Speed L: %f, Speed R: %f \n", speedometer_left.getSpeed(), speedometer_right.getSpeed());
 
     last_millis = millis();
   }
 
-  left_motor.runMotorControl();
+  // left_motor.runMotorControl();
   right_motor.runMotorControl();
-  speedometer_left.runSpeedometer();
+  // speedometer_left.runSpeedometer();
   speedometer_right.runSpeedometer();
-  moveController.runMoveControl();
+  // moveController.runMoveControl();
 }
 
 void gamepadInput() {
   Dabble.processInput();             //this function is used to refresh data obtained from smartphone.Hence calling this function is mandatory in order to get data properly from your mobile.
   //Serial.print("KeyPressed: ");
 
-  static int8_t speed_left = 0;
   if (GamePad.isUpPressed()) {
-    speed_left = modSpeed(1, speed_left);
+    moveController.setSpeed(1.3);
   } else if (GamePad.isDownPressed()) {
-    speed_left = modSpeed(-1, speed_left);
+    moveController.setSpeed(0);
   } else {
-    speed_left = modSpeed(0, speed_left);
   }
 
-  static int8_t speed_right = 0;
   if (GamePad.isTrianglePressed()) {
-    speed_right = modSpeed(1, speed_right);
+    right_motor.setTargetSpeed(20);
   } else if (GamePad.isCrossPressed()) {
-    speed_right = modSpeed(-1, speed_right);
+    right_motor.setTargetSpeed(30);
-  } else {
+  } else if (GamePad.isCirclePressed()){
-    speed_right = modSpeed(0, speed_right);
+    right_motor.setTargetSpeed(0);
   }
 
-  left_motor.setTargetSpeed(speed_left);
-  right_motor.setTargetSpeed(speed_right);
-
   // int a = GamePad.getAngle();
   // int b = GamePad.getRadius();
   // Serial.printf("Winkel: %d, Radius: %d \n", a, b);
 }
-
-int8_t modSpeed(int8_t dir, int8_t speed) {
-  int8_t new_speed = 0;
-  if (dir == 1) {
-    if (speed > 0) {
-      new_speed = speed + 5;
-    } else if (speed == 0) {
-      new_speed = 5;
-    } else {
-      new_speed = speed + 5;
-    }
-  } else if (dir == 0) {
-    if (speed > 0) {
-      new_speed = speed - 5;
-    } else if (speed == 0) {
-      new_speed = 0;
-    } else {
-      new_speed = speed + 5;
-    }
-  } else if (dir == -1) {
-    if (speed > 0) {
-      new_speed = speed - 5;
-    } else if (speed == 0) {
-      new_speed = -5;
-    } else {
-      new_speed = speed - 5;
-    }
-  }
-
-  if (new_speed < -100) {
-    new_speed = -100;
-  }
-
-  if (new_speed > 100) {
-    new_speed = 100;
-  }
-
-  return new_speed;
-}
-

src/motorControl.cpp

@@ -78,7 +78,7 @@ void MotorControl::runMotorControl() {
 
 void MotorControl::setTargetSpeed(int16_t speed) {
     //TODO: Exceptionhandling
-    if (speed <= 100 || speed <= -100) {
+    if (speed <= 100 && speed >= -100) {
         this->target_speed = speed;
     } else {
         Serial.println("Invalid Argument in MotorControl::setTargetSpeed");
@@ -101,10 +101,25 @@ int16_t MotorControl::getSpeed() {
     return this->speed;
 }
 
+int16_t MotorControl::getTargetSpeed() {
+    return this->target_speed;
+}
+
 bool MotorControl::isTargetSpeedReached() {
-    if (this->target_speed == this->speed) {
+    if (this->target_speed == this->speed)
+        return true;
+    return false;
+}
+
+bool MotorControl::isAccelerationPositive() {
+    if (speed < target_speed)
+        return true;
+    return false;
+}
+
+bool MotorControl::isAccelerationNegative() {
+    if (speed > target_speed)
         return true;
-    }
     return false;
 }
 

src/motorControl.h

@@ -15,7 +15,10 @@ class MotorControl {
         void toString();
 
         int16_t getSpeed();
+        int16_t getTargetSpeed();
         bool isTargetSpeedReached();
+        bool isAccelerationPositive();
+        bool isAccelerationNegative();
 
     private:
         void setSpeed(int16_t speed);

src/moveControl.cpp

@@ -15,13 +15,72 @@ void MoveControl::init(MotorControl *left_motor, MotorControl *right_motor,
 }
 
 void MoveControl::runMoveControl() {
+    static uint64_t last_millis = 0;
+    if (millis() - last_millis < RUN_MOVE_CONTROL_DELAY) {
+        return;
+    }
+    last_millis = millis();
+
+    this->calcWheelSpeed();
+    //this->regulateMotorPower(this->left_motor, this->left_speedometer, this->wheelspeed_left_target);
+    this->regulateMotorPower(this->right_motor, this->right_speedometer, this->wheelspeed_right_target);
 
 }
 
-void MoveControl::setSpeed(int8_t speed) {
+void MoveControl::setSpeed(double speed) {
-    this->speed = speed;
+    this->x_speed = speed;
 }
 
 void MoveControl::setRotationspeed(double speed) {
     this->rotation_speed = speed;
 }
+
+void MoveControl::calcWheelSpeed() {
+    // original formula:
+    // (1 / r) / 1  b \ / x \ = / Xl \
+    //         \ 1 -b / \ T /   \ Xr /
+
+    // (1 / r) * 1
+    const static double A = 15.82278481;
+    // (1 / r) * b
+    const static double B = 2.096518987;
+
+    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);
+}
+
+void MoveControl::regulateMotorPower(MotorControl *motor, Speedometer *cur_speed, double tar_speed) {
+    if (tar_speed > 0) {
+        // Direction FORWARD
+        if (cur_speed->getSpeed() - tar_speed < 0) {
+            // Too fast
+            if (motor->isAccelerationPositive() || motor->isTargetSpeedReached()) {
+                motor->setTargetSpeed(motor->getTargetSpeed() - SPEED_STEPS);
+            }
+        } else {
+            // Too slow
+            if (motor->isAccelerationNegative() || motor->isTargetSpeedReached()) {
+                motor->setTargetSpeed(motor->getTargetSpeed() + SPEED_STEPS);
+            }
+        }
+
+    } else if (tar_speed < 0) {
+        // Direction BACKWARD
+        if (cur_speed->getSpeed() - tar_speed > 0) {
+            // Too fast
+            if (motor->isAccelerationPositive() || motor->isTargetSpeedReached()) {
+                motor->setTargetSpeed(motor->getTargetSpeed() - SPEED_STEPS);
+            }
+        } else {
+            // Too slow
+            if (motor->isAccelerationNegative() || motor->isTargetSpeedReached()) {
+                motor->setTargetSpeed(motor->getTargetSpeed() + SPEED_STEPS);
+            }
+        }
+
+
+    } else {
+        // Direction STOP
+        motor->setTargetSpeed(0);
+    }
+}

src/moveControl.h

@@ -5,6 +5,7 @@
 
 #include "motorControl.h"
 #include "speedometer.h"
+#include "config.h"
 
 class MoveControl {
     public:
@@ -12,17 +13,23 @@ class MoveControl {
         void init(MotorControl *left_motor, MotorControl *right_motor,
                     Speedometer *left_encoder, Speedometer *right_encoder);
         void runMoveControl();
-        void setSpeed(int8_t speed);
+        void setSpeed(double speed);
         void setRotationspeed(double speed);
 
     private:
+        void calcWheelSpeed();
+        static void regulateMotorPower(MotorControl *motor, Speedometer *cur_speed, double tar_speed);
+
         MotorControl *left_motor;
         MotorControl *right_motor;
         Speedometer *left_speedometer;
         Speedometer *right_speedometer;
 
-        int8_t speed = 0;
+        double x_speed = 0;
         double rotation_speed = 0;
+
+        double wheelspeed_left_target = 0;
+        double wheelspeed_right_target = 0;
 };
 
 #endif // MOVE_CONTROL_H

src/speedometer.cpp

@@ -1,7 +1,9 @@
 #include "speedometer.h"
 
 Speedometer::Speedometer() {
-
+    for (int i = 0; i < BUF_SIZE; i++) {
+        this->buf[i] = 0;
+    }
 }
 
 void Speedometer::init(uint8_t pinA, uint8_t pinB) {
@@ -21,7 +23,8 @@ void Speedometer::runSpeedometer() {
     uint16_t elapsed_time = time - this->last_millis;
     this->last_millis = time;
 
-    int64_t count = encoder.getCount();
+    int16_t count = encoder.getCount();
+    this->addValToBuf(count);
     encoder.clearCount();
 
     uint8_t direction = STOP;
@@ -31,7 +34,7 @@ void Speedometer::runSpeedometer() {
         direction = BACKWARD;
     }
 
-    count = abs(count);                                     // Absolute time in milliseconds
+    count = abs(this->getAverage());                                     // Absolute time in milliseconds
     double n = (double)count / ENC_STEPS;                           // Wheel revolutions in absolute time
     double u = (double)n / ((double)elapsed_time / 1000);   // Wheel revolutions per second
     double ms = u * (WHEEL_DIAMETER * PI);                // Speed in m/s
@@ -44,7 +47,9 @@ void Speedometer::runSpeedometer() {
         this->speed = 0;
     }
 
-    //Serial.printf("time: %d, count: %d, n: %f, u: %f, ms: %f \n", time, count, n, u, ms);
+    Serial.printf("time: %d, ", time);
+    Serial.printf("count: %d, ",count);
+    Serial.printf("n: %f, u: %f, ms: %f \n", n, u, ms);
 }
 
 double Speedometer::getSpeed() {
@@ -60,3 +65,21 @@ uint8_t Speedometer::getDirection() {
         return STOP;
     }
 }
+
+void Speedometer::addValToBuf(int16_t val) {
+    this->buf[this->bufPos] = val;
+    this->bufPos++;
+
+    if (this->bufPos == BUF_SIZE) {
+        this->bufPos = 0;
+    }
+}
+
+int16_t Speedometer::getAverage() {
+    int16_t sum = 0;
+    for (int i = 0; i < BUF_SIZE; i++) {
+        sum += this->buf[i];
+    }
+
+    return sum / BUF_SIZE;
+}

src/speedometer.h

@@ -17,10 +17,16 @@ class Speedometer {
 
 
     private:
+        void addValToBuf(int16_t val);
+        int16_t getAverage();
+
         ESP32Encoder encoder;
 
         double speed = 0;
         uint32_t last_millis = 0;
+
+        uint8_t bufPos = 0;
+        int16_t buf[BUF_SIZE];
 };
 
 #endif // SPEEDOMETER_H