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/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;
 
