From 4a70cc1adfd2616a7ecf07e671d6b576871319f9 Mon Sep 17 00:00:00 2001
From: Stefan Priebe <stefan-git@prie.be>
Date: Wed, 24 Aug 2022 21:57:45 +0200
Subject: [PATCH] robot: do not check for linearmotion time + DEADTIME  -
 instead use a custom timeout

---
 sunray/robot.cpp | 39 ++++++++++++++++++++++++++++-----------
 1 file changed, 28 insertions(+), 11 deletions(-)

diff --git a/sunray/robot.cpp b/sunray/robot.cpp
index 215054c51..e68a6173b 100644
--- a/sunray/robot.cpp
+++ b/sunray/robot.cpp
@@ -147,6 +147,9 @@ unsigned long nextTempTime = 0;
 unsigned long imuDataTimeout = 0;
 unsigned long nextSaveTime = 0;
 
+unsigned long liftTimeout = 0;
+unsigned long bumperTimeout = 0;
+
 bool wifiFound = false;
 char ssid[] = WIFI_SSID;      // your network SSID (name)
 char pass[] = WIFI_PASS;        // your network password
@@ -744,22 +747,36 @@ bool detectObstacle(){
   
   #ifdef ENABLE_LIFT_DETECTION
     #ifdef LIFT_OBSTACLE_AVOIDANCE
-      if ( (millis() > linearMotionStartTime + BUMPER_DEADTIME) && (liftDriver.triggered()) ) {
-        CONSOLE.println("lift sensor obstacle!");    
-        statMowBumperCounter++;
-        triggerObstacle();    
-        return true;
+      if (liftDriver.triggered()) {
+        if (liftTimeout == 0) {
+          liftTimeout = millis() + BUMPER_DEADTIME;
+        } else if (liftTimeout < millis()) {
+          liftTimeout = 0;
+          CONSOLE.println("lift sensor obstacle!");    
+          statMowBumperCounter++;
+          triggerObstacle();    
+          return true;
+        }
+      } else {
+        liftTimeout = 0;
       }
     #endif
   #endif
 
-  if ( (millis() > linearMotionStartTime + BUMPER_DEADTIME) && (bumper.obstacle()) ){  
-    CONSOLE.println("bumper obstacle!");    
-    statMowBumperCounter++;
-    triggerObstacle();    
-    return true;
+  if (bumper.obstacle()) {
+    if (bumperTimeout == 0) {
+      bumperTimeout = millis() + BUMPER_DEADTIME;
+    } else if (bumperTimeout < millis()) {
+      bumperTimeout = 0;
+      CONSOLE.println("bumper obstacle!");    
+      statMowBumperCounter++;
+      triggerObstacle();    
+      return true;
+    }
+  } else {
+    bumperTimeout = 0;
   }
-  
+
   if (sonar.obstacle() && (maps.wayMode != WAY_DOCK)){
     CONSOLE.println("sonar obstacle!");    
     statMowSonarCounter++;