commit e5606def6b638600982feab777c2fff00b84d10f
Author: galberding <galberding@techfak.uni-bielefeld.de>
Date:   Sun Jun 21 14:24:03 2020 +0200

    Add state transition tracking, prevent overflow of tracking buffer

diff --git a/devices/DiWheelDrive/global.hpp b/devices/DiWheelDrive/global.hpp
index 6f17087..63000fc 100644
--- a/devices/DiWheelDrive/global.hpp
+++ b/devices/DiWheelDrive/global.hpp
@@ -169,8 +169,8 @@ public:
   int rpmHardLeft[2] = {5,20};
   int rpmSoftRight[2] = {rpmSoftLeft[1],rpmSoftLeft[0]};
   int rpmHardRight[2] = {rpmHardLeft[1],rpmHardLeft[0]};
-  // uint32_t stateTracker[24] = { 0 };
-
+  uint32_t stateTracker[24] = { 0 };
+  uint32_t stateTransitionCounter = 0;
   // Line Following thresholds set due to calibration
   // MaxDelta: 18676, FL: 4289, FR: 22965
   // Thresh FL: 5241, FR: 25528
diff --git a/devices/DiWheelDrive/main.cpp b/devices/DiWheelDrive/main.cpp
index c7238c9..408a01b 100644
--- a/devices/DiWheelDrive/main.cpp
+++ b/devices/DiWheelDrive/main.cpp
@@ -914,58 +914,58 @@ 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");
-  // uint8_t IDLE                = 0;
-  // uint8_t FOLLOW_LINE         = 1;
-  // uint8_t DETECT_STATION      = 2;
-  // uint8_t REVERSE             = 3;
-  // uint8_t PUSH_BACK           = 4;
-  // uint8_t CHECK_POSITIONING   = 5;
-  // uint8_t CHECK_VOLTAGE       = 6;
-  // uint8_t CHARGING            = 7;
-  // uint8_t RELEASE             = 8;
-  // uint8_t RELEASE_TO_CORRECT  = 9;
-  // uint8_t CORRECT_POSITIONING = 10;
-  // uint8_t TURN                = 12;
-  // uint8_t INACTIVE            = 13;
-  // uint8_t CALIBRATION         = 14;
-  // uint8_t CALIBRATION_CHECK   = 15;
-  // uint8_t DEVIATION_CORRECTION = 16;
-  // uint8_t DOCKING_ERROR       = 16+1;
-  // uint8_t REVERSE_TIMEOUT_ERROR   = 16+2;
-  // uint8_t CALIBRATION_ERROR   = 16+3;
-  // uint8_t WHITE_DETECTION_ERROR   = 16+4;
-  // uint8_t PROXY_DETECTION_ERROR   = 16+5;
-  // uint8_t NO_CHARGING_POWER_ERROR   = 16+6;
-  // uint8_t 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;
-
-  // }
+  uint8_t IDLE                = 0;
+  uint8_t FOLLOW_LINE         = 1;
+  uint8_t DETECT_STATION      = 2;
+  uint8_t REVERSE             = 3;
+  uint8_t PUSH_BACK           = 4;
+  uint8_t CHECK_POSITIONING   = 5;
+  uint8_t CHECK_VOLTAGE       = 6;
+  uint8_t CHARGING            = 7;
+  uint8_t RELEASE             = 8;
+  uint8_t RELEASE_TO_CORRECT  = 9;
+  uint8_t CORRECT_POSITIONING = 10;
+  uint8_t TURN                = 12;
+  uint8_t INACTIVE            = 13;
+  uint8_t CALIBRATION         = 14;
+  uint8_t CALIBRATION_CHECK   = 15;
+  uint8_t DEVIATION_CORRECTION = 16;
+  uint8_t DOCKING_ERROR       = 16+1;
+  uint8_t REVERSE_TIMEOUT_ERROR   = 16+2;
+  uint8_t CALIBRATION_ERROR   = 16+3;
+  uint8_t WHITE_DETECTION_ERROR   = 16+4;
+  uint8_t PROXY_DETECTION_ERROR   = 16+5;
+  uint8_t NO_CHARGING_POWER_ERROR   = 16+6;
+  uint8_t 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;
+
+  }
 }
 
 
diff --git a/devices/DiWheelDrive/userthread.cpp b/devices/DiWheelDrive/userthread.cpp
index 1912a74..0998ac3 100644
--- a/devices/DiWheelDrive/userthread.cpp
+++ b/devices/DiWheelDrive/userthread.cpp
@@ -723,6 +723,7 @@ UserThread::main()
       break;
       // ---------------------------------------
     case states::RELEASE:
+
       if (global.forwardSpeed != DETECTION_SPEED){
           global.rpmForward[0] = DETECTION_SPEED;
         }
@@ -781,7 +782,7 @@ UserThread::main()
       break;
       // ---------------------------------------
     case states::TEST_MAP_STATE:{
-      /* Test suit for amiro map */
+      // Test suit for amiro map
 
 
       setAttributes(global.testmap, 0, 1, 2, 1);
@@ -790,7 +791,7 @@ UserThread::main()
       setAttributes(global.testmap, 3, 0, 0, 0xff);
       // AmiroMap map = AmiroMap(&global);
 
-    //   // --------------------------------------------------
+      // --------------------------------------------------
 
       global.tcase = 0;
       map.initialize();
@@ -839,56 +840,73 @@ UserThread::main()
       break;
     }
 
+    // In case a new state is set:
+    // 1. Record the state transition
     if (currentState != newState){
+
+      global.stateTransitionCounter++;
+      // Clear all state transitions to prevent overflow
+      if (global.stateTransitionCounter >= 255) {
+        global.stateTransitionCounter = 0;
+        for (int i = 0; i < 24; i++) {
+          global.stateTracker[i] = 0;
+        }
+}
+      // Transmit the new state over can
       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;}
+
+      // Increase state count for specific state
+      // TODO: Improve with dictionary or other than switch case
+      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;}
     }
+
+    // Keep track of last state and set the new state for next iteration
     prevState = currentState;
     currentState = newState;
 
