commit 8dced1c9827d46a2bba855136cec0f19824391d5
Author: galberding <galberding@techfak.uni-bielefeld.de>
Date:   Tue May 26 13:02:23 2020 +0200

    Tune thresholds for TURN

diff --git a/devices/DiWheelDrive/userthread.cpp b/devices/DiWheelDrive/userthread.cpp
index 3a4c48e..a6b1cc5 100644
--- a/devices/DiWheelDrive/userthread.cpp
+++ b/devices/DiWheelDrive/userthread.cpp
@@ -1,7 +1,7 @@
 // #include "userthread.hpp"
 #include "global.hpp"
 #include <cmath>
-#include "linefollow.hpp" 
+#include "linefollow.hpp"
 #include "userthread.hpp"
 // #include <cmath>
 // #include "global.hpp"
@@ -16,7 +16,7 @@ bool running = false;
 
 /**
  * Set speed.
- * 
+ *
  * @param rpmSpeed speed for left and right wheel in rounds/min
  */
 void UserThread::setRpmSpeedFuzzy(const int (&rpmSpeed)[2]) {
@@ -76,7 +76,7 @@ void UserThread::getProxySectorVals(uint16_t (&proxVals)[8], uint16_t (&sProx)[8
   for (int i=0; i<8; i++){
     sProx[i] = (proxVals[i] < proxVals[(i+1) % 8]) ? proxVals[i] : proxVals[(i+1) % 8];
     // chprintf((BaseSequentialStream*)&global.sercanmux1, "%d: %d, ", i, sProx[i]);
-    
+
   }
     // chprintf((BaseSequentialStream*)&global.sercanmux1, "\n");
 
@@ -95,7 +95,7 @@ void UserThread::proxSectorSpeedCorrection(int (&rpmSpeed)[2], uint16_t (&proxVa
   int32_t sPMax = 0;
   getProxySectorVals(proxVals, sProx);
   getMaxFrontSectorVal(sProx, sPMax);
-  
+
   int32_t speedL = rpmSpeed[0] - (sPMax * pCtrl.pFactor);
   int32_t speedR = rpmSpeed[1] - (sPMax * pCtrl.pFactor);
 
@@ -123,7 +123,7 @@ void UserThread::proxSectorSpeedCorrection(int (&rpmSpeed)[2], uint16_t (&proxVa
     }
   }
   chargeAsLED();
-    
+
   // chprintf((BaseSequentialStream*)&global.sercanmux1, "Max: %d factor: %d, Panel: %d SpeedL: %d SpeedR: %d ActualL: %d ActualR: %d\n",sPMax,  pCtrl.pFactor,  sPMax * pCtrl.pFactor, speedL, speedR, rpmSpeed[0], rpmSpeed[1]);
 
 
@@ -150,7 +150,7 @@ void UserThread::preventCollision( int (&rpmSpeed)[2], uint16_t (&proxVals)[8])
   }else{
     utCount.ringProxCount = 0;
   }
-  
+
 }
 
 
@@ -158,17 +158,17 @@ void UserThread::preventCollision( int (&rpmSpeed)[2], uint16_t (&proxVals)[8])
  * Blocks as long as the position changes.
  */
 void UserThread::checkForMotion(){
-  int motion = 1;
+  bool motion = true;
   int led = 0;
   types::position oldPos = global.odometry.getPosition();
   while(motion){
-    this->sleep(500);
+    this->sleep(200);
     types::position tmp = global.odometry.getPosition();
-    motion = abs(oldPos.x - tmp.x)+ abs(oldPos.y - tmp.y)+abs(oldPos.z - tmp.z);
+    motion = oldPos.x != tmp.x; //abs(oldPos.x - tmp.x)+ abs(oldPos.y - tmp.y)+abs(oldPos.z - tmp.z);
     oldPos = tmp;
-      global.robot.setLightColor((led + 1) % 8, Color(Color::YELLOW));
-      global.robot.setLightColor(led % 8, Color(Color::BLACK));
-      led++;
+    global.robot.setLightColor((led + 1) % 8, Color(Color::YELLOW));
+    global.robot.setLightColor(led % 8, Color(Color::BLACK));
+    led++;
   }
   lightAllLeds(Color::BLACK);
 }
@@ -192,7 +192,7 @@ bool UserThread::checkFrontalObject(){
 }
 
 bool UserThread::checkPinVoltage(){
-  return global.ltc4412.isPluggedIn(); 
+  return global.ltc4412.isPluggedIn();
 }
 
 bool UserThread::checkPinEnabled(){
@@ -208,7 +208,7 @@ int UserThread::checkDockingSuccess(){
   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)){
   if (abs(start.x - stop_.x) > 200 /* || (start.y + stop_.y) */){
@@ -220,7 +220,7 @@ int UserThread::checkDockingSuccess(){
     lightAllLeds(Color::GREEN);
     success = 1;
   }
-  
+
   // this->sleep(500);
   lightAllLeds(Color::BLACK);
   return success;
@@ -236,7 +236,7 @@ int UserThread::getProxyRingSum(){
 
 int32_t UserThread::meanDeviation(uint16_t a, uint16_t b){
   int32_t diff = a - b;
-  int32_t res = 0; 
+  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];
@@ -281,8 +281,8 @@ UserThread::main()
     * read accelerometer z-value
     */
     accel_z = global.lis331dlh.getAccelerationForce(LIS331DLH::AXIS_Z);
-        
-    if (accel_z < -900 /*-0.9g*/) { 
+
+    if (accel_z < -900 /*-0.9g*/) {
       // Start line following when AMiRo is rotated
       if(currentState == states::INACTIVE){
         newState = states::FOLLOW_LINE;
@@ -344,7 +344,7 @@ UserThread::main()
     }
     // newState = currentState;
 
-    // Get sensor data 
+    // Get sensor data
     // 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++){
@@ -380,12 +380,12 @@ UserThread::main()
           global.robot.calibrate();
         }
         for(int i=0; i <= proxCalib.meanWindow; i++){
-          proxCalib.buf += global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_LEFT].getProximityScaledWoOffset() 
-                          + global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_RIGHT].getProximityScaledWoOffset(); 
+          proxCalib.buf += global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_LEFT].getProximityScaledWoOffset()
+                          + global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_RIGHT].getProximityScaledWoOffset();
           this->sleep(CAN::UPDATE_PERIOD);
         }
         proxCalib.buf = proxCalib.buf / (2*proxCalib.meanWindow);
-        
+
         if(proxCalib.calibrateBlack){
           global.linePID.BThresh = proxCalib.buf;
         }else  {
@@ -415,7 +415,7 @@ UserThread::main()
         if (global.forwardSpeed != global.rpmForward[0]){
           global.forwardSpeed = global.rpmForward[0];
         }
-        
+
         if(lf.getStrategy() != lStrategy){
           lf.setStrategy(lStrategy);
         }
@@ -436,7 +436,7 @@ UserThread::main()
 
         if(utCount.ringProxCount > RING_PROX_DETECTION_TIMEOUT){
           utCount.ringProxCount = 0;
-          
+
 
           checkForMotion();
           // Check if only front sensors are active
@@ -458,7 +458,7 @@ UserThread::main()
 
           setRpmSpeed(rpmSpeed);
         }
-        
+
       break;
       // ---------------------------------------
     case states::TURN:{
@@ -466,18 +466,22 @@ UserThread::main()
       int factor = SPEED_CONVERSION_FACTOR;
       int frontL = global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_LEFT].getProximityScaledWoOffset();
       int frontR = global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_RIGHT].getProximityScaledWoOffset();
+      int blackSensor = 0;
       if (lf.getStrategy() == LineFollowStrategy::EDGE_RIGHT) {
         factor = -factor;
+        blackSensor = frontL;
+      }else{
+        blackSensor = frontR;
       }
-      
+
       rpmSpeed[0] = factor * CHARGING_SPEED;
       rpmSpeed[1] = -factor * CHARGING_SPEED;
       setRpmSpeed(rpmSpeed);
-        
-      if ((frontL > 2* pCtrl.threshHigh ) && (frontR > 2* pCtrl.threshHigh )){
+
+      if ((blackSensor >= global.linePID.WThresh )){
         utCount.whiteCount = 1;
-      }else{
-        if(utCount.whiteCount){
+      }else {
+        if((utCount.whiteCount == 1) && (blackSensor <= global.linePID.BThresh)){
           utCount.whiteCount = 0;
           newState = states::FOLLOW_LINE;
           setRpmSpeed(stop);
@@ -487,24 +491,24 @@ UserThread::main()
     }
       // ---------------------------------------
       case states::DETECT_STATION:
-        
+
         if (global.forwardSpeed != DETECTION_SPEED){
           global.forwardSpeed = DETECTION_SPEED;
         }
         if(lf.getStrategy() != LineFollowStrategy::EDGE_RIGHT){
           lf.setStrategy(LineFollowStrategy::EDGE_RIGHT);
         }
-        
+
         lf.followLine(rpmSpeed);
         setRpmSpeed(rpmSpeed);
         // // Detect marker before docking station
         // if ((WL+WR) < PROXY_WHEEL_THRESH){
-        // Use proxy ring 
+        // Use proxy ring
         if ((rProx[3]+rProx[4]) > RING_PROX_FRONT_THRESH){
 
           setRpmSpeed(stop);
           checkForMotion();
-          // 180° Rotation 
+          // 180° Rotation
           global.distcontrol.setTargetPosition(0, ROTATION_180, ROTATION_DURATION);
           // BaseThread::sleep(8000);
           checkForMotion();
@@ -548,14 +552,14 @@ UserThread::main()
           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 
+          // 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 
+          // Case L
           utCount.stateTime = 0;
           setRpmSpeed(stop);
           devCor.RCase = false;
@@ -657,8 +661,8 @@ UserThread::main()
           global.robot.requestCharging(1);
         } else {
           if(checkPinVoltage()){
-            // Pins are under voltage -> correctly docked 
-            
+            // Pins are under voltage -> correctly docked
+
             newState = states::CHARGING;
           }else{
             utCount.errorCount++;
@@ -670,7 +674,7 @@ UserThread::main()
             if((rProx[0] >= PROX_MAX_VAL) && (rProx[7] >= PROX_MAX_VAL)){
               newState = states::RELEASE_TO_CORRECT;
             } else {
-              newState = states::RELEASE_TO_CORRECT; //states::CORRECT_POSITIONING; 
+              newState = states::RELEASE_TO_CORRECT; //states::CORRECT_POSITIONING;
             }
 
             if (utCount.errorCount > DOCKING_ERROR_THRESH){
@@ -681,7 +685,7 @@ UserThread::main()
       break;
       // ---------------------------------------
       case states::RELEASE_TO_CORRECT:
-        
+
         global.distcontrol.setTargetPosition(0, ROTATION_20, ROTATION_DURATION);
         checkForMotion();
         // move 1cm forward
@@ -716,7 +720,7 @@ UserThread::main()
           global.robot.requestCharging(0);
         }else{
           global.motorcontrol.setMotorEnable(true);
-          // TODO: Use controlled 
+          // TODO: Use controlled
           //Rotate -20° to free from magnet
           global.distcontrol.setTargetPosition(0, ROTATION_20, ROTATION_DURATION);
           checkForMotion();
@@ -805,7 +809,7 @@ UserThread::main()
           {global.stateTracker[states::CALIBRATION_CHECK] += 1;}
         else if (newState == states::DEVIATION_CORRECTION)
           {global.stateTracker[states::DEVIATION_CORRECTION] += 1;}
-        
+
         else if (newState == states::DOCKING_ERROR){global.stateTracker[16+(-states::DOCKING_ERROR)] += 1;}
         else if (newState == states::REVERSE_TIMEOUT_ERROR){global.stateTracker[16+(-states::REVERSE_TIMEOUT_ERROR)] += 1;}
         else if (newState == states::CALIBRATION_ERROR){global.stateTracker[16+(-states::CALIBRATION_ERROR)] += 1;}
@@ -821,7 +825,7 @@ UserThread::main()
       //   // chprintf((BaseSequentialStream*)&global.sercanmux1, "Transmit state %d\n", newState);
       //   global.robot.transmitState(currentState);
       //   // global.robot.setOdometry(global.odometry.getPosition());
-        
+
       // }else{
       //   utCount.stateCount++;
       // }
diff --git a/devices/DiWheelDrive/userthread.hpp b/devices/DiWheelDrive/userthread.hpp
index 52c85ac..d31b1ea 100644
--- a/devices/DiWheelDrive/userthread.hpp
+++ b/devices/DiWheelDrive/userthread.hpp
@@ -5,7 +5,7 @@
 #include <amiroosconf.h>
 #include <amiro/Color.h>
 // #include "global.hpp"
-// #include "linefollow.hpp" 
+// #include "linefollow.hpp"
 #include <cmath>
 
 
@@ -22,7 +22,7 @@
 #define DOCKING_CORRECTION_TIMEOUT 200
 #define REVERSE_DOCKING_TIMEOUT 2*DOCKING_CORRECTION_TIMEOUT
 #define REVERSE_ADJUSTMENT_TIMEOUT 200
-// #define MAX_RING_PROX_VALUE_DEVIATION 
+// #define MAX_RING_PROX_VALUE_DEVIATION
 
 // Thresh for wheel proxy sensors, when summed values fall below the state changes
 // #define PROXY_WHEEL_THRESH 18000
@@ -35,7 +35,7 @@
 // #define WHITE_COUNT_THRESH 150
 #define WHITE_DETETION_TIMEOUT 150
 // #define RING_PROX_COUNT_THRESH 1000
-#define RING_PROX_DETECTION_TIMEOUT 800
+#define RING_PROX_DETECTION_TIMEOUT 400
 // Rotation around 180 degrees in microradian
 // #define ROTATION_180 3141592
 #define ROTATION_180 3141592
@@ -121,7 +121,7 @@ class UserThread : public chibios_rt::BaseStaticThread<USER_THREAD_STACK_SIZE>
     uint32_t buf = 0;
     uint8_t meanWindow = 150;
   };
-  
+
   struct deviation_correction {
     bool RCase = true;
     int8_t pCount = 0;
@@ -132,7 +132,7 @@ class UserThread : public chibios_rt::BaseStaticThread<USER_THREAD_STACK_SIZE>
   // static const struct ut_counter emptyUtCount;
   ut_counter utCount;
   proxy_ctrl pCtrl;
-  bottom_prox_calibration proxCalib; 
+  bottom_prox_calibration proxCalib;
   deviation_correction devCor;
 
 
@@ -160,7 +160,7 @@ private:
   * Returns percentage of mean deviation between two given values.
   * It is intended to calculate the mean deviation between two proxy sensor
   * values. PROX_DEVIATION_MEAN_WINDOW determains the size of the mean window.
-  * Keep in mind that initial results are wrong. 
+  * Keep in mind that initial results are wrong.
   * */
   int32_t meanDeviation(uint16_t a, uint16_t b);
 
@@ -168,7 +168,7 @@ private:
    * Check sectors around and stop if a thresh in one sector is detected.
    */
   void preventCollision(int (&rpmSpeed)[2], uint16_t (&proxVals)[8]);
-  
+
   /**
    * Same as prevent collision but also lowers the speed when object is detected.
    */
@@ -176,7 +176,7 @@ private:
   void getProxySectorVals(uint16_t (&proxVals)[8], uint16_t (&sProx)[8]);
   void getMaxFrontSectorVal(uint16_t (&sProx)[8], int32_t &sPMax);
   void chargeAsLED();
-  
+
   /**
    * Returns true when front sensors reaching high values
    * and all others are low. This indicates that the loading station is ahead.
@@ -186,11 +186,11 @@ private:
 
   /**
    * Check if current position changes when the wheel are deactivated.
-   * 
+   *
    * When AMiRo drives towards the loading station, it stops when a specific marker is reached.
    * In order to validate that the AMiRo is correctly positioned in the loading station
    * the wheels are turned off. When the position remains the same the docking procedure
-   * was successful (return 1) otherwise a correction is needed (return 0). 
+   * was successful (return 1) otherwise a correction is needed (return 0).
    */
   int checkDockingSuccess();
 
