diff --git a/sunray/config_example.h b/sunray/config_example.h index a908cf2e1..1a4270d38 100644 --- a/sunray/config_example.h +++ b/sunray/config_example.h @@ -243,6 +243,7 @@ Also, you may choose the serial port below for serial monitor output (CONSOLE). // #define BUMPER_ENABLE true #define BUMPER_ENABLE false #define BUMPER_DEADTIME 1000 // linear motion dead-time (ms) after bumper is allowed to trigger +#define BUMPER_TRIGGER_DELAY 2000 // bumper must be active for (ms) to trigger // ----- battery charging current measurement (INA169) -------------- @@ -514,4 +515,3 @@ Also, you may choose the serial port below for serial monitor output (CONSOLE). // 2. Locate file 'packages/arduino/hardware/sam/xxxxx/cores/arduino/RingBuffer.h #define SERIAL_BUFFER_SIZE 1024 - diff --git a/sunray/robot.cpp b/sunray/robot.cpp index 0ed46f9fb..1f82d0551 100644 --- a/sunray/robot.cpp +++ b/sunray/robot.cpp @@ -59,7 +59,7 @@ MPU9250_DMP imu; AmRobotDriver robotDriver; AmMotorDriver motorDriver; AmBatteryDriver batteryDriver; - AmBumperDriver bumper; + AmBumperDriver bumper(BUMPER_TRIGGER_DELAY); AmStopButtonDriver stopButton; AmRainSensorDriver rainDriver; #endif diff --git a/sunray/src/driver/AmRobotDriver.cpp b/sunray/src/driver/AmRobotDriver.cpp index da9b99e32..588f2f7ef 100644 --- a/sunray/src/driver/AmRobotDriver.cpp +++ b/sunray/src/driver/AmRobotDriver.cpp @@ -14,8 +14,8 @@ volatile int odomTicksLeft = 0; volatile int odomTicksRight = 0; -volatile bool leftPressed = false; -volatile bool rightPressed = false; +volatile uint32_t leftTriggeredSince = 0; +volatile uint32_t rightTriggeredSince = 0; bool faultActive = LOW; @@ -281,11 +281,11 @@ void AmBatteryDriver::keepPowerOn(bool flag){ // ------------------------------------------------------------------------------------ void BumperLeftInterruptRoutine(){ - leftPressed = (digitalRead(pinBumperLeft) == LOW); + leftTriggeredSince = digitalRead(pinBumperLeft) == LOW ? millis() : 0; } void BumperRightInterruptRoutine(){ - rightPressed = (digitalRead(pinBumperRight) == LOW); + rightTriggeredSince = digitalRead(pinBumperRight) == LOW ? millis() : 0; } @@ -297,14 +297,18 @@ void AmBumperDriver::begin(){ } void AmBumperDriver::getTriggeredBumper(bool &leftBumper, bool &rightBumper){ - leftBumper = leftPressed; - rightBumper = rightPressed; + const uint32_t now = millis(); + leftBumper = leftTriggeredSince != 0 && (now - leftTriggeredSince) > triggerTime; + rightBumper = rightTriggeredSince != 0 && (now - rightTriggeredSince) > triggerTime; } bool AmBumperDriver::obstacle(){ - return (leftPressed || rightPressed); + bool left, right; + getTriggeredBumper(left, right); + + return left || right; } - + void AmBumperDriver::run(){ } diff --git a/sunray/src/driver/AmRobotDriver.h b/sunray/src/driver/AmRobotDriver.h index f0ca0ce9c..25b35bd41 100644 --- a/sunray/src/driver/AmRobotDriver.h +++ b/sunray/src/driver/AmRobotDriver.h @@ -56,13 +56,16 @@ class AmBatteryDriver : public BatteryDriver { }; class AmBumperDriver: public BumperDriver { - public: + private: + uint32_t triggerTime; + public: + AmBumperDriver(uint32_t _triggerTime) : triggerTime(_triggerTime) {} void begin() override; void run() override; bool obstacle() override; // get triggered bumper - void getTriggeredBumper(bool &leftBumper, bool &rightBumper) override; + void getTriggeredBumper(bool &leftBumper, bool &rightBumper) override; };