more stable while waiting mode

include/main.h

@@ -257,7 +257,7 @@ void setVelocity()
             vel[0] = unpacker.velocityX();
             vel[0] = unpacker.velocityX();
             vel[1] = unpacker.velocityY();
             vel[1] = unpacker.velocityY();
             vel[2] = unpacker.velocityW();
             vel[2] = unpacker.velocityW();
-            stopwatch.restart();
+            stopwatch.start();
         }
         }
         unpacker.pop();
         unpacker.pop();
     }
     }
@@ -268,7 +268,7 @@ void setVelocity()
         {
         {
             if (b_motor_active)
             if (b_motor_active)
             {
             {
-                if (fps.isNextFrame()) whill.move(vel[0], vel[1], vel[2]);
+                if (fps.isNext()) whill.move(vel[0], vel[1], vel[2]);
             }
             }
             else
             else
             {
             {

lib/StopWatch/StopWatch.h

@@ -4,87 +4,116 @@
 
 
 class StopWatch
 class StopWatch
 {
 {
-public:
+protected:
 
 
-    void start() { play(); us_start_ = micros(); }
-    void stop() { pause(); us_start_ = 0; }
-    void restart() { stop(); start(); }
+    const int64_t UNAVAILABLE              = 0xFFFFFFFFFFFFFFFF;
+    const int64_t UINT32_NUMERIC_LIMIT_MAX = 0x00000000FFFFFFFF;
 
 
-    void play() { is_running_ = true; us_elapsed_ = us(); }
-    void pause() { is_running_ = false; us_start_ = micros() - us_elapsed_; }
+public:
 
 
-    bool isRunning() { return is_running_; }
+    virtual ~StopWatch() {}
 
 
-    inline void setCurrTime(double s)
+    inline void start()
     {
     {
-        us_offset_ += s * 1000000. - us();
+        running = true;
+        prev_us = micros();
+        origin = now = (int64_t)prev_us;
+        overflow = 0;
     }
     }
-    void setOffsetUs(double us_offset) { us_offset_ = us_offset; }
-
-    // TODO: how to handle over int32_t range
-    inline double us() const
+    inline void stop()
     {
     {
-        if (us_start_ == 0) return 0;
-        return (double)micros() - us_start_ + us_offset_;
+        running = false;
+        prev_us = 0;
+        origin = now = UNAVAILABLE;
+        overflow = 0;
     }
     }
 
 
-    inline double ms() const { return us() * 0.001; }
-    inline double sec() const { return us() * 0.000001; }
+    inline bool isRunning() const { return running; }
+    inline bool isPausing() const { return (!running && (origin != UNAVAILABLE)); }
+
+    inline int64_t us()
+    {
+        if      ( isPausing()) ;
+        else if (!isRunning()) return 0;
+        else
+        {
+            uint32_t curr_us = micros();
+            if (curr_us < prev_us) overflow += UINT32_NUMERIC_LIMIT_MAX + (int64_t)1;
+            prev_us = curr_us;
+            now = (int64_t)curr_us + overflow;
+        }
+        return now - origin;
+    }
+    inline double ms() { return (double)us() * 0.001; }
+    inline double sec() { return (double)us() * 0.000001; }
 
 
 private:
 private:
 
 
-    double us_start_ {0.0};
-    double us_elapsed_ {0.0};
-    double us_offset_ {0.0};
-    bool is_running_ {false};
+    bool running {false};
+    uint32_t prev_us {0};
+    int64_t origin {UNAVAILABLE};
+    int64_t now {UNAVAILABLE};
+    int64_t overflow {0};
 };
 };
 
 
-class FrameRateCounter : public StopWatch
+class IntervalCounter : public StopWatch
 {
 {
 public:
 public:
 
 
-    FrameRateCounter(double fps = 40.0)
-    : fps_(fps)
-    , interval_(getTargetIntervalMicros())
-    , curr_frame_(0)
-    , prev_frame_(0)
-    , b_zero_start_(false)
-    {
-    }
+    IntervalCounter (double sec)
+    : available(false)
+    , interval((uint32_t)(sec * 1000000.))
+    , next(interval)
+    , cnt(0)
+    {}
 
 
-    double frame()
-    {
-        return us() / interval_;
-    }
+    virtual ~IntervalCounter() {}
 
 
-    bool isNextFrame()
+    inline void start() { this->StopWatch::start(); next = interval; cnt = 0; }
+    inline void stop() { this->StopWatch::stop(); next = UNAVAILABLE; cnt = 0; }
+    inline void restart() { stop(); start(); }
+
+    inline bool update()
+    {
+        if (us() > next)
         {
         {
-        bool b;
-        double curr_frame = frame();
-        if (b_zero_start_) b = (floor(curr_frame) != floor(prev_frame_));
-        else               b = (ceil(curr_frame) != ceil(prev_frame_));
-        prev_frame_ = curr_frame;
-        return b;
+            available = true;
+            next = interval * (cnt++ + 1);
         }
         }
+        else
+            available = false;
 
 
-    void setFrameRate(double fps)
-    {
-        fps_ = fps;
-        interval_ = getTargetIntervalMicros();
+        return available;
     }
     }
 
 
-    void setZeroStart(bool b) { b_zero_start_ = b; }
+    inline bool isNext() { return available; }
+
+    inline double count() { return cnt; }
+
+    inline void setInterval(double i) { interval = (int64_t)(i * 1000000.); }
 
 
 private:
 private:
 
 
-    double getTargetIntervalMicros() const { return (1000000.0 / fps_); }
+    bool available {false};
+    int64_t interval {0};
+    int64_t next {0};
+    int64_t cnt {0};
+};
+
+class FrameRateCounter : public IntervalCounter
+{
+public:
+
+    FrameRateCounter(double fps) // second
+    : IntervalCounter(1.0 / fps)
+    { }
+    virtual ~FrameRateCounter() {}
+
+    inline double frame() { return count(); }
 
 
-    double fps_ {40.0};
-    double interval_ {0.0};
-    double curr_frame_ {0};
-    double prev_frame_ {0};
-    bool b_zero_start_ {true};
+    inline void setFrameRate(double fps) { setInterval(1. / fps); }
 };
 };
 
 
 
 
+
 #endif // STOPWATCH_H
 #endif // STOPWATCH_H

lib/StopWatch/StopWatch_190205.h

+#pragma once
+#ifndef STOPWATCH_H
+#define STOPWATCH_H
+
+class StopWatch
+{
+protected:
+
+    const uint64_t UNAVAILABLE = 0xFFFFFFFFFFFFFFFF;
+    const uint64_t UINT32_NUMERIC_LIMIT_MAX = 0x00000000FFFFFFFF;
+
+public:
+
+    virtual ~StopWatch() {}
+
+    inline void start()
+    {
+        running = true;
+        prev_us = micros();
+        origin = now = prev_us;
+        overflow = 0;
+    }
+    inline void stop()
+    {
+        running = false;
+        prev_us = 0;
+        origin = now = UNAVAILABLE;
+        overflow = 0;
+    }
+
+    inline bool isRunning() const { return running; }
+    inline bool isPausing() const { return (!running && (origin != UNAVAILABLE)); }
+
+    inline uint64_t us()
+    {
+        Serial.println("inside us()");
+        Serial.println((uint32_t)origin, HEX);
+        Serial.println(micros(), HEX);
+
+        if      ( isPausing()) ;
+        else if (!isRunning()) return 0;
+        else
+        {
+            uint32_t curr_us = micros();
+            // Serial.print("uint32_t micros() = ");
+            // Serial.print(prev_us);
+            // Serial.print(", ");
+            // Serial.println(curr_us);
+            if (curr_us < prev_us)
+            {
+                overflow += UINT32_NUMERIC_LIMIT_MAX + (uint64_t)1;
+                Serial.println("micros() overflow!!!!!!!!!");
+                Serial.print("uint32_t micros() = ");
+                Serial.print(prev_us);
+                Serial.print(", ");
+                Serial.println(curr_us);
+                // delay(100000);
+            }
+            prev_us = curr_us;
+            now = (uint64_t)curr_us + overflow;
+        }
+        // Serial.print("int64_t us_curr, start_ = ");
+        // Serial.print((uint32_t)now);
+        // Serial.print(" - ");
+        // Serial.println((uint32_t)origin);
+        return now - origin;
+    }
+    inline double ms() { return (double)us() * 0.001; }
+    inline double sec() { return (double)us() * 0.000001; }
+
+private:
+
+    bool running {false};
+    uint32_t prev_us {0};
+    uint64_t origin {UNAVAILABLE};
+    uint64_t now {UNAVAILABLE};
+    uint64_t overflow {0};
+};
+
+class IntervalCounter : public StopWatch
+{
+public:
+
+    IntervalCounter (double sec)
+    : available(false)
+    , interval((uint32_t)(sec * 1000000.))
+    , next(interval)
+    , cnt(0)
+    {}
+
+    virtual ~IntervalCounter() {}
+
+    inline void start() { this->StopWatch::start(); next = interval; cnt = 0; }
+    inline void stop() { this->StopWatch::stop(); next = UNAVAILABLE; cnt = 0; }
+    inline void restart() { stop(); start(); }
+
+    inline bool update()
+    {
+        uint64_t uss = us();
+
+        Serial.print("curr, next = ");
+        Serial.print((uint32_t)(uss & 0xFFFFFFFF00000000 >> 32));
+        Serial.print(" ");
+        Serial.print((uint32_t)(uss & 0x00000000FFFFFFFF));
+        Serial.print(", ");
+        Serial.print((uint32_t)(next & 0xFFFFFFFF00000000 >> 32));
+        Serial.print(" ");
+        Serial.print((uint32_t)(next & 0x00000000FFFFFFFF));
+        Serial.print(" : ");
+        Serial.print((uint32_t)(interval & 0xFFFFFFFF00000000 >> 32));
+        Serial.print(" ");
+        Serial.print((uint32_t)(interval& 0x00000000FFFFFFFF));
+        Serial.print(", ");
+        Serial.print((uint32_t)(cnt & 0xFFFFFFFF00000000 >> 32));
+        Serial.print(" ");
+        Serial.println((uint32_t)(cnt & 0x00000000FFFFFFFF));
+
+        if (uss > next)
+        {
+            Serial.println("Next");
+            available = true;
+            next = interval * (cnt++ + 1);
+        }
+        else
+        {
+            available = false;
+        }
+        return available;
+    }
+
+    inline bool isNext() { return available; }
+
+    inline double count() { return cnt; }
+
+    inline void setInterval(double i) { interval = (uint64_t)(i * 1000000.); }
+
+private:
+
+    bool available {false};
+    uint64_t interval {0};
+    uint64_t next {0};
+    uint64_t cnt {0};
+};
+
+class FrameRateCounter : public IntervalCounter
+{
+public:
+
+    FrameRateCounter(double fps) // second
+    : IntervalCounter(1.0 / fps)
+    { }
+    virtual ~FrameRateCounter() {}
+
+    inline double frame() { return count(); }
+
+    inline void setFrameRate(double fps) { setInterval(1. / fps); }
+};
+
+
+
+#endif // STOPWATCH_H

lib/StopWatch/StopWatch_SEMICON.h

+#pragma once
+#ifndef STOPWATCH_H
+#define STOPWATCH_H
+
+class StopWatch
+{
+public:
+
+    void start() { play(); us_start_ = micros(); }
+    void stop() { pause(); us_start_ = 0; }
+    void restart() { stop(); start(); }
+
+    void play() { is_running_ = true; us_elapsed_ = us(); }
+    void pause() { is_running_ = false; us_start_ = micros() - us_elapsed_; }
+
+    bool isRunning() { return is_running_; }
+
+    inline void setCurrTime(double s)
+    {
+        us_offset_ += s * 1000000. - us();
+    }
+    void setOffsetUs(double us_offset) { us_offset_ = us_offset; }
+
+    // TODO: how to handle over int32_t range
+    inline double us() const
+    {
+        if (us_start_ == 0) return 0;
+        return (double)micros() - us_start_ + us_offset_;
+    }
+
+    inline double ms() const { return us() * 0.001; }
+    inline double sec() const { return us() * 0.000001; }
+
+private:
+
+    double us_start_ {0.0};
+    double us_elapsed_ {0.0};
+    double us_offset_ {0.0};
+    bool is_running_ {false};
+};
+
+class FrameRateCounter : public StopWatch
+{
+public:
+
+    FrameRateCounter(double fps = 40.0)
+    : fps_(fps)
+    , interval_(getTargetIntervalMicros())
+    , curr_frame_(0)
+    , prev_frame_(0)
+    , b_zero_start_(false)
+    {
+    }
+
+    double frame()
+    {
+        return us() / interval_;
+    }
+
+    bool isNextFrame()
+    {
+        bool b;
+        double curr_frame = frame();
+        if (b_zero_start_) b = (floor(curr_frame) != floor(prev_frame_));
+        else               b = (ceil(curr_frame) != ceil(prev_frame_));
+        prev_frame_ = curr_frame;
+        return b;
+    }
+
+    void setFrameRate(double fps)
+    {
+        fps_ = fps;
+        interval_ = getTargetIntervalMicros();
+    }
+
+    void setZeroStart(bool b) { b_zero_start_ = b; }
+
+private:
+
+    double getTargetIntervalMicros() const { return (1000000.0 / fps_); }
+
+    double fps_ {40.0};
+    double interval_ {0.0};
+    double curr_frame_ {0};
+    double prev_frame_ {0};
+    bool b_zero_start_ {true};
+};
+
+
+#endif // STOPWATCH_H