fix speedometer for new encoder

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 ms = u * (diameter * PI);                        // Speed in m/s
+    
+    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) {
-        this->speed = ms;
-    } else if (count < 0) {
-        this->speed = ms * (-1);
-    } else {
-        this->speed = 0;
+    switch (this->currentDirection) {
+        case Direction::Forward :
+            this->speed = ms;
+            break;
+        
+        case Direction::Backward :
+            this->speed = -ms;
+            break;
+
+        case Direction::None :
+            this->speed = 0;
+            break;
     }
 
-    // std::cout << "Speedometer::runSpeedometer speed: " << (int) this->speed << std::endl;
+    // 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,12 +132,17 @@ 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++;
 
     if (bufPos == bufSize)
-        bufPos = 0;
+       bufPos = 0;
 }
 
 void Speedometer::updateAvgBufSize() {

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