fix speedometer for new encoder

include/moveControl.h

@@ -80,6 +80,12 @@ class MoveControl {
          */
         void setDrivingStatus(DrivingStatus status);
 
+        /**
+         * @brief Stops the engine immediately
+         * 
+         */
+        void emergencyStop();
+
         /**
          * @brief Set the target speed
          * 
@@ -156,6 +162,8 @@ class MoveControl {
         void setDelay(uint8_t delay) { this->delay = delay; }
 
     private:
+        void setSpeedometerDirection(Speedometer *speedometer, double value);
+
         /**
          * @brief Converts values into wheel speeds
          * 

lib/Speedometer/speedometer.cpp

@@ -30,59 +30,59 @@ uint16_t Speedometer::loop() {
         return -1;
 
     uint32_t time = millis();
-    uint16_t elapsed_time = time - this->last_millis_loop;
+    uint16_t elapsedTime = time - this->lastMillisLoop;
 
     //Cancel if delayLoop is not reached
-    if (elapsed_time < this->delayLoop)
-        return elapsed_time;
+    if (elapsedTime < this->delayLoop)
+        return elapsedTime;
     
     runSpeedometer();
-    this->last_millis_loop = time;
-    return elapsed_time;
+    this->lastMillisLoop = time;
+    return elapsedTime;
 }
 
 void Speedometer::runSpeedometer() {
     uint32_t time = millis();
 
-    uint16_t elapsed_time = time - last_millis_calc;
-    last_millis_calc = time;
-
-    int16_t count = this->pulseCounter->getValue();
-    switch (this->currentDirection) {
-        case Direction::Forward :
-            this->addValToBuf(count);
-            break;
-
-        case Direction::Backward :
-            this->addValToBuf(-count);
-            break;
-
-        case Direction::None :
-            this->addValToBuf(0);
-            break;
-        
-        default:
-            std::cout << "Wrong value in Speedometer::runSpeedometer" << std::endl;
-            break;
-    }
+    uint16_t elapsedTime = time - this->lastMillisCalc;
+    this->lastMillisCalc = time;
 
+    this->addValToBuf(this->pulseCounter->getValue());
     this->pulseCounter->clear();
     this->pulseCounter->resume();
 
-    uint16_t count_abs = abs(this->calcAverage());          // Absolute time in milliseconds
-    double n = (double)count_abs / steps;                   // Wheel revolutions in absolute time
-    double u = (double)n / ((double)elapsed_time / 1000);   // Wheel revolutions per second
+    
+    double n = (double)this->calcAverage() / this->steps;   // Wheel revolutions in absolute time
+    double u = n / ((double)elapsedTime / 1000);           // Wheel revolutions per second
     double ms = u * (diameter * PI);                      // Speed in m/s
 
-    if (count > 0) {
+    switch (this->currentDirection) {
+        case Direction::Forward :
             this->speed = ms;
-    } else if (count < 0) {
-        this->speed = ms * (-1);
-    } else {
+            break;
+        
+        case Direction::Backward :
+            this->speed = -ms;
+            break;
+
+        case Direction::None :
             this->speed = 0;
+            break;
     }
 
+    // if (this->speed)
+    //     this->printCounter++;
+    // if (this->printCounter > 10) {
+    //     this->printCounter = 0;
     //     std::cout << "Speedometer::runSpeedometer speed: " << (int) this->speed << std::endl;
+    // }
+}
+
+void Speedometer::setDirection(Direction dir) {
+    if (this->currentDirection == dir)
+        return;
+    this->currentDirection = dir;
+    this->clearAvgBuf();
 }
 
 void Speedometer::setNumOfValForAvg(uint8_t val) {
@@ -132,6 +132,11 @@ void Speedometer::initAvgBuf() {
         this->buf[i] = 0;
 }
 
+void Speedometer::clearAvgBuf() {
+    for (uint8_t i = 0; i < bufSize; i++) 
+        this->buf[i] = 0;
+}
+
 void Speedometer::addValToBuf(int16_t val) {
     this->buf[this->bufPos] = val;
     this->bufPos++;

lib/Speedometer/speedometer.h

@@ -89,7 +89,7 @@ class Speedometer {
          * 
          * @param dir Direction
          */
-        void setDirection(Direction dir) { this->currentDirection = dir; }
+        void setDirection(Direction dir);
 
         /**
          * @brief Set the number of last values ​​to be taken into account for the average.
@@ -150,6 +150,7 @@ class Speedometer {
     private:
         void init(uint8_t pin, double diameter, uint16_t steps);
         void initAvgBuf();
+        void clearAvgBuf();
         void addValToBuf(int16_t val);
         void updateAvgBufSize();
         int16_t calcAverage();
@@ -162,6 +163,7 @@ class Speedometer {
         double speed = 0;
         double diameter;
 
+        uint8_t printCounter = 0;
         uint8_t bufSize = BUFSIZE;
         uint8_t delayLoop = DELAY_SPEEDOMETER;
         uint8_t bufPos = 0;
@@ -169,8 +171,8 @@ class Speedometer {
 
         int16_t *buf = nullptr;
 
-        uint32_t last_millis_loop = 0;
-        uint32_t last_millis_calc = 0;
+        uint32_t lastMillisLoop = 0;
+        uint32_t lastMillisCalc = 0;
 };
 
 #endif // SPEEDOMETER_H

src/moveControl.cpp

@@ -112,17 +112,31 @@ void MoveControl::setDrivingStatus(DrivingStatus status) {
     // }
 }
 
+void MoveControl::emergencyStop() {
+    this->left_motor->emergencyStop();
+    this->right_motor->emergencyStop();
+    this->setSpeed(0);
+    this->setRotationSpeed(0);
+    this->setRawPowerLeft(0);
+    this->setRawPowerRight(0);
+    this->driving_status = DrivingStatus::stop;
+}
+
 void MoveControl::setSpeed(double speed) {
-    if (speed < 0.1 && speed > -0.1)
+    if (speed < 0.1 && speed > -0.1) {
         this->x_speed = 0;
-    else
+        return;
+    }
+
     this->x_speed = speed;
 }
 
 void MoveControl::setRotationSpeed(double speed) {
-    if (speed < 0.1 && speed > -0.1)
+    if (speed < 0.1 && speed > -0.1){
         this->rotation_speed = 0;
-    else
+        return;
+    }
+
     this->rotation_speed = speed;
 }
 
@@ -155,6 +169,15 @@ PID* MoveControl::getPID(uint8_t side) {
         return nullptr;
 }
 
+void MoveControl::setSpeedometerDirection(Speedometer *speedometer, double value) {
+    if (value > 0)
+        speedometer->setDirection(Speedometer::Direction::Forward);
+    else if (value < 0)
+        speedometer->setDirection(Speedometer::Direction::Backward);
+    else
+        speedometer->setDirection(Speedometer::Direction::None);
+}
+
 void MoveControl::calcTargetWheelSpeed() {
     /* original formula:
        (1 / r) / 1  b \ / x \ = / Xl \
@@ -174,16 +197,22 @@ void MoveControl::regulateMotors() {
         case DrivingStatus::stop :
             this->left_motor->setTargetPower(0);
             this->right_motor->setTargetPower(0);
+            this->setSpeedometerDirection(this->left_speedometer, 0);
+            this->setSpeedometerDirection(this->right_speedometer, 0);
             break;
 
         case DrivingStatus::drive :
             this->left_motor->setTargetPower( (int8_t) this->left_pid_out);
             this->right_motor->setTargetPower( (int8_t) this->right_pid_out);
+            this->setSpeedometerDirection(this->left_speedometer, this->left_pid_out);
+            this->setSpeedometerDirection(this->right_speedometer, this->right_pid_out);
             break;
 
         case DrivingStatus::raw :
             this->left_motor->setTargetPower(this->rawPowerLeft);
             this->right_motor->setTargetPower(this->rawPowerRight);
+            this->setSpeedometerDirection(this->left_speedometer, this->rawPowerLeft);
+            this->setSpeedometerDirection(this->right_speedometer, this->rawPowerRight);
             break;
 
         default: