Set error if bumper stays permanently triggered

alfred/config.h

@@ -286,9 +286,11 @@ Also, you may choose the serial port below for serial monitor output (CONSOLE).
 // see Wiki on how to install bumperduino or freewheel sensor:
 // https://wiki.ardumower.de/index.php?title=Bumper_sensor
 // https://wiki.ardumower.de/index.php?title=Free_wheel_sensor
-// #define BUMPER_ENABLE true
 #define BUMPER_ENABLE true
-#define BUMPER_DEADTIME 1000  // linear motion dead-time (ms) after bumper is allowed to trigger
+//#define BUMPER_ENABLE false
+#define BUMPER_DEADTIME 1000  		// linear motion dead-time (ms) after bumper is allowed to trigger
+#define BUMPER_TRIGGER_DELAY  0		// bumper must be active for (ms) to trigger
+#define BUMPER_MAX_TRIGGER_TIME 30	// if bumpersensor stays permanent triggered mower will stop with bumper error (time in seconds; 0 = disabled)																																				  
 
 
 // ----- battery charging current measurement (INA169) --------------

sunray/bumper.cpp

@@ -0,0 +1,86 @@
+// Ardumower Sunray
+// Copyright (c) 2013-2020 by Alexander Grau, Grau GmbH
+// Licensed GPLv3 for open source use
+// or Grau GmbH Commercial License for commercial use (http://grauonline.de/cms2/?page_id=153)
+
+#include "bumper.h"
+#include "config.h"
+#include "robot.h"
+#include <Arduino.h>
+
+volatile bool inputLeftPressed = false;
+volatile bool inputRightPressed = false;
+
+volatile bool outputLeftPressed = false;
+volatile bool outputRightPressed = false;
+
+unsigned long leftPressedOnDelay = 0; // on delay timer (BUMPER_TRIGGER_DELAY) for the bumper inputs
+unsigned long rightPressedOnDelay = 0;
+
+unsigned long bumperStayActivTime = 0;    // duration, the bumper stays triggered
+unsigned long lastCallBumperObstacle = 0; // last call for bumper.obstacle
+
+
+void Bumper::begin(){
+  bumperDriver.begin();
+
+  leftPressedOnDelay  = millis();
+  rightPressedOnDelay = millis();
+}
+
+void Bumper::run() {
+  if (BUMPER_ENABLE){
+    bumperDriver.run();
+
+    inputLeftPressed   = bumperDriver.getLeftBumper();
+    inputRightPressed  = bumperDriver.getRightBumper();
+    bool bumperLeft   = false;
+    bool bumperRight  = false;
+
+    // delay for the bumper inputs
+    if (inputLeftPressed){
+      if (millis() >= (leftPressedOnDelay + BUMPER_TRIGGER_DELAY)) bumperLeft = true;
+    } else leftPressedOnDelay = millis();
+
+    if (inputRightPressed){
+      if (millis() >= (rightPressedOnDelay + BUMPER_TRIGGER_DELAY)) bumperRight = true;
+    } else rightPressedOnDelay = millis();
+
+    if (millis() > (linearMotionStartTime + BUMPER_DEADTIME)){
+      outputLeftPressed   = bumperLeft;
+      outputRightPressed  = bumperRight;
+    } else outputLeftPressed = outputRightPressed = false;
+
+    // check if bumper stays triggered for a long time periode (maybe blocked)
+    if ((bumperRight || bumperLeft) && (BUMPER_MAX_TRIGGER_TIME > 0)){
+      if ((abs(motor.linearSpeedSet) >= 0.05) || (abs(motor.angularSpeedSet) >= 0.05)) { // if no movement, bumperStayActivTime paused
+        bumperStayActivTime = bumperStayActivTime + (millis()-lastCallBumperObstacle);
+      }
+      if ((bumperStayActivTime) > (BUMPER_MAX_TRIGGER_TIME * 1000)){ // maximum trigger time reached -> set error
+        if (stateOp != OP_ERROR){
+          stateSensor = SENS_BUMPER;
+          CONSOLE.println("ERROR BUMPER BLOCKED - BUMPER_MAX_TRIGGER_TIME exceeded. See config.h for further information");
+          setOperation(OP_ERROR);
+        }
+      }
+    } else bumperStayActivTime = 0;
+    lastCallBumperObstacle = millis();
+  }
+}
+
+
+bool Bumper::obstacle(){
+  if (BUMPER_ENABLE){
+    return (outputLeftPressed || outputRightPressed);
+  }
+  else return false;
+}
+
+// send separated signals without delay to sensortest
+bool Bumper::testLeft(){
+  return (inputLeftPressed);
+}
+
+bool Bumper::testRight(){
+  return (inputRightPressed);
+}

sunray/bumper.h

@@ -0,0 +1,20 @@
+// Ardumower Sunray 
+// Copyright (c) 2013-2020 by Alexander Grau, Grau GmbH
+// Licensed GPLv3 for open source use
+// or Grau GmbH Commercial License for commercial use (http://grauonline.de/cms2/?page_id=153)
+
+
+#ifndef BUMPER_H
+#define BUMPER_H
+
+class Bumper {
+	public:
+	void begin();
+	void run();
+	bool obstacle();
+	bool testLeft();    // for sensortest
+	bool testRight();   // for sensortest
+	protected:
+};
+
+#endif

sunray/config_example.h

@@ -293,10 +293,11 @@ Also, you may choose the serial port below for serial monitor output (CONSOLE).
 // see Wiki on how to install bumperduino or freewheel sensor:
 // https://wiki.ardumower.de/index.php?title=Bumper_sensor
 // https://wiki.ardumower.de/index.php?title=Free_wheel_sensor
-// #define BUMPER_ENABLE true
+//#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  0 // bumper must be active for (ms) to trigger
+#define BUMPER_MAX_TRIGGER_TIME 30  // if bumpersensor stays permanent triggered mower will stop with bumper error (time in seconds; 0 = disabled)
 
 // ----- battery charging current measurement (INA169) --------------
 // the Marotronics charger outputs max 1.5A 
@@ -611,4 +612,3 @@ Also, you may choose the serial port below for serial monitor output (CONSOLE).
 #ifdef BNO055
   #define MPU9250   // just to make mpu driver happy to compile something
 #endif
-

sunray/robot.cpp

@@ -43,7 +43,7 @@
 #include "cpu.h"
 #include "i2c.h"
 #include "src/test/test.h"
-
+#include "bumper.h"
 
 // #define I2C_SPEED  10000
 #define _BV(x) (1 << (x))
@@ -65,7 +65,7 @@ const signed char orientationMatrix[9] = {
   SerialRobotDriver robotDriver;
   SerialMotorDriver motorDriver(robotDriver);
   SerialBatteryDriver batteryDriver(robotDriver);
-  SerialBumperDriver bumper(robotDriver);
+  SerialBumperDriver bumperDriver(robotDriver);
   SerialStopButtonDriver stopButton(robotDriver);
   SerialRainSensorDriver rainDriver(robotDriver);
   SerialLiftSensorDriver liftDriver(robotDriver);
@@ -74,7 +74,7 @@ const signed char orientationMatrix[9] = {
   SimRobotDriver robotDriver;
   SimMotorDriver motorDriver(robotDriver);
   SimBatteryDriver batteryDriver(robotDriver);
-  SimBumperDriver bumper(robotDriver);
+  SimBumperDriver bumperDriver(robotDriver);
   SimStopButtonDriver stopButton(robotDriver);
   SimRainSensorDriver rainDriver(robotDriver);
   SimLiftSensorDriver liftDriver(robotDriver);
@@ -83,7 +83,7 @@ const signed char orientationMatrix[9] = {
   AmRobotDriver robotDriver;
   AmMotorDriver motorDriver;
   AmBatteryDriver batteryDriver;
-  AmBumperDriver bumper;
+  AmBumperDriver bumperDriver;
   AmStopButtonDriver stopButton;
   AmRainSensorDriver rainDriver;
   AmLiftSensorDriver liftDriver;
@@ -102,6 +102,7 @@ PinManager pinMan;
 BLEConfig bleConfig;
 Buzzer buzzer;
 Sonar sonar;
+Bumper bumper;
 VL53L0X tof(VL53L0X_ADDRESS_DEFAULT);
 Map maps;
 RCModel rcmodel;
@@ -203,7 +204,7 @@ void sensorTest(){
   while (millis() < stopTime){
     sonar.run();
     bumper.run();
-	liftDriver.run();
+    liftDriver.run();
     if (millis() > nextMeasureTime){
       nextMeasureTime = millis() + 1000;      
       if (SONAR_ENABLE){
@@ -228,16 +229,19 @@ void sensorTest(){
         CONSOLE.print("\t");
       }    
       if (BUMPER_ENABLE){
-        CONSOLE.print("bumper (triggered): ");
-        CONSOLE.print(((int)bumper.obstacle()));
+        CONSOLE.print("bumper (left,right,triggered): ");
+        CONSOLE.print(((int)bumper.testLeft()));
         CONSOLE.print("\t");
-
+        CONSOLE.print(((int)bumper.testRight()));
+        CONSOLE.print("\t");
+        CONSOLE.print(((int)bumper.obstacle()));
+        CONSOLE.print("\t");       
       }
-	#ifdef ENABLE_LIFT_DETECTION 
+	    #ifdef ENABLE_LIFT_DETECTION 
         CONSOLE.print("lift sensor (triggered): ");		
         CONSOLE.print(((int)liftDriver.triggered()));	
         CONSOLE.print("\t");							            
-    #endif  
+      #endif  
 
       CONSOLE.println();  
       watchdogReset();
@@ -441,6 +445,12 @@ void outputConfig(){
   CONSOLE.println(RAIN_ENABLE);
   CONSOLE.print("BUMPER_ENABLE: ");
   CONSOLE.println(BUMPER_ENABLE);
+  CONSOLE.print("BUMPER_DEADTIME: ");
+  CONSOLE.println(BUMPER_DEADTIME);
+  CONSOLE.print("BUMPER_TRIGGER_DELAY: ");
+  CONSOLE.println(BUMPER_TRIGGER_DELAY);
+  CONSOLE.print("BUMPER_MAX_TRIGGER_TIME: ");
+  CONSOLE.println(BUMPER_MAX_TRIGGER_TIME);  
   CONSOLE.print("CURRENT_FACTOR: ");
   CONSOLE.println(CURRENT_FACTOR);
   CONSOLE.print("GO_HOME_VOLTAGE: ");
@@ -743,14 +753,13 @@ bool detectObstacle(){
     #endif
   #endif
 
-  if (BUMPER_ENABLE){
-    if ( (millis() > linearMotionStartTime + BUMPER_DEADTIME) && (bumper.obstacle()) ){  
-      CONSOLE.println("bumper obstacle!");    
-      statMowBumperCounter++;
-      triggerObstacle();    
-      return true;
-    }
+  if (bumper.obstacle()){  
+    CONSOLE.println("bumper obstacle!");    
+    statMowBumperCounter++;
+    triggerObstacle();    
+    return true;
   }
+
   if (sonar.obstacle() && (maps.wayMode != WAY_DOCK)){
     CONSOLE.println("sonar obstacle!");    
     statMowSonarCounter++;
@@ -849,6 +858,7 @@ void run(){
   sonar.run();
   maps.run();  
   rcmodel.run();
+  bumper.run();
 
   // state saving
   if (millis() >= nextSaveTime){  
@@ -1045,4 +1055,3 @@ void setOperation(OperationType op, bool allowRepeat, bool initiatedbyOperator){
   activeOp->changeOperationType(stateOp, initiatedbyOperator);
   saveState();
 }
-

sunray/robot.h

@@ -19,6 +19,7 @@
 #include "battery.h"
 #include "ble.h"
 #include "pinman.h"
+#include "bumper.h"
 #include "buzzer.h"
 #include "sonar.h"
 #include "VL53L0X.h"
@@ -105,7 +106,7 @@ extern int motorErrorCounter;
   extern SerialRobotDriver robotDriver;
   extern SerialMotorDriver motorDriver;
   extern SerialBatteryDriver batteryDriver;
-  extern SerialBumperDriver bumper;
+  extern SerialBumperDriver bumperDriver;
   extern SerialStopButtonDriver stopButton;
   extern SerialRainSensorDriver rainDriver;
   extern SerialLiftSensorDriver liftDriver;  
@@ -114,7 +115,7 @@ extern int motorErrorCounter;
   extern SimRobotDriver robotDriver;
   extern SimMotorDriver motorDriver;
   extern SimBatteryDriver batteryDriver;
-  extern SimBumperDriver bumper;
+  extern SimBumperDriver bumperDriver;
   extern SimStopButtonDriver stopButton;
   extern SimRainSensorDriver rainDriver;
   extern SimLiftSensorDriver liftDriver;
@@ -123,7 +124,7 @@ extern int motorErrorCounter;
   extern AmRobotDriver robotDriver;
   extern AmMotorDriver motorDriver;
   extern AmBatteryDriver batteryDriver;
-  extern AmBumperDriver bumper;
+  extern AmBumperDriver bumperDriver;
   extern AmStopButtonDriver stopButton;
   extern AmRainSensorDriver rainDriver;
   extern AmLiftSensorDriver liftDriver;
@@ -141,6 +142,7 @@ extern int motorErrorCounter;
 extern Motor motor;
 extern Battery battery;
 extern BLEConfig bleConfig;
+extern Bumper bumper;
 extern Buzzer buzzer;
 extern Sonar sonar;
 extern VL53L0X tof;

sunray/src/driver/AmRobotDriver.cpp

@@ -640,6 +640,13 @@ bool AmBumperDriver::obstacle(){
   return (leftPressed || rightPressed);
 }
 
+bool AmBumperDriver::getLeftBumper(){
+  return (leftPressed);
+}
+
+bool AmBumperDriver::getRightBumper(){
+  return (rightPressed);
+}
 
 void AmBumperDriver::run(){  
 }

sunray/src/driver/AmRobotDriver.h

@@ -117,7 +117,9 @@ class AmBumperDriver: public BumperDriver {
     void begin() override;
     void run() override;
     bool obstacle() override;
-
+    bool getLeftBumper() override;
+    bool getRightBumper() override;
+
     // get triggered bumper
     void getTriggeredBumper(bool &leftBumper, bool &rightBumper) override;  	  		    
 };

sunray/src/driver/RobotDriver.h

@@ -70,6 +70,8 @@ class BumperDriver {
     virtual void begin() = 0;
     virtual void run() = 0;
     virtual bool obstacle() = 0;
+    virtual bool getLeftBumper() = 0;
+    virtual bool getRightBumper() = 0;
 
     // get triggered bumper
     virtual void getTriggeredBumper(bool &leftBumper, bool &rightBumper) = 0;  	  		    

sunray/src/driver/SerialRobotDriver.cpp

@@ -757,6 +757,14 @@ bool SerialBumperDriver::obstacle(){
   return (serialRobot.triggeredLeftBumper || serialRobot.triggeredRightBumper); 
 }
 
+bool SerialBumperDriver::getLeftBumper(){
+  return (serialRobot.triggeredLeftBumper);
+}
+
+bool SerialBumperDriver::getRightBumper(){
+  return (serialRobot.triggeredRightBumper);
+}	
+
 void SerialBumperDriver::getTriggeredBumper(bool &leftBumper, bool &rightBumper){
   leftBumper = serialRobot.triggeredLeftBumper;
   rightBumper = serialRobot.triggeredRightBumper;

sunray/src/driver/SerialRobotDriver.h

@@ -123,6 +123,8 @@ class SerialBumperDriver: public BumperDriver {
     void begin() override;
     void run() override;
     bool obstacle() override;
+    bool getLeftBumper() override;
+    bool getRightBumper() override;
     void getTriggeredBumper(bool &leftBumper, bool &rightBumper) override;  	  		    
 };
 

sunray/src/driver/SimRobotDriver.cpp

@@ -318,6 +318,14 @@ bool SimBumperDriver::obstacle(){
   return (simTriggered || simRobot.robotIsBumpingIntoObstacle);
 }
 
+bool SimBumperDriver::getLeftBumper(){
+  return (simTriggered || simRobot.robotIsBumpingIntoObstacle);
+}
+
+bool SimBumperDriver::getRightBumper(){
+  return (simTriggered || simRobot.robotIsBumpingIntoObstacle);
+}
+
 void SimBumperDriver::getTriggeredBumper(bool &leftBumper, bool &rightBumper){
   leftBumper = (simTriggered || simRobot.robotIsBumpingIntoObstacle);
   rightBumper = (simTriggered || simRobot.robotIsBumpingIntoObstacle);

sunray/src/driver/SimRobotDriver.h

@@ -102,6 +102,8 @@ class SimBumperDriver: public BumperDriver {
     void begin() override;
     void run() override;
     bool obstacle() override;
+    bool getLeftBumper() override;
+    bool getRightBumper() override;
     void getTriggeredBumper(bool &leftBumper, bool &rightBumper) override;  	  		    
     // ----- simulate errors, sensor triggers ----
     void setSimTriggered(bool flag);