add debouncer

include/main.h

@@ -10,6 +10,7 @@
 #include <MPU9250.h>
 #include <MPU9250.h>
 #include <PCA9624.h>
 #include <PCA9624.h>
 #include <RzmEepromMap.h>
 #include <RzmEepromMap.h>
+#include <Bounce2.h>
 
 
 using namespace Whill;
 using namespace Whill;
 
 
@@ -22,6 +23,7 @@ PCA9624 irled(0x60);
 RzmEepromMap eepmap;
 RzmEepromMap eepmap;
 StopWatch stopwatch;
 StopWatch stopwatch;
 FrameRateCounter fps(30);
 FrameRateCounter fps(30);
+Bounce emergency_debouncer;
 
 
 void printSpeedProfile(WhillOmni::LR lr)
 void printSpeedProfile(WhillOmni::LR lr)
 {
 {

src/main.cpp

@@ -4,8 +4,6 @@
 #include "global.h"
 #include "global.h"
 #include "main.h"
 #include "main.h"
 
 
-void disableMotor() { b_motor_active = false; }
-
 void setup()
 void setup()
 {
 {
     Serial.begin(115200);
     Serial.begin(115200);
@@ -27,6 +25,8 @@ void setup()
     // ----- Emergency Switch -----
     // ----- Emergency Switch -----
 
 
     pinMode(PIN_EMERGENCY, INPUT);
     pinMode(PIN_EMERGENCY, INPUT);
+    emergency_debouncer.attach(PIN_EMERGENCY, INPUT);
+    emergency_debouncer.interval(200);
 
 
     // ----- check I2C -----
     // ----- check I2C -----
     Wire.begin();
     Wire.begin();
@@ -67,7 +67,8 @@ void setup()
     whill.streaming(true, WHILL_INFO_INTERVAL_MS, DATASET::SENSOR_INFO);
     whill.streaming(true, WHILL_INFO_INTERVAL_MS, DATASET::SENSOR_INFO);
     delay(1000);
     delay(1000);
 
 
-    attachInterrupt(digitalPinToInterrupt(PIN_EMERGENCY), disableMotor, RISING);
+    if (digitalRead(PIN_EMERGENCY) == LOW) b_motor_active = true;
+    else                                   b_motor_active = false;
 
 
     Serial.println("start program");
     Serial.println("start program");
     fps.start();
     fps.start();
@@ -76,6 +77,16 @@ void setup()
 
 
 void loop()
 void loop()
 {
 {
+    emergency_debouncer.update();
+    if (emergency_debouncer.rose())
+    {
+        b_motor_active = false;
+    }
+    if (emergency_debouncer.fell())
+    {
+        b_motor_active = true;
+    }
+
     setVelocity();
     setVelocity();
 
 
     whill.parse();
     whill.parse();