commit d4c6efa993167c38f08e2d924b626691f97c7464
Author: galberding <galberding@techfak.uni-bielefeld.de>
Date:   Mon May 25 20:04:01 2020 +0200

    Change turn logic by utilizing the bottom sensors, add shell command infos for
    displaying all state transitions

diff --git a/devices/DiWheelDrive/global.hpp b/devices/DiWheelDrive/global.hpp
index 6a7b47f..6831a09 100644
--- a/devices/DiWheelDrive/global.hpp
+++ b/devices/DiWheelDrive/global.hpp
@@ -169,7 +169,7 @@ public:
   int rpmHardLeft[2] = {5,20};
   int rpmSoftRight[2] = {rpmSoftLeft[1],rpmSoftLeft[0]};
   int rpmHardRight[2] = {rpmHardLeft[1],rpmHardLeft[0]};
-  
+  uint32_t stateTracker[23] = { 0 };
 
   // Line Following thresholds set due to calibration
   // MaxDelta: 18676, FL: 4289, FR: 22965
diff --git a/devices/DiWheelDrive/main.cpp b/devices/DiWheelDrive/main.cpp
index bf7cacc..4ff0fd1 100644
--- a/devices/DiWheelDrive/main.cpp
+++ b/devices/DiWheelDrive/main.cpp
@@ -908,6 +908,65 @@ void shellRequestPrintCoordinate(BaseSequentialStream *chp, int argc, char *argv
   chprintf(chp, "X:%i Y:%i\n",oldPos.x, oldPos.y);
 }
 
+
+void shellRequestErrorInfo(BaseSequentialStream *chp, int argc, char *argv[]){
+  // Print out the error info collected. Clear buffer after calling
+  chprintf(chp, "Error Info\n");
+  chprintf(chp, "\n");
+  int IDLE                = 0;
+  int FOLLOW_LINE         = 1;
+  int DETECT_STATION      = 2;
+  int REVERSE             = 3;
+  int PUSH_BACK           = 4;
+  int CHECK_POSITIONING   = 5;
+  int CHECK_VOLTAGE       = 6;
+  int CHARGING            = 7;
+  int RELEASE             = 8;
+  int RELEASE_TO_CORRECT  = 9;
+  int CORRECT_POSITIONING = 10;
+  int TURN                = 12;
+  int INACTIVE            = 13;
+  int CALIBRATION         = 14;
+  int CALIBRATION_CHECK   = 15;
+  int DEVIATION_CORRECTION = 16;
+  int DOCKING_ERROR       = 16+1;
+  int REVERSE_TIMEOUT_ERROR   = 16+2;
+  int CALIBRATION_ERROR   = 16+3;
+  int WHITE_DETECTION_ERROR   = 16+4;
+  int PROXY_DETECTION_ERROR   = 16+5;
+  int NO_CHARGING_POWER_ERROR   = 16+6;
+  int UNKNOWN_STATE_ERROR   = 16+7;
+
+  chprintf(chp, "IDLE: %d\n", global.stateTracker[IDLE]);
+  chprintf(chp, "FOLLOW_LINE: %d\n", global.stateTracker[FOLLOW_LINE]);
+  chprintf(chp, "DETECT_STATION: %d\n", global.stateTracker[DETECT_STATION]);
+  chprintf(chp, "REVERSE: %d\n", global.stateTracker[REVERSE]);
+  chprintf(chp, "PUSH_BACK: %d\n", global.stateTracker[PUSH_BACK]);
+  chprintf(chp, "CHECK_POSITIONING: %d\n", global.stateTracker[CHECK_POSITIONING]);
+  chprintf(chp, "CHECK_VOLTAGE: %d\n", global.stateTracker[CHECK_VOLTAGE]);
+  chprintf(chp, "CHARGING: %d\n", global.stateTracker[CHARGING]);
+  chprintf(chp, "RELEASE: %d\n", global.stateTracker[RELEASE]);
+  chprintf(chp, "RELEASE_TO_CORRECT: %d\n", global.stateTracker[RELEASE_TO_CORRECT]);
+  chprintf(chp, "CORRECT_POSITIONING: %d\n", global.stateTracker[CORRECT_POSITIONING]);
+  chprintf(chp, "TURN: %d\n", global.stateTracker[TURN]);
+  chprintf(chp, "INACTIVE: %d\n", global.stateTracker[INACTIVE]);
+  chprintf(chp, "CALIBRATION: %d\n", global.stateTracker[CALIBRATION]);
+  chprintf(chp, "CALIBRATION_CHECK: %d\n", global.stateTracker[CALIBRATION_CHECK]);
+  chprintf(chp, "DEVIATION_CORRECTION: %d\n", global.stateTracker[DEVIATION_CORRECTION]);
+  chprintf(chp, "DOCKING_ERROR: %d\n", global.stateTracker[DOCKING_ERROR]);
+  chprintf(chp, "REVERSE_TIMEOUT_ERROR: %d\n", global.stateTracker[REVERSE_TIMEOUT_ERROR]);
+  chprintf(chp, "CALIBRATION_ERROR: %d\n", global.stateTracker[CALIBRATION_ERROR]);
+  chprintf(chp, "WHITE_DETECTION_ERROR: %d\n", global.stateTracker[WHITE_DETECTION_ERROR]);
+  chprintf(chp, "PROXY_DETECTION_ERROR: %d\n", global.stateTracker[PROXY_DETECTION_ERROR]);
+  chprintf(chp, "NO_CHARGING_POWER_ERROR: %d\n", global.stateTracker[NO_CHARGING_POWER_ERROR]);
+  chprintf(chp, "UNKNOWN_STATE_ERROR: %d\n", global.stateTracker[UNKNOWN_STATE_ERROR]);
+
+  for (int i=0; i<24;i++){
+    global.stateTracker[i] = 0;
+  }
+
+}
+
 static const ShellCommand commands[] = {
   {"shutdown", shellRequestShutdown},
   {"wakeup", shellRequestWakeup},
@@ -941,6 +1000,7 @@ static const ShellCommand commands[] = {
   {"printMagnetometerRes", shellRequestMagnetoMeterPrint},
   {"printLocation", shellRequestPrintCoordinate},
   {"checkPowerPins", shellRequestCheckPower},
+  {"infos", shellRequestErrorInfo},
   {NULL, NULL}
 };
 
diff --git a/devices/DiWheelDrive/userthread.cpp b/devices/DiWheelDrive/userthread.cpp
index ab5d51c..3a4c48e 100644
--- a/devices/DiWheelDrive/userthread.cpp
+++ b/devices/DiWheelDrive/userthread.cpp
@@ -2,6 +2,7 @@
 #include "global.hpp"
 #include <cmath>
 #include "linefollow.hpp" 
+#include "userthread.hpp"
 // #include <cmath>
 // #include "global.hpp"
 using namespace amiro;
@@ -138,8 +139,8 @@ void UserThread::preventCollision( int (&rpmSpeed)[2], uint16_t (&proxVals)[8])
   }
 
   if((proxVals[3] > pCtrl.threshMid) || (proxVals[4] > pCtrl.threshMid)){
-      rpmSpeed[0] = rpmSpeed[0] / 4;
-      rpmSpeed[1] = rpmSpeed[1] / 4;
+      rpmSpeed[0] = rpmSpeed[0] / 3;
+      rpmSpeed[1] = rpmSpeed[1] / 3;
   }
 
   if((proxVals[3] > pCtrl.threshHigh) || (proxVals[4] > pCtrl.threshHigh)){
@@ -435,7 +436,20 @@ UserThread::main()
 
         if(utCount.ringProxCount > RING_PROX_DETECTION_TIMEOUT){
           utCount.ringProxCount = 0;
-          newState = states::TURN;
+          
+
+          checkForMotion();
+          // Check if only front sensors are active
+          if (checkFrontalObject()) {
+            // global.distcontrol.setTargetPosition(0, 2792526, ROTATION_DURATION);
+            // // BaseThread::sleep(8000);
+            // checkForMotion();
+            this->utCount.whiteCount = 0;
+            newState = states::TURN;
+            // lf.promptStrategyChange(LineFollowStrategy::EDGE_LEFT);
+          } else {
+            newState = states::PROXY_DETECTION_ERROR;
+          }
         }
 
         if (lf.getStrategy() == LineFollowStrategy::FUZZY){
@@ -447,28 +461,40 @@ UserThread::main()
         
       break;
       // ---------------------------------------
-      case states::TURN:
-        checkForMotion();
-        // Check if only front sensors are active 
-        if(checkFrontalObject()){
-          global.distcontrol.setTargetPosition(0, 2792526, ROTATION_DURATION);
-          // BaseThread::sleep(8000);
-          checkForMotion();
+    case states::TURN:{
+        // Check the line strategy in order to continue driving on the right side
+      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();
+      if (lf.getStrategy() == LineFollowStrategy::EDGE_RIGHT) {
+        factor = -factor;
+      }
+      
+      rpmSpeed[0] = factor * CHARGING_SPEED;
+      rpmSpeed[1] = -factor * CHARGING_SPEED;
+      setRpmSpeed(rpmSpeed);
+        
+      if ((frontL > 2* pCtrl.threshHigh ) && (frontR > 2* pCtrl.threshHigh )){
+        utCount.whiteCount = 1;
+      }else{
+        if(utCount.whiteCount){
+          utCount.whiteCount = 0;
           newState = states::FOLLOW_LINE;
-          // lf.promptStrategyChange(LineFollowStrategy::EDGE_LEFT);
-        }else{
-          newState = states::PROXY_DETECTION_ERROR;
+          setRpmSpeed(stop);
         }
+      }
       break;
+    }
       // ---------------------------------------
       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
@@ -514,7 +540,9 @@ UserThread::main()
         // utCount.stateTime++;
 
         // 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) )){
+        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;
@@ -653,7 +681,7 @@ UserThread::main()
       break;
       // ---------------------------------------
       case states::RELEASE_TO_CORRECT:
-
+        
         global.distcontrol.setTargetPosition(0, ROTATION_20, ROTATION_DURATION);
         checkForMotion();
         // move 1cm forward
@@ -688,7 +716,7 @@ UserThread::main()
           global.robot.requestCharging(0);
         }else{
           global.motorcontrol.setMotorEnable(true);
-
+          // TODO: Use controlled 
           //Rotate -20° to free from magnet
           global.distcontrol.setTargetPosition(0, ROTATION_20, ROTATION_DURATION);
           checkForMotion();
@@ -745,6 +773,46 @@ UserThread::main()
       if (currentState != newState){
         chprintf((BaseSequentialStream*)&global.sercanmux1, "Transmit state %d\n", newState);
         global.robot.transmitState(newState);
+        if (newState == states::IDLE)
+          {global.stateTracker[states::IDLE] += 1;}
+        else if (newState == states::FOLLOW_LINE)
+          {global.stateTracker[states::FOLLOW_LINE] += 1;}
+        else if (newState == states::DETECT_STATION)
+          {global.stateTracker[states::DETECT_STATION] += 1;}
+        else if (newState == states::REVERSE)
+          {global.stateTracker[states::REVERSE] += 1;}
+        else if (newState == states::PUSH_BACK)
+          {global.stateTracker[states::PUSH_BACK] += 1;}
+        else if (newState == states::CHECK_POSITIONING)
+          {global.stateTracker[states::CHECK_POSITIONING] += 1;}
+        else if (newState == states::CHECK_VOLTAGE)
+          {global.stateTracker[states::CHECK_VOLTAGE] += 1;}
+        else if (newState == states::CHARGING)
+          {global.stateTracker[states::CHARGING] += 1;}
+        else if (newState == states::RELEASE)
+          {global.stateTracker[states::RELEASE] += 1;}
+        else if (newState == states::RELEASE_TO_CORRECT)
+          {global.stateTracker[states::RELEASE_TO_CORRECT] += 1;}
+        else if (newState == states::CORRECT_POSITIONING)
+          {global.stateTracker[states::CORRECT_POSITIONING] += 1;}
+        else if (newState == states::TURN)
+          {global.stateTracker[states::TURN] += 1;}
+        else if (newState == states::INACTIVE)
+          {global.stateTracker[states::INACTIVE] += 1;}
+        else if (newState == states::CALIBRATION)
+          {global.stateTracker[states::CALIBRATION] += 1;}
+        else if (newState == states::CALIBRATION_CHECK)
+          {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;}
+        else if (newState == states::WHITE_DETECTION_ERROR){global.stateTracker[16+(-states::WHITE_DETECTION_ERROR)] += 1;}
+        else if (newState == states::PROXY_DETECTION_ERROR){global.stateTracker[16+(-states::PROXY_DETECTION_ERROR)] += 1;}
+        else if (newState == states::NO_CHARGING_POWER_ERROR){global.stateTracker[16+(-states::NO_CHARGING_POWER_ERROR)] += 1;}
+        else if (newState == states::UNKNOWN_STATE_ERROR){global.stateTracker[16+(-states::UNKNOWN_STATE_ERROR)] += 1;}
       }
       prevState = currentState;
       currentState = newState;
diff --git a/devices/DiWheelDrive/userthread.hpp b/devices/DiWheelDrive/userthread.hpp
index 8328cd3..52c85ac 100644
--- a/devices/DiWheelDrive/userthread.hpp
+++ b/devices/DiWheelDrive/userthread.hpp
@@ -8,11 +8,15 @@
 // #include "linefollow.hpp" 
 #include <cmath>
 
+
+
+// TODO: Clean up and sort defines!
 // Speed when driving towards the docking station
 #define CHARGING_SPEED 5
 #define DETECTION_SPEED 10
 #define DIST_THRESH 100
 #define RELEASE_COUNT 200
+#define SPEED_CONVERSION_FACTOR 1000000 //Convert value to um/s
 // Thresh to determain how much update steps should pass while alining
 // #define MAX_CORRECTION_STEPS 200
 #define DOCKING_CORRECTION_TIMEOUT 200
