commit 10bf9cc0dd224ec150da9a1b69fa70d860b3fdc4
Author: galberding <galberding@techfak.uni-bielefeld.de>
Date:   Tue Feb 18 17:08:39 2020 +0100

    Introducing error states (negative lable): DOCKING_ERROR, REVERSE_TIMEOUT_ERROR, CALIBRATION_ERROR, WHITE_DETECTION_ERROR, PROXY_DETECTION_ERROR, NO_CHARGING_POWER_ERROR (not in use), UNKNOWN_STATE_ERROR.
    Bootom sensors can be calibrated for specific colors with MSG_CALIBRATE_BLACK/WHITE,
    Add new object detection system that reduces the speed when frontal objects are detected and stops at defined distance.

diff --git a/devices/DiWheelDrive/userthread.cpp b/devices/DiWheelDrive/userthread.cpp
index 52efae2..0bbe765 100644
--- a/devices/DiWheelDrive/userthread.cpp
+++ b/devices/DiWheelDrive/userthread.cpp
@@ -1,7 +1,9 @@
-#include "userthread.hpp"
+// #include "userthread.hpp"
 #include "global.hpp"
+#include <cmath>
 #include "linefollow.hpp" 
-
+// #include <cmath>
+// #include "global.hpp"
 using namespace amiro;
 
 extern Global global;
@@ -51,6 +53,106 @@ void UserThread::showChargingState(){
   lightAllLeds(Color::BLACK);
 }
 
+void UserThread::chargeAsLED(){
+  uint8_t numLeds = global.robot.getPowerStatus().state_of_charge / 12;
+  Color color = Color::GREEN;
+  if (numLeds <= 2){
+    color = Color::RED;
+  }else if(numLeds <= 6){
+    color = Color::YELLOW;
+  }
+  for (int i=0; i<numLeds; i++){
+    lightOneLed(color, i);
+    // this->sleep(300);
+  }
+  // this->sleep(1000);
+  // lightAllLeds(Color::BLACK);
+}
+
+// ----------------------------------------------------------------
+
+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");
+
+}
+
+
+void UserThread::getMaxFrontSectorVal(uint16_t (&sProx)[8], int32_t &sPMax){
+  for (int i=2; i<5; i++){
+    sPMax = (sPMax < sProx[i]) ? sProx[i] : sPMax;
+  }
+}
+
+void UserThread::proxSectorSpeedCorrection(int (&rpmSpeed)[2], uint16_t (&proxVals)[8]){
+  int i;
+  uint16_t sProx[8];
+  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);
+
+
+
+  if(sPMax > pCtrl.threshMid){
+      rpmSpeed[0] = 0;
+      rpmSpeed[1] = 0;
+      pCtrl.staticCont++;
+  }else if((speedL > 0) || (speedR > 0)){
+    pCtrl.staticCont = 0;
+    rpmSpeed[0] = speedL;
+    rpmSpeed[1] = speedR;
+  }else{
+    rpmSpeed[0] = 4000000 + (rpmSpeed[0] - global.rpmForward[0] * 1000000);
+    rpmSpeed[1] = 4000000 + (rpmSpeed[1] - global.rpmForward[0] * 1000000);
+  }
+
+  for(i=4; i<5; i++){
+    if ((proxVals[i] > pCtrl.threshMid) && (proxVals[i+1] > pCtrl.threshLow)){
+      rpmSpeed[0] = -5000000 ;
+      rpmSpeed[1] = -5000000 ;
+      // pCtrl.staticCont++;
+      break;
+    }
+  }
+  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]);
+
+
+}
+// -------------------------------------------------------------------
+
+
+void UserThread::preventCollision( int (&rpmSpeed)[2], uint16_t (&proxVals)[8]) {
+
+  if((proxVals[3] > pCtrl.threshLow) || (proxVals[4] > pCtrl.threshLow)){
+      rpmSpeed[0] = rpmSpeed[0] / 2;
+      rpmSpeed[1] = rpmSpeed[1] / 2;
+  }
+
+  if((proxVals[3] > pCtrl.threshMid) || (proxVals[4] > pCtrl.threshMid)){
+      rpmSpeed[0] = rpmSpeed[0] / 4;
+      rpmSpeed[1] = rpmSpeed[1] / 4;
+  }
+
+  if((proxVals[3] > pCtrl.threshHigh) || (proxVals[4] > pCtrl.threshHigh)){
+      rpmSpeed[0] = 0;
+      rpmSpeed[1] = 0;
+      utCount.ringProxCount++;
+  }else{
+    utCount.ringProxCount = 0;
+  }
+  
+}
+
+
 /**
  * Blocks as long as the position changes.
  */
@@ -67,10 +169,11 @@ void UserThread::checkForMotion(){
       global.robot.setLightColor(led % 8, Color(Color::BLACK));
       led++;
   }
+  lightAllLeds(Color::BLACK);
 }
 
 bool UserThread::checkFrontalObject(){
-  uint32_t thresh = 1000;
+  uint32_t thresh = pCtrl.threshMid;
   uint32_t prox;
   for(int i=0; i<8; i++){
     prox = global.robot.getProximityRingValue(i);
@@ -99,14 +202,15 @@ int UserThread::checkDockingSuccess(){
   // setRpmSpeed(stop);
   checkForMotion();
   int success = 0;
-  global.odometry.resetPosition();
+  // global.odometry.resetPosition();
   types::position start = global.startPos = global.odometry.getPosition();
   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 ((start.x + stop_.x)  || (start.y + stop_.y)){
+  if (abs(start.x - stop_.x) > 0 /* || (start.y + stop_.y) */){
     lightAllLeds(Color::RED);
     // Enable Motor again if docking was not successful
     global.motorcontrol.setMotorEnable(true);
@@ -147,23 +251,11 @@ UserThread::main()
    */
   // User thread state:
 
-
-  //  int whiteBuf = 0;
-  //  int proxyBuf = 0;
-  //  int reverseBuf = 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::EDGE_RIGHT;
   for (uint8_t led = 0; led < 8; ++led) {
     global.robot.setLightColor(led, Color(Color::BLACK));
   }
   running = false;
+  LineFollowStrategy lStrategy = LineFollowStrategy::EDGE_RIGHT;
   LineFollow lf(&global);
   /*
    * LOOP
@@ -177,7 +269,7 @@ UserThread::main()
         
     if (accel_z < -900 /*-0.9g*/) { 
       // Start line following when AMiRo is rotated
-      if(utState == states::IDLE){
+      if(currentState == states::INACTIVE){
         newState = states::FOLLOW_LINE;
       }else{
         newState = states::IDLE;
@@ -191,39 +283,51 @@ UserThread::main()
       global.msgReceived = false;
       // running = true;
       switch(global.lfStrategy){
-        case msg_content::START:
-          newState = states::FOLLOW_LINE;
-          break;
-        case msg_content::STOP:
+        case msg_content::MSG_START:
+          newState = states::CALIBRATION_CHECK;
+        break;
+        case msg_content::MSG_STOP:
           newState = states::IDLE;
-          break;
-        case msg_content::EDGE_RIGHT:
+        break;
+        case msg_content::MSG_EDGE_RIGHT:
           // newState = states::FOLLOW_LINE;
           lStrategy = LineFollowStrategy::EDGE_RIGHT;
-          break;
-        case  msg_content::EDGE_LEFT:
+        break;
+        case  msg_content::MSG_EDGE_LEFT:
           // newState = states::FOLLOW_LINE;
           lStrategy = LineFollowStrategy::EDGE_LEFT;
-          break;
-        case msg_content::FUZZY:
+        break;
+        case msg_content::MSG_FUZZY:
           // newState = states::FOLLOW_LINE;
           lStrategy = LineFollowStrategy::FUZZY;
-          break;
-        case msg_content::DOCK:
+        break;
+        case msg_content::MSG_DOCK:
           newState = states::DETECT_STATION;
-          break;
-        case msg_content::UNDOCK:
+        break;
+        case msg_content::MSG_UNDOCK:
           newState = states::RELEASE;
-          break;
-        case msg_content::CHARGE:
+        break;
+        case msg_content::MSG_CHARGE:
           newState = states::CHARGING;
-          break;
+        break;
+        case msg_content::MSG_RESET_ODOMETRY:
+          global.odometry.resetPosition();
+        break;
+        case msg_content::MSG_CALIBRATE_BLACK:
+          proxCalib.calibrateBlack = true;
+          // global.odometry.resetPosition();
+          newState = states::CALIBRATION;
+        break;
+        case msg_content::MSG_CALIBRATE_WHITE:
+          proxCalib.calibrateBlack = false;
+          newState = states::CALIBRATION;
+        break;
         default:
           newState = states::IDLE;
-          break;
+        break;
       }
     }
-    // newState = utState;
+    // newState = currentState;
 
     // Get sensor data 
     // uint16_t WL = global.vcnl4020[constants::DiWheelDrive::PROX_WHEEL_LEFT].getProximityScaledWoOffset();
@@ -233,23 +337,60 @@ UserThread::main()
     }
     // int FL = global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_LEFT].getProximityScaledWoOffset();
     // int FR = global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_RIGHT].getProximityScaledWoOffset();
-    switch(utState){
+    switch(currentState){
+      case states::INACTIVE:
+        // Dummy state to deactivate every interaction
+      break;
+      // ---------------------------------------
+      case states::CALIBRATION_CHECK:
+        // global.robot.calibrate();
+        if(global.linePID.BThresh >= global.linePID.WThresh){
+          newState = states::CALIBRATION_ERROR;
+        }else{
+          newState = states::FOLLOW_LINE;
+        }
+      break;
+      // ---------------------------------------
+      case states::CALIBRATION:
+        /* Calibrate the global thresholds for black or white.
+            This values will be used by the line follow object
+        */
+
+        proxCalib.buf = 0;
+        if(proxCalib.calibrateBlack){
+          chprintf((BaseSequentialStream*)&global.sercanmux1, "Black Calibration, Place AMiRo on black Surface!\n");
+          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(); 
+          this->sleep(CAN::UPDATE_PERIOD);
+        }
+        proxCalib.buf = proxCalib.buf / (2*proxCalib.meanWindow);
+        
+        if(proxCalib.calibrateBlack){
+          global.linePID.BThresh = proxCalib.buf;
+        }else  {
+          global.linePID.WThresh  = proxCalib.buf;
+        }
+          chprintf((BaseSequentialStream*)&global.sercanmux1, "Black: %d, White: %d!\n", global.linePID.BThresh, global.linePID.WThresh);
+
+        newState = states::IDLE;
+      break;
+      // ---------------------------------------
       case states::IDLE:
         global.motorcontrol.setMotorEnable(true);
         setRpmSpeed(stop);
         if(/* checkPinVoltage() && */ checkPinEnabled()){
           global.robot.requestCharging(0);
         }
+        // pCtrl.pFactor = 0;
+        pCtrl.staticCont = 0;
         utCount.whiteCount = 0;
-        utCount.proxyCount = 0;
+        utCount.ringProxCount = 0;
         utCount.errorCount = 0;
-        utCount.idleCount++;
-        if (utCount.idleCount > 10){
-          utCount.idleCount = 0;
-        // chprintf((BaseSequentialStream*)&global.sercanmux1, "Transmit state %d\n", newState);
-        global.robot.transmitState(utState);
-      }
-        break;
+        newState = states::INACTIVE;
+      break;
       // ---------------------------------------
       case states::FOLLOW_LINE:
       // Set correct forward speed to every strategy
@@ -263,36 +404,21 @@ UserThread::main()
 
         if(lf.followLine(rpmSpeed)){
           utCount.whiteCount++;
-          if(utCount.whiteCount >= WHITE_COUNT_THRESH){
-            rpmSpeed[0] = stop[0];
-            rpmSpeed[1] = stop[1];
-            newState = states::IDLE;
+          if(utCount.whiteCount >= WHITE_DETETION_TIMEOUT){
+            setRpmSpeed(stop);
+            utCount.whiteCount = 0;
+            newState = states::WHITE_DETECTION_ERROR;
           }
         }else{
           utCount.whiteCount = 0;
-          // setRpmSpeed(rpmSpeed);
         }
 
-        if(getProxyRingSum() > PROXY_RING_THRESH){
-          utCount.proxyCount++;
-          if(utCount.proxyCount > WHITE_COUNT_THRESH){
-            rpmSpeed[0] = stop[0];
-            rpmSpeed[1] = stop[1];
-            // newState = states::IDLE;
-            if (continue_on_obstacle){
-              newState = states::TURN;
-              utCount.proxyCount = 0;
-            }else{
-              newState = states::IDLE;
-            }
-          }
-        }else{
-            utCount.proxyCount = 0;
-        }
+        preventCollision(rpmSpeed, rProx);
+        // proxSectorSpeedCorrection(rpmSpeed, rProx);
 
-        if ((rProx[3] + rProx[4]) > RING_PROX_FRONT_THRESH){
-          rpmSpeed[0] = stop[0];
-          rpmSpeed[1] = stop[1];
+        if(utCount.ringProxCount > RING_PROX_DETECTION_TIMEOUT){
+          utCount.ringProxCount = 0;
+          newState = states::TURN;
         }
 
         if (lf.getStrategy() == LineFollowStrategy::FUZZY){
@@ -302,21 +428,21 @@ UserThread::main()
           setRpmSpeed(rpmSpeed);
         }
         
-        break;
+      break;
       // ---------------------------------------
       case states::TURN:
         checkForMotion();
         // Check if only front sensors are active 
         if(checkFrontalObject()){
-          global.distcontrol.setTargetPosition(0, ROTATION_180, ROTATION_DURATION);
+          global.distcontrol.setTargetPosition(0, 2792526, ROTATION_DURATION);
           // BaseThread::sleep(8000);
           checkForMotion();
           newState = states::FOLLOW_LINE;
-          lf.promptStrategyChange(LineFollowStrategy::EDGE_LEFT);
+          // lf.promptStrategyChange(LineFollowStrategy::EDGE_LEFT);
         }else{
-          newState = states::IDLE;
+          newState = states::PROXY_DETECTION_ERROR;
         }
-        break;
+      break;
       // ---------------------------------------
       case states::DETECT_STATION:
         if (global.forwardSpeed != DETECTION_SPEED){
@@ -341,7 +467,7 @@ UserThread::main()
           checkForMotion();
           newState = states::CORRECT_POSITIONING;
         }
-        break;
+      break;
       // ---------------------------------------
       case states::CORRECT_POSITIONING:
         if (global.forwardSpeed != CHARGING_SPEED){
@@ -353,14 +479,14 @@ UserThread::main()
         lf.followLine(rpmSpeed);
         setRpmSpeed(rpmSpeed);
 
-        utCount.stepCount++;
-        if (utCount.stepCount >= MAX_CORRECTION_STEPS){
-          utCount.stepCount = 0;
+        utCount.stateTime++;
+        if (utCount.stateTime >= DOCKING_CORRECTION_TIMEOUT){
+          utCount.stateTime = 0;
           newState = states::REVERSE;
           setRpmSpeed(stop);
           checkForMotion();
         }
-        break;
+      break;
       // ---------------------------------------
       case states::REVERSE:
         if(lf.getStrategy() != LineFollowStrategy::REVERSE){
@@ -368,24 +494,25 @@ UserThread::main()
         }
         lf.followLine(rpmSpeed);
         setRpmSpeed(rpmSpeed);
+        utCount.stateTime++;
 
-        // 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
-        // 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();
-          utCount.stepCount = 0;
+          utCount.stateTime = 0;
           newState = states::PUSH_BACK;
+        }else if (utCount.stateTime >= REVERSE_DOCKING_TIMEOUT){
+          setRpmSpeed(stop);
+          utCount.stateTime = 0;
+          utCount.errorCount++;
+          if (utCount.errorCount >= DOCKING_ERROR_THRESH){
+            newState = states::DOCKING_ERROR;
+          }else{
+            newState = states::CORRECT_POSITIONING;
+          }
         }
-        break;
+
+      break;
       // ---------------------------------------
       case states::PUSH_BACK:
         if(lf.getStrategy() != LineFollowStrategy::REVERSE){
@@ -394,30 +521,26 @@ UserThread::main()
         lf.followLine(rpmSpeed);
         setRpmSpeed(rpmSpeed);
 
-        utCount.stepCount++;
-        if (utCount.stepCount > PUSH_BACK_COUNT){
+        utCount.stateTime++;
+        if (utCount.stateTime > PUSH_BACK_TIMEOUT){
+          utCount.stateTime = 0;
           newState = states::CHECK_POSITIONING;
         }
-        break;
+      break;
       // ---------------------------------------
       case states::CHECK_POSITIONING:
         setRpmSpeed(stop);
         checkForMotion();
-        // if((rProx[0] >= PROX_MAX_VAL) && (rProx[7] >= PROX_MAX_VAL)){
         if(checkDockingSuccess()){
           newState = states::CHECK_VOLTAGE;
         }else{
           utCount.errorCount++;
           newState = states::CORRECT_POSITIONING;
-          if (utCount.errorCount > DOCKING_ERROR_THRESH){
-              newState = states::ERROR;
+          if (utCount.errorCount >= DOCKING_ERROR_THRESH){
+              newState = states::DOCKING_ERROR;
             }
         }
-        // }
-        // else{
-        //   newState = CORRECT_POSITIONING;
-        // }
-        break;
+      break;
       // ---------------------------------------
       case states::CHECK_VOLTAGE:
         if(!checkPinEnabled()){
@@ -425,7 +548,7 @@ UserThread::main()
         } else {
           if(checkPinVoltage()){
             // Pins are under voltage -> correctly docked 
-            utCount.errorCount = 0;
+            
             newState = states::CHARGING;
           }else{
             utCount.errorCount++;
@@ -441,11 +564,11 @@ UserThread::main()
             }
 
             if (utCount.errorCount > DOCKING_ERROR_THRESH){
-              newState = states::ERROR;
+              newState = states::DOCKING_ERROR;
             }
           }
         }
-        break;
+      break;
       // ---------------------------------------
       case states::RELEASE_TO_CORRECT:
 
@@ -458,19 +581,14 @@ UserThread::main()
         global.distcontrol.setTargetPosition(0, -2*ROTATION_20, ROTATION_DURATION);
         checkForMotion();
 
-        global.distcontrol.setTargetPosition(7000, 0, ROTATION_DURATION);
+        global.distcontrol.setTargetPosition(1500, 0, ROTATION_DURATION);
         checkForMotion();
         newState = states::CORRECT_POSITIONING;
-        break;
-      // ---------------------------------------
-      case states::ERROR:
-        utCount.errorCount = 0;
-        // lStrategy = LineFollowStrategy::EDGE_RIGHT;
-        newState = states::RELEASE;
-        break;
+      break;
       // ---------------------------------------
       case states::CHARGING:
         global.motorcontrol.setMotorEnable(false);
+        utCount.errorCount = 0;
         // Formulate Request to enable charging
         if(/* checkPinVoltage() && */ !checkPinEnabled()){
           global.robot.requestCharging(1);
@@ -478,8 +596,7 @@ UserThread::main()
         if(checkPinEnabled()){
           showChargingState();
         }
-        break;
-      
+      break;
       // ---------------------------------------
       case states::RELEASE:
       if (global.forwardSpeed != DETECTION_SPEED){
@@ -497,8 +614,8 @@ UserThread::main()
           global.distcontrol.setTargetPosition(5000, 0, ROTATION_DURATION);
           checkForMotion();
           // rotate back
-          global.distcontrol.setTargetPosition(0, -ROTATION_20, ROTATION_DURATION);
-          checkForMotion();
+          // global.distcontrol.setTargetPosition(0, -ROTATION_20, ROTATION_DURATION);
+          // checkForMotion();
 
           // global.distcontrol.setTargetPosition(5000, 0, ROTATION_DURATION);
           // checkForMotion();
@@ -509,16 +626,55 @@ UserThread::main()
           // setRpmSpeed(rpmSpeed);
         }
         // lightAllLeds(Color::BLACK);
-        break;
-      
+      break;
+      // ---------------------------------------
+      case states::DOCKING_ERROR:
+        newState = states::RELEASE;
+      break;
+      // ---------------------------------------
+      case states::REVERSE_TIMEOUT_ERROR:
+        newState = states::IDLE;
+      break;
+      // ---------------------------------------
+      case states::CALIBRATION_ERROR:
+        newState = states::IDLE;
+      break;
+      // ---------------------------------------
+      case states::WHITE_DETECTION_ERROR:
+        newState = states::IDLE;
+      break;
+      // ---------------------------------------
+      case states::PROXY_DETECTION_ERROR:
+        newState = states::IDLE;
+      break;
+      // ---------------------------------------
+      case states::NO_CHARGING_POWER_ERROR:
+        newState = states::IDLE;
+      break;
+      // ---------------------------------------
+      case states::UNKNOWN_STATE_ERROR:
+        newState = states::IDLE;
+      break;
+      // ---------------------------------------
       default:
-        break;
+        newState = states::UNKNOWN_STATE_ERROR;
+      break;
       }
-      if (utState != newState){
+      if (currentState != newState){
         chprintf((BaseSequentialStream*)&global.sercanmux1, "Transmit state %d\n", newState);
         global.robot.transmitState(newState);
       }
-      utState = newState;
+      prevState = currentState;
+      currentState = newState;
+      if (utCount.stateCount > CAN_TRANSMIT_STATE_THRESH){
+          utCount.stateCount = 0;
+        // chprintf((BaseSequentialStream*)&global.sercanmux1, "Transmit state %d\n", newState);
+        global.robot.transmitState(currentState);
+        // global.robot.setOdometry(global.odometry.getPosition());
+        
+      }else{
+        utCount.stateCount++;
+      }
     this->sleep(CAN::UPDATE_PERIOD);
   }
 
