- todos

- test driving

lib/Speedometer/speedometer.cpp

@@ -16,14 +16,14 @@ Speedometer::Speedometer(uint8_t pin, double diameter, uint16_t steps) {
     this->steps = steps;
 
     this->pulseCounter = new Counter(pin);
-    this->pulseCounter->setFilterValue(1000); // ignore pulses less than 1000 x 2.5ns
+    this->pulseCounter->setFilterValue(1023); // ignore pulses less than 1000 x 2.5ns
 
     this->pulseCounter->clear();
     this->pulseCounter->resume();
 
     Component::loopDelay = Speedometer::loopDelay;
 
-    clearAvgBuf();
+    this->clearAvgBuf();
 }
 
 Speedometer::~Speedometer() {
@@ -47,18 +47,27 @@ void Speedometer::run() {
     double n = (double)pulse / 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
+    double rad = u * 2 * PI;
+
+    if (speed < 0.1) {
+        speed = 0;
+        rad = 0;
+    }
 
     switch (this->currentDirection) {
         case Direction::Forward :
             this->speed = ms;
+            this->rad = rad;
             break;
         
         case Direction::Backward :
             this->speed = -ms;
+            this->rad = -rad;
             break;
 
         case Direction::None :
             this->speed = 0;
+            this->rad = 0;
             break;
     }