commit b24df8adc5c717a7d826afd0fabc2c74de4d0fa0
Author: galberding <galberding@techfak.uni-bielefeld.de>
Date:   Wed Feb 19 16:19:38 2020 +0100

    Add DEVIATION_CORRECTION state for a more precise docking maneuver

diff --git a/devices/DiWheelDrive/userthread.cpp b/devices/DiWheelDrive/userthread.cpp
index 0bbe765..c6713de 100644
--- a/devices/DiWheelDrive/userthread.cpp
+++ b/devices/DiWheelDrive/userthread.cpp
@@ -233,6 +233,20 @@ int UserThread::getProxyRingSum(){
   return prox_sum;
 }
 
+int32_t UserThread::meanDeviation(uint16_t a, uint16_t b){
+  int32_t diff = a - b;
+  int32_t res = 0; 
+  devCor.proxbuf[devCor.pCount] = (diff*100)/((a+b)/2);
+  for (int i = 0; i< PROX_DEVIATION_MEAN_WINDOW; i++){
+    res += devCor.proxbuf[i];
+  }
+  devCor.pCount++;
+  devCor.pCount = devCor.pCount % PROX_DEVIATION_MEAN_WINDOW;
+
+  devCor.currentDeviation =  res / PROX_DEVIATION_MEAN_WINDOW;
+  return devCor.currentDeviation;
+}
+
 
 UserThread::UserThread() :
   chibios_rt::BaseStaticThread<USER_THREAD_STACK_SIZE>()
@@ -335,6 +349,9 @@ UserThread::main()
     for(int i=0; i<8;i++){
       rProx[i] = global.robot.getProximityRingValue(i);
     }
+
+    // Continously update devication values
+    meanDeviation(rProx[0] & 0xFFF0, rProx[7] & 0xFFF0);
     // int FL = global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_LEFT].getProximityScaledWoOffset();
     // int FR = global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_RIGHT].getProximityScaledWoOffset();
     switch(currentState){
@@ -494,13 +511,28 @@ UserThread::main()
         }
         lf.followLine(rpmSpeed);
         setRpmSpeed(rpmSpeed);
-        utCount.stateTime++;
+        // utCount.stateTime++;
 
-        if((rProx[0] >= PROX_MAX_VAL) || (rProx[7] >= PROX_MAX_VAL)){
+        // Docking is only successful if Deviation is in range and sensors are at their max values.
+        if((rProx[0] >= PROX_MAX_VAL) && (rProx[7] >= PROX_MAX_VAL) && ((devCor.currentDeviation > -MAX_DEVIATION_FACTOR) &&  (devCor.currentDeviation < MAX_DEVIATION_FACTOR) )){
           // setRpmSpeed(stop);
           // checkForMotion();
           utCount.stateTime = 0;
           newState = states::PUSH_BACK;
+        }else if ((devCor.currentDeviation <= -MAX_DEVIATION_FACTOR) && ((rProx[0] > DEVIATION_DIST_THRESH) || (rProx[7] > DEVIATION_DIST_THRESH))){
+          // Case R 
+          utCount.stateTime = 0;
+          setRpmSpeed(stop);
+          devCor.RCase = true;
+          lightAllLeds(Color::YELLOW);
+          newState = states::DEVIATION_CORRECTION;
+        }else if ((devCor.currentDeviation >= MAX_DEVIATION_FACTOR) && ((rProx[0] > DEVIATION_DIST_THRESH) || (rProx[7] > DEVIATION_DIST_THRESH))){
+          // Case L 
+          utCount.stateTime = 0;
+          setRpmSpeed(stop);
+          devCor.RCase = false;
+          lightAllLeds(Color::WHITE);
+          newState = states::DEVIATION_CORRECTION;
         }else if (utCount.stateTime >= REVERSE_DOCKING_TIMEOUT){
           setRpmSpeed(stop);
           utCount.stateTime = 0;
@@ -514,6 +546,48 @@ UserThread::main()
 
       break;
       // ---------------------------------------
+      case states::DEVIATION_CORRECTION:
+        // if(lf.getStrategy() != LineFollowStrategy::REVERSE){
+        //   lf.setStrategy(LineFollowStrategy::REVERSE);
+        // }
+        // lf.followLine(rpmSpeed);
+        // setRpmSpeed(rpmSpeed);
+        if(utCount.stateTime < DEVIATION_CORRECTION_DURATION / 2 ){
+          if(devCor.RCase){
+            rpmSpeed[0] = 0;
+            rpmSpeed[1] = DEVIATION_CORRECTION_SPEED;
+          }else {
+            rpmSpeed[0] = DEVIATION_CORRECTION_SPEED;
+            rpmSpeed[1] = 0;
+          }
+          setRpmSpeed(rpmSpeed);
+        }else if ((utCount.stateTime >= DEVIATION_CORRECTION_DURATION / 2) && (utCount.stateTime < DEVIATION_CORRECTION_DURATION)){
+          if(devCor.RCase){
+            rpmSpeed[0] = DEVIATION_CORRECTION_SPEED;
+            rpmSpeed[1] = 0;
+          }else {
+            rpmSpeed[0] = 0;
+            rpmSpeed[1] = DEVIATION_CORRECTION_SPEED;
+          }
+          setRpmSpeed(rpmSpeed);
+        }else if (utCount.stateTime >= DEVIATION_CORRECTION_DURATION + 10) {  
+          // Wait to clear the mean window buffer
+          setRpmSpeed(stop);
+        }else{
+          utCount.stateTime = 0;
+          newState = states::REVERSE;
+          setRpmSpeed(stop);
+        }
+
+        utCount.stateTime++;
+
+
+        // if (utCount.stateTime > PUSH_BACK_TIMEOUT){
+        //   utCount.stateTime = 0;
+        //   newState = states::CHECK_POSITIONING;
+        // }
+      break;
+      // ---------------------------------------
       case states::PUSH_BACK:
         if(lf.getStrategy() != LineFollowStrategy::REVERSE){
           lf.setStrategy(LineFollowStrategy::REVERSE);
