commit 61544eee1520bd51d0077cbd1e96cf9b52f8034c
Author: galberding <galberding@techfak.uni-bielefeld.de>
Date:   Thu Nov 28 08:40:44 2019 +0100

    Bugfix: white and  protection will not be active, for some time, after RELEASE state

diff --git a/devices/DiWheelDrive/userthread.cpp b/devices/DiWheelDrive/userthread.cpp
index 3107ba1..dd51204 100644
--- a/devices/DiWheelDrive/userthread.cpp
+++ b/devices/DiWheelDrive/userthread.cpp
@@ -80,7 +80,7 @@ int UserThread::checkDockingSuccess(){
   global.odometry.resetPosition();
   types::position start = global.startPos = global.odometry.getPosition();
   global.motorcontrol.toggleMotorEnable();
-  this->sleep(1000);
+  this->sleep(500);
   types::position stop_ = global.endPos = global.odometry.getPosition();
   
   // Amiro moved, docking was not successful
@@ -94,7 +94,7 @@ int UserThread::checkDockingSuccess(){
     success = 1;
   }
   
-  this->sleep(500);
+  // this->sleep(500);
   lightAllLeds(Color::BLACK);
   return success;
 }
@@ -129,10 +129,15 @@ states newState;
 
    int whiteBuf = 0;
    int proxyBuf = 0;
+   int releaseBuf = 0;
    int correctionStep = 0;
+   int dist_count = 0;
+   bool needDistance = false;
+
    uint16_t rProx[8]; // buffer for ring proxy values
   int rpmSpeed[2] = {0};
   int stop[2] = {0};
+  int turn[2] = {5,-5};
   LineFollowStrategy lStrategy = LineFollowStrategy::FUZZY;
   for (uint8_t led = 0; led < 8; ++led) {
     global.robot.setLightColor(led, Color(Color::BLACK));
@@ -216,7 +221,8 @@ states newState;
         if(/* checkPinVoltage() && */ checkPinEnabled()){
           global.robot.requestCharging(0);
         }
-        
+        whiteBuf = 0;
+        proxyBuf = 0;
         break;
       // ---------------------------------------
       case states::FOLLOW_LINE:
@@ -231,36 +237,53 @@ states newState;
 
         //TODO: Check if white is detected and stop threshold is reached
         if(lf.followLine(rpmSpeed)){
-          
+          whiteBuf++;
           if(whiteBuf >= WHITE_COUNT_THRESH){
-            setRpmSpeed(stop);
+            rpmSpeed[0] = stop[0];
+            rpmSpeed[1] = stop[1];
             newState = states::IDLE;
-          }else{
-            whiteBuf++;
           }
         }else{
           whiteBuf = 0;
-          setRpmSpeed(rpmSpeed);
+          // setRpmSpeed(rpmSpeed);
         }
 
         if(getProxyRingSum() > PROXY_RING_THRESH){
-          setRpmSpeed(stop);
           proxyBuf++;
           if(proxyBuf > WHITE_COUNT_THRESH){
             newState = states::IDLE;
+            rpmSpeed[0] = stop[0];
+            rpmSpeed[1] = stop[1];
           }
         }else{
             proxyBuf = 0;
-          }
+        }
+
+        if ((rProx[3] + rProx[4]) > RING_PROX_FRONT_THRESH){
+          rpmSpeed[0] = stop[0];
+          rpmSpeed[1] = stop[1];
+        }
+
+        // if (needDistance){
+        //   whiteBuf = 0;
+        //   proxyBuf = 0;
+        //   dist_count++;
+        //   if(dist_count > DIST_THRESH){
+        //     dist_count = 0;
+        //     needDistance = false;
+        //   }
+        // }
+        // whiteBuf = 0;
+        // proxyBuf = 0;
         // lf.followLine(rpmSpeed);
-        // setRpmSpeed(rpmSpeed);
+        setRpmSpeed(rpmSpeed);
         
         break;
       // ---------------------------------------
       case states::DETECT_STATION:
-        // if (global.forwardSpeed != CHARGING_SPEED){
-        //   global.forwardSpeed = CHARGING_SPEED;
-        // }
+        if (global.forwardSpeed != DETECTION_SPEED){
+          global.forwardSpeed = DETECTION_SPEED;
+        }
         if(lf.getStrategy() != LineFollowStrategy::EDGE_RIGHT){
           lf.setStrategy(LineFollowStrategy::EDGE_RIGHT);
         }
@@ -312,25 +335,32 @@ states newState;
         // Is of those sensors at it max val means that the AMiRo cant drive back
         // so check if correctly positioned
         //definitely wrong positioned -> correct position directly without rotation
-        if ((rProx[0] >= PROX_MAX_VAL && rProx[7] < PROX_MAX_VAL) || (rProx[0] < PROX_MAX_VAL && rProx[7] >= PROX_MAX_VAL)){
-          setRpmSpeed(stop);
-          checkForMotion();
-          newState = states::CORRECT_POSITIONING;
-        }
         // Both sensors are at a wall -> potential loading station dock and on magnet -> need to rotate
+        // if((rProx[0] >= PROX_MAX_VAL) && (rProx[7] >= PROX_MAX_VAL)){
+        //   setRpmSpeed(stop);
+        //   checkForMotion();
+        //   newState = states::CHECK_POSITIONING;
+        // } else 
         if((rProx[0] >= PROX_MAX_VAL) || (rProx[7] >= PROX_MAX_VAL)){
-          setRpmSpeed(stop);
-          checkForMotion();
+          // setRpmSpeed(stop);
+          // checkForMotion();
           newState = states::CHECK_POSITIONING;
         }
         break;
       // ---------------------------------------
       case states::CHECK_POSITIONING:
-        if(checkDockingSuccess()){
-          newState = states::CHECK_VOLTAGE;
-        }else{
-          newState = states::CORRECT_POSITIONING;
-        }
+        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;
+          }
+        // }
+        // else{
+        //   newState = CORRECT_POSITIONING;
+        // }
         break;
       // ---------------------------------------
       case states::CHECK_VOLTAGE:
@@ -344,7 +374,11 @@ states newState;
             // No voltage on pins -> falsely docked
             // deactivate pins
             global.robot.requestCharging(0);
-            newState = states::RELEASE_TO_CORRECT;
+            if((rProx[0] >= PROX_MAX_VAL) && (rProx[7] >= PROX_MAX_VAL)){
+              newState = states::RELEASE_TO_CORRECT;
+            } else {
+              newState = states::CORRECT_POSITIONING;
+            }
           }
         }
         break;
@@ -379,33 +413,43 @@ states newState;
       
       // ---------------------------------------
       case states::RELEASE:
+      if (global.forwardSpeed != DETECTION_SPEED){
+          global.rpmForward[0] = DETECTION_SPEED;
+        }
         if(/* checkPinVoltage() && */ checkPinEnabled()){
           global.robot.requestCharging(0);
-        }
-
-        if(checkPinEnabled()){
-          showChargingState();
         }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);
+          // }
           //Rotate -20° to free from magnet
           global.distcontrol.setTargetPosition(0, ROTATION_20, ROTATION_DURATION);
-        checkForMotion();
-        // move 1cm forward
-        global.distcontrol.setTargetPosition(5000, 0, ROTATION_DURATION);
-        checkForMotion();
-        // rotate back
-        global.distcontrol.setTargetPosition(0, -ROTATION_20, ROTATION_DURATION);
-        checkForMotion();
+          checkForMotion();
+          // move 1cm forward
+          global.distcontrol.setTargetPosition(5000, 0, ROTATION_DURATION);
+          checkForMotion();
+          // rotate back
+          global.distcontrol.setTargetPosition(0, -ROTATION_20, ROTATION_DURATION);
+          checkForMotion();
 
-        global.distcontrol.setTargetPosition(5000, 0, ROTATION_DURATION);
-        checkForMotion();
+          // global.distcontrol.setTargetPosition(5000, 0, ROTATION_DURATION);
+          // checkForMotion();
           lStrategy = LineFollowStrategy::EDGE_RIGHT;
           newState = states::FOLLOW_LINE;
-          whiteBuf = -100;
+            // whiteBuf = -100;
+          // lf.followLine(rpmSpeed);
+          // setRpmSpeed(rpmSpeed);
         }
-        lightAllLeds(Color::BLACK);
+        // lightAllLeds(Color::BLACK);
         break;
       
       default:
