commit 27d4e1faa92fb89c12702e58689a0d05b5c9ab92
Author: galberding <galberding@techfak.uni-bielefeld.de>
Date:   Thu Dec 12 09:15:34 2019 +0100

    Add ERROR state to FSM

diff --git a/devices/DiWheelDrive/userthread.cpp b/devices/DiWheelDrive/userthread.cpp
index b2f8de7..b505c05 100644
--- a/devices/DiWheelDrive/userthread.cpp
+++ b/devices/DiWheelDrive/userthread.cpp
@@ -83,15 +83,15 @@ int UserThread::checkDockingSuccess(){
   int success = 0;
   global.odometry.resetPosition();
   types::position start = global.startPos = global.odometry.getPosition();
-  global.motorcontrol.toggleMotorEnable();
-  this->sleep(500);
+  global.motorcontrol.setMotorEnable(false);
+  this->sleep(1000);
   types::position stop_ = global.endPos = global.odometry.getPosition();
   
   // Amiro moved, docking was not successful
   if ((start.x + stop_.x) || (start.y + stop_.y)){
     lightAllLeds(Color::RED);
     // Enable Motor again if docking was not successful
-    global.motorcontrol.toggleMotorEnable();
+    global.motorcontrol.setMotorEnable(true);
     success = 0;
   }else{
     lightAllLeds(Color::GREEN);
@@ -133,10 +133,10 @@ states newState;
 
    int whiteBuf = 0;
    int proxyBuf = 0;
-   int releaseBuf = 0;
+  //  int reverseBuf = 0;
    int correctionStep = 0;
-   int dist_count = 0;
-   bool needDistance = false;
+  //  int dist_count = 0;
+  //  bool needDistance = false;
 
    uint16_t rProx[8]; // buffer for ring proxy values
   int rpmSpeed[2] = {0};
@@ -218,15 +218,14 @@ states newState;
     // int FR = global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_RIGHT].getProximityScaledWoOffset();
     switch(utState){
       case states::IDLE:
-        if (!global.motorcontrol.getMotorEnable()){
-            global.motorcontrol.toggleMotorEnable();
-        }
+        global.motorcontrol.setMotorEnable(true);
         setRpmSpeed(stop);
         if(/* checkPinVoltage() && */ checkPinEnabled()){
           global.robot.requestCharging(0);
         }
         whiteBuf = 0;
         proxyBuf = 0;
+        utCount.errorCount = 0;
         break;
       // ---------------------------------------
       case states::FOLLOW_LINE:
@@ -353,6 +352,20 @@ states newState;
         if((rProx[0] >= PROX_MAX_VAL) || (rProx[7] >= PROX_MAX_VAL)){
           // setRpmSpeed(stop);
           // checkForMotion();
+          utCount.reverseCount = 0;
+          newState = states::PUSH_BACK;
+        }
+        break;
+      // ---------------------------------------
+      case states::PUSH_BACK:
+        if(lf.getStrategy() != LineFollowStrategy::REVERSE){
+          lf.setStrategy(LineFollowStrategy::REVERSE);
+        }
+        lf.followLine(rpmSpeed);
+        setRpmSpeed(rpmSpeed);
+
+        utCount.reverseCount++;
+        if (utCount.reverseCount > PUSH_BACK_COUNT){
           newState = states::CHECK_POSITIONING;
         }
         break;
@@ -361,11 +374,15 @@ states newState;
         setRpmSpeed(stop);
         checkForMotion();
         // if((rProx[0] >= PROX_MAX_VAL) && (rProx[7] >= PROX_MAX_VAL)){
-          if(checkDockingSuccess()){
-            newState = states::CHECK_VOLTAGE;
-          }else{
-            newState = states::CORRECT_POSITIONING;
-          }
+        if(checkDockingSuccess()){
+          newState = states::CHECK_VOLTAGE;
+        }else{
+          utCount.errorCount++;
+          newState = states::CORRECT_POSITIONING;
+          if (utCount.errorCount > DOCKING_ERROR_THRESH){
+              newState = states::ERROR;
+            }
+        }
         // }
         // else{
         //   newState = CORRECT_POSITIONING;
@@ -378,24 +395,30 @@ states newState;
         } else {
           if(checkPinVoltage()){
             // Pins are under voltage -> correctly docked 
+            utCount.errorCount = 0;
             newState = states::CHARGING;
           }else{
+            utCount.errorCount++;
             // No voltage on pins -> falsely docked
             // deactivate pins
-            if (!global.motorcontrol.getMotorEnable()){
-            global.motorcontrol.toggleMotorEnable();
-            }
+            global.motorcontrol.setMotorEnable(true);
             global.robot.requestCharging(0);
+            // TODO: Soft release when docking falsely
             if((rProx[0] >= PROX_MAX_VAL) && (rProx[7] >= PROX_MAX_VAL)){
               newState = states::RELEASE_TO_CORRECT;
             } else {
-              newState = states::CORRECT_POSITIONING;
+              newState = states::RELEASE_TO_CORRECT; //states::CORRECT_POSITIONING; 
+            }
+
+            if (utCount.errorCount > DOCKING_ERROR_THRESH){
+              newState = states::ERROR;
             }
           }
         }
         break;
       // ---------------------------------------
       case states::RELEASE_TO_CORRECT:
+
         global.distcontrol.setTargetPosition(0, ROTATION_20, ROTATION_DURATION);
         checkForMotion();
         // move 1cm forward
@@ -410,10 +433,14 @@ states newState;
         newState = states::CORRECT_POSITIONING;
         break;
       // ---------------------------------------
+      case states::ERROR:
+        utCount.errorCount = 0;
+        lStrategy = LineFollowStrategy::EDGE_RIGHT;
+        newState = states::FOLLOW_LINE;
+        break;
+      // ---------------------------------------
       case states::CHARGING:
-        if (global.motorcontrol.getMotorEnable()){
-          global.motorcontrol.toggleMotorEnable();
-        }
+        global.motorcontrol.setMotorEnable(false);
         // Formulate Request to enable charging
         if(/* checkPinVoltage() && */ !checkPinEnabled()){
           global.robot.requestCharging(1);
@@ -431,18 +458,8 @@ states newState;
         if(/* checkPinVoltage() && */ checkPinEnabled()){
           global.robot.requestCharging(0);
         }else{
-          if (!global.motorcontrol.getMotorEnable()){
-            global.motorcontrol.toggleMotorEnable();
-          }
-          // if (releaseBuf > RELEASE_COUNT){
-          //   releaseBuf = 0;
-          //   setRpmSpeed(stop);
-          //   needDistance = true;
-          //   newState = states::FOLLOW_LINE;
-          // }else{
-          //   releaseBuf++;
-          //   setRpmSpeed(turn);
-          // }
+          global.motorcontrol.setMotorEnable(true);
+
           //Rotate -20° to free from magnet
           global.distcontrol.setTargetPosition(0, ROTATION_20, ROTATION_DURATION);
           checkForMotion();
@@ -467,6 +484,9 @@ states newState;
       default:
         break;
       }
+      if (utState != newState){
+        global.robot.transmitState(newState);
+      }
       utState = newState;
     this->sleep(CAN::UPDATE_PERIOD);
   }
diff --git a/devices/DiWheelDrive/userthread.hpp b/devices/DiWheelDrive/userthread.hpp
index 5e6cad9..185026f 100644
--- a/devices/DiWheelDrive/userthread.hpp
+++ b/devices/DiWheelDrive/userthread.hpp
@@ -17,6 +17,8 @@
 #define PROXY_WHEEL_THRESH 18000
 // Thresh for detecting obsticles
 #define PROXY_RING_THRESH 15000
+
+#define PUSH_BACK_COUNT 50
 // Thresh for how long (update steps) the front sensors are allowed to detect white
 #define WHITE_COUNT_THRESH 150
 // Rotation around 180 degrees in microradian
@@ -28,6 +30,9 @@
 #define RING_PROX_FRONT_THRESH 18000
 #define PROX_MAX_VAL 65430
 
+// Threshold for failing to dock
+#define DOCKING_ERROR_THRESH 4
+
 namespace amiro {
 
 
@@ -48,20 +53,31 @@ class UserThread : public chibios_rt::BaseStaticThread<USER_THREAD_STACK_SIZE>
     };
 
     // States of user thread state machine
-    enum states{
-      FOLLOW_LINE,
-      RELEASE,
-      RELEASE_TO_CORRECT,
-      CHARGING,
-      DETECT_STATION,
-      CORRECT_POSITIONING,
-      REVERSE,
-      CHECK_POSITIONING,
-      CHECK_VOLTAGE,
-      IDLE
+    enum states : uint8_t{
+      IDLE                = 0,
+      FOLLOW_LINE         = 1,
+      DETECT_STATION      = 2,
+      REVERSE             = 3,
+      PUSH_BACK           = 4,
+      CHECK_POSITIONING   = 5,
+      CHECK_VOLTAGE       = 6,
+      CHARGING            = 7,
+      RELEASE             = 8,
+      RELEASE_TO_CORRECT  = 9,
+      CORRECT_POSITIONING = 10,
+      ERROR               = 11
     };
 
+  struct ut_counter{
+      int whiteCount = 0;
+      int proxyCount = 0;
+      int reverseCount = 0;
+      int correctionCount = 0;
+      int errorCount = 0;
+  };
 
+  // static const struct ut_counter emptyUtCount;
+  ut_counter utCount;
 
   explicit UserThread();
 
@@ -84,6 +100,7 @@ private:
   int getProxyRingSum();
 
 
+
   /**
    * Check if current position changes when the wheel are deactivated.
    * 
