commit 019224ff86e1a6b0921b44284be040fab39843e3
Author: galberding <galberding@techfak.uni-bielefeld.de>
Date:   Tue Nov 26 16:47:02 2019 +0100

    Add new correction behaviour when amiro drives to the loading station and fails to dock, change undock behaviour (AMiRo rotates clockwise 20 degrees to free from station magnet, drives 0.5 cm, rotates back and drives again

diff --git a/devices/DiWheelDrive/userthread.cpp b/devices/DiWheelDrive/userthread.cpp
index 08f6dd3..3107ba1 100644
--- a/devices/DiWheelDrive/userthread.cpp
+++ b/devices/DiWheelDrive/userthread.cpp
@@ -130,6 +130,7 @@ states newState;
    int whiteBuf = 0;
    int proxyBuf = 0;
    int correctionStep = 0;
+   uint16_t rProx[8]; // buffer for ring proxy values
   int rpmSpeed[2] = {0};
   int stop[2] = {0};
   LineFollowStrategy lStrategy = LineFollowStrategy::FUZZY;
@@ -199,8 +200,11 @@ states newState;
     newState = utState;
 
     // Get sensor data 
-    int WL = global.vcnl4020[constants::DiWheelDrive::PROX_WHEEL_LEFT].getProximityScaledWoOffset();
-    int WR = global.vcnl4020[constants::DiWheelDrive::PROX_WHEEL_RIGHT].getProximityScaledWoOffset();
+    uint16_t WL = global.vcnl4020[constants::DiWheelDrive::PROX_WHEEL_LEFT].getProximityScaledWoOffset();
+    uint16_t WR = global.vcnl4020[constants::DiWheelDrive::PROX_WHEEL_RIGHT].getProximityScaledWoOffset();
+    for(int i=0; i<8;i++){
+      rProx[i] = global.robot.getProximityRingValue(i);
+    }
     // int FL = global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_LEFT].getProximityScaledWoOffset();
     // int FR = global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_RIGHT].getProximityScaledWoOffset();
     switch(utState){
@@ -254,9 +258,9 @@ states newState;
         break;
       // ---------------------------------------
       case states::DETECT_STATION:
-        if (global.forwardSpeed != CHARGING_SPEED){
-          global.forwardSpeed = CHARGING_SPEED;
-        }
+        // if (global.forwardSpeed != CHARGING_SPEED){
+        //   global.forwardSpeed = CHARGING_SPEED;
+        // }
         if(lf.getStrategy() != LineFollowStrategy::EDGE_RIGHT){
           lf.setStrategy(LineFollowStrategy::EDGE_RIGHT);
         }
@@ -264,7 +268,10 @@ states newState;
         lf.followLine(rpmSpeed);
         setRpmSpeed(rpmSpeed);
         // // Detect marker before docking station
-        if ((WL+WR) < PROXY_WHEEL_THRESH){
+        // if ((WL+WR) < PROXY_WHEEL_THRESH){
+        // Use proxy ring 
+        if ((rProx[3]+rProx[4]) > RING_PROX_FRONT_THRESH){
+
           setRpmSpeed(stop);
           checkForMotion();
           // 180° Rotation 
@@ -276,6 +283,9 @@ states newState;
         break;
       // ---------------------------------------
       case states::CORRECT_POSITIONING:
+        if (global.forwardSpeed != CHARGING_SPEED){
+          global.forwardSpeed = CHARGING_SPEED;
+        }
         if(lf.getStrategy() != LineFollowStrategy::EDGE_LEFT){
           lf.promptStrategyChange(LineFollowStrategy::EDGE_LEFT);
         }
@@ -298,7 +308,17 @@ states newState;
         lf.followLine(rpmSpeed);
         setRpmSpeed(rpmSpeed);
 
-        if ((WL+WR) < PROXY_WHEEL_THRESH){
+        // if ((WL+WR) < PROXY_WHEEL_THRESH){
+        // 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;
@@ -324,9 +344,25 @@ states newState;
             // No voltage on pins -> falsely docked
             // deactivate pins
             global.robot.requestCharging(0);
-            newState = states::CORRECT_POSITIONING;
+            newState = states::RELEASE_TO_CORRECT;
           }
         }
+        break;
+      // ---------------------------------------
+      case states::RELEASE_TO_CORRECT:
+        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, -2*ROTATION_20, ROTATION_DURATION);
+        checkForMotion();
+
+        global.distcontrol.setTargetPosition(7000, 0, ROTATION_DURATION);
+        checkForMotion();
+        newState = states::CORRECT_POSITIONING;
+        break;
       // ---------------------------------------
       case states::CHARGING:
         if (global.motorcontrol.getMotorEnable()){
@@ -355,9 +391,19 @@ states newState;
           }
           //Rotate -20° to free from magnet
           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();
           lStrategy = LineFollowStrategy::EDGE_RIGHT;
           newState = states::FOLLOW_LINE;
+          whiteBuf = -100;
         }
         lightAllLeds(Color::BLACK);
         break;
diff --git a/devices/DiWheelDrive/userthread.hpp b/devices/DiWheelDrive/userthread.hpp
index b6a4eef..a1f4127 100644
--- a/devices/DiWheelDrive/userthread.hpp
+++ b/devices/DiWheelDrive/userthread.hpp
@@ -9,7 +9,7 @@
 // Speed when driving towards the docking station
 #define CHARGING_SPEED 5
 // Thresh to determain how much update steps should pass while alining
-#define MAX_CORRECTION_STEPS 250
+#define MAX_CORRECTION_STEPS 260
 // Thresh for wheel proxy sensors, when summed values fall below the state changes
 #define PROXY_WHEEL_THRESH 18000
 // Thresh for detecting obsticles
@@ -22,6 +22,8 @@
 #define ROTATION_20 -349065
 #define ROTATION_DURATION 10000
 
+#define RING_PROX_FRONT_THRESH 20000
+#define PROX_MAX_VAL 65530
 
 namespace amiro {
 
@@ -46,6 +48,7 @@ class UserThread : public chibios_rt::BaseStaticThread<USER_THREAD_STACK_SIZE>
     enum states{
       FOLLOW_LINE,
       RELEASE,
+      RELEASE_TO_CORRECT,
       CHARGING,
       DETECT_STATION,
       CORRECT_POSITIONING,
