fix bug

include/main.h

@@ -262,6 +262,7 @@ void setVelocity()
         unpacker.pop();
         unpacker.pop();
     }
     }
 
 
+    fps.update();
     if (stopwatch.isRunning())
     if (stopwatch.isRunning())
     {
     {
         if (stopwatch.ms() < XBEE_COMMAND_TIMEOUT_MS)
         if (stopwatch.ms() < XBEE_COMMAND_TIMEOUT_MS)

lib/StopWatch/LICENSE

+MIT License
+
+Copyright (c) 2019 Hideaki Tai
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.

lib/StopWatch/README.md

+# StopWatch
+Arduino library to monitor time and frame rate from any time if you want.
+This library has three classes, `StopWatch` , `FrameRateCounter`, and `IntervalCounter`.
+
+## Usage
+
+### StopWatch
+
+``` C++
+#include <StopWatch.h>
+StopWatch stopwatch;
+
+void setup()
+{
+    Serial.begin(115200);
+    stopwatch.start();
+}
+
+void loop()
+{
+    if (stopwatch.isRunning())
+    {
+        Serial.print("millis() : ");
+        Serial.print(millis());
+        Serial.print(", stopwatch : ");
+        Serial.println(stopwatch.ms());
+    }
+    if (stopwatch.ms() > 5000) stopwatch.restart();
+}
+```
+
+### FrameRateCounter
+
+``` C++
+#include <StopWatch.h>
+FrameRateCounter fps(30);
+
+void setup()
+{
+    Serial.begin(115200);
+    fps.start();
+}
+
+void loop()
+{
+    fps.update();
+
+    if (fps.isRunning())
+    {
+        if (fps.isNext())
+        {
+            Serial.print("frame no. = ");
+            Serial.print(fps.frame());
+            Serial.print(", time = ");
+            Serial.println(fps.ms());
+        }
+    }
+    if (fps.frame() > 150) fps.restart();
+}
+```
+
+### IntervalCounter
+
+``` C++
+#include <StopWatch.h>
+IntervalCounter interval(1.0); // interval is 1.0[sec]
+
+void setup()
+{
+    Serial.begin(115200);
+    interval.start();
+}
+
+void loop()
+{
+    interval.update();
+
+    if (interval.isRunning())
+    {
+        if (interval.isNext())
+        {
+            Serial.print("interval count = ");
+            Serial.print(interval.count());
+            Serial.print(", time = ");
+            Serial.println(interval.ms());
+        }
+    }
+    if (interval.count() > 10) interval.restart();
+}
+```
+
+## APIs
+
+### Common
+
+``` C++
+    inline void start()
+    inline void stop()
+    inline void restart()
+
+    inline void play()
+    inline void pause()
+
+    inline bool isRunning()
+    inline bool isPausing()
+
+    inline double us() const
+    inline double ms() const
+    inline double sec() const
+
+    inline void offsetUs(double us)
+    inline void offsetMs(double ms)
+    inline void offsetSec(double sec)
+```
+
+### For FrameRateCounter & IntervalCounter
+
+``` C++
+    FrameRateCounter(double fps)
+    inline bool update()
+    inline bool isNext()
+    inline double frame() const
+    inline void setFrameRate(double fps)
+    inline void setFirstFrameToOne(bool b) // default : false (zero frame is first frame no.)
+    inline void offset(int64_t offset)
+```
+
+### For FrameRateCounter & IntervalCounter
+
+``` C++
+    IntervalCounter(double interval) // second
+    inline bool update()
+    inline bool isNext()
+    inline double count() const
+    inline void setInterval(double interval)
+    inline void offset(int64_t offset)
+```
+
+
+## License
+
+MIT

lib/StopWatch/StopWatch.h

@@ -18,20 +18,52 @@ public:
         running = true;
         running = true;
         prev_us = micros();
         prev_us = micros();
         origin = now = (int64_t)prev_us;
         origin = now = (int64_t)prev_us;
-        overflow = 0;
+        overflow = offset = 0;
     }
     }
     inline void stop()
     inline void stop()
     {
     {
         running = false;
         running = false;
         prev_us = 0;
         prev_us = 0;
         origin = now = UNAVAILABLE;
         origin = now = UNAVAILABLE;
-        overflow = 0;
+        overflow = offset = 0;
     }
     }
+    inline void play()
+    {
+        if (isPausing())
+        {
+            running = true;
+            uint32_t curr_us = micros();
+            if (curr_us > prev_us)
+                origin += (int64_t)(curr_us - prev_us);
+            else
+                origin += UINT32_NUMERIC_LIMIT_MAX - (int64_t)(prev_us - curr_us);
+            prev_us = curr_us;
+        }
+        else if (isRunning()) ;
+        else                  start();
+    }
+    inline void pause()
+    {
+        if      (isRunning()) { microsec(); running = false; }
+        else if (isPausing()) ;
+        else                  stop();
+    }
+    inline void restart() { stop(); start(); }
 
 
     inline bool isRunning() const { return running; }
     inline bool isRunning() const { return running; }
     inline bool isPausing() const { return (!running && (origin != UNAVAILABLE)); }
     inline bool isPausing() const { return (!running && (origin != UNAVAILABLE)); }
 
 
-    inline int64_t us()
+    inline double us() { return (double)microsec(); }
+    inline double ms() { return us() * 0.001; }
+    inline double sec() { return us() * 0.000001; }
+
+    inline void offsetUs(double us) { offset = (int64_t)us; }
+    inline void offsetMs(double ms) { offsetUs(1000. * ms); }
+    inline void offsetSec(double sec) { offsetUs(1000000. * sec); }
+
+protected:
+
+    inline int64_t microsec()
     {
     {
         if      ( isPausing()) ;
         if      ( isPausing()) ;
         else if (!isRunning()) return 0;
         else if (!isRunning()) return 0;
@@ -42,10 +74,8 @@ public:
             prev_us = curr_us;
             prev_us = curr_us;
             now = (int64_t)curr_us + overflow;
             now = (int64_t)curr_us + overflow;
         }
         }
-        return now - origin;
+        return (double)(now - origin + offset);
     }
     }
-    inline double ms() { return (double)us() * 0.001; }
-    inline double sec() { return (double)us() * 0.000001; }
 
 
 private:
 private:
 
 
@@ -54,13 +84,14 @@ private:
     int64_t origin {UNAVAILABLE};
     int64_t origin {UNAVAILABLE};
     int64_t now {UNAVAILABLE};
     int64_t now {UNAVAILABLE};
     int64_t overflow {0};
     int64_t overflow {0};
+    int64_t offset {0};
 };
 };
 
 
 class IntervalCounter : public StopWatch
 class IntervalCounter : public StopWatch
 {
 {
 public:
 public:
 
 
-    IntervalCounter (double sec)
+    explicit IntervalCounter (double sec)
     : available(false)
     : available(false)
     , interval((uint32_t)(sec * 1000000.))
     , interval((uint32_t)(sec * 1000000.))
     , next(interval)
     , next(interval)
@@ -69,13 +100,13 @@ public:
 
 
     virtual ~IntervalCounter() {}
     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 void start() { StopWatch::start(); next = interval; cnt = 0; }
+    inline void stop() { StopWatch::stop(); next = UNAVAILABLE; cnt = 0; }
+    inline void restart() { IntervalCounter::stop(); IntervalCounter::start(); }
 
 
     inline bool update()
     inline bool update()
     {
     {
-        if (us() > next)
+        if (microsec() > next)
         {
         {
             available = true;
             available = true;
             next = interval * (cnt++ + 1);
             next = interval * (cnt++ + 1);
@@ -86,11 +117,10 @@ public:
         return available;
         return available;
     }
     }
 
 
-    inline bool isNext() { return available; }
-
-    inline double count() { return cnt; }
-
+    inline bool isNext() const { return available; }
+    inline double count() const { return cnt; }
     inline void setInterval(double i) { interval = (int64_t)(i * 1000000.); }
     inline void setInterval(double i) { interval = (int64_t)(i * 1000000.); }
+    inline void offset(int64_t offset) { offsetUs(interval * offset); cnt += offset; }
 
 
 private:
 private:
 
 
@@ -104,14 +134,18 @@ class FrameRateCounter : public IntervalCounter
 {
 {
 public:
 public:
 
 
-    FrameRateCounter(double fps) // second
+    explicit FrameRateCounter(double fps) // second
     : IntervalCounter(1.0 / fps)
     : IntervalCounter(1.0 / fps)
     { }
     { }
     virtual ~FrameRateCounter() {}
     virtual ~FrameRateCounter() {}
 
 
-    inline double frame() { return count(); }
-
+    inline double frame() const { return is_one_start ? (count() + 1.0) : count(); }
     inline void setFrameRate(double fps) { setInterval(1. / fps); }
     inline void setFrameRate(double fps) { setInterval(1. / fps); }
+    inline void setFirstFrameToOne(bool b) { is_one_start = b; }
+
+private:
+
+    bool is_one_start {false};
 };
 };
 
 
 
 

lib/StopWatch/examples/FrameRateCounter/FrameRateCounter.ino

+#include <StopWatch.h>
+
+FrameRateCounter fps(30);
+
+void setup()
+{
+    Serial.begin(115200);
+
+    fps.start();
+}
+
+void loop()
+{
+    fps.update();
+    
+    if (fps.isRunning())
+    {
+        if (fps.isNext())
+        {
+            Serial.print("frame no. = ");
+            Serial.print(fps.frame());
+            Serial.print(", time = ");
+            Serial.println(fps.ms());
+        }
+    }
+
+    if (fps.frame() > 150) fps.restart();
+}

lib/StopWatch/examples/IntervalCounter/IntervalCounter.ino

+#include <StopWatch.h>
+IntervalCounter interval(1.0); // interval is 1.0[sec]
+
+void setup()
+{
+    Serial.begin(115200);
+    interval.start();
+}
+
+void loop()
+{
+    interval.update();
+    
+    if (interval.isRunning())
+    {
+        if (interval.isNext())
+        {
+            Serial.print("interval count = ");
+            Serial.print(interval.count());
+            Serial.print(", time = ");
+            Serial.println(interval.ms());
+        }
+    }
+    if (interval.count() > 10) interval.restart();
+}

lib/StopWatch/examples/StopWatch/StopWatch.ino

+#include <StopWatch.h>
+
+StopWatch stopwatch;
+
+void setup()
+{
+    Serial.begin(115200);
+
+    stopwatch.start();
+}
+
+void loop()
+{
+    if (stopwatch.isRunning())
+    {
+        Serial.print("millis() : ");
+        Serial.print(millis());
+        Serial.print(", stopwatch : ");
+        Serial.println(stopwatch.ms());
+    }
+
+    if (stopwatch.ms() > 5000) stopwatch.restart();
+}

lib/StopWatch/library.json

+{
+    "name": "StopWatch",
+    "keywords": "timing, framerate, stopwatch, fps",
+    "description": "Arduino library to monitor time and frame rate from any time if you want",
+    "repository":
+    {
+        "type": "git",
+        "url": "https://github.com/hideakitai/StopWatch.git"
+    },
+    "authors":
+    {
+        "name": "Hideaki Tai",
+        "url": "https://github.com/hideakitai",
+        "maintainer": true
+    },
+    "version": "0.1.5",
+    "license": "MIT",
+    "frameworks": "*",
+    "platforms": "*"
+}

lib/StopWatch/library.properties

+name=StopWatch
+version=0.1.5
+author=hideakitai
+maintainer=hideakitai
+sentence=Arduino library to monitor time and frame rate from any time if you want
+paragraph=Arduino library to monitor time and frame rate from any time if you want
+category=Timing
+url=https://github.com/hideakitai
+architectures=*