improved moveControl

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) {
-    this->speed = speed;
+void MoveControl::setSpeed(double 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);
+    }
+}