commit 3c3c3bb9832660f2a7629193cc5fb294062132a0
Author: galberding <galberding@techfak.uni-bielefeld.de>
Date:   Sun Jun 28 14:23:38 2020 +0200

    Rename and move userthread states to Constants

diff --git a/devices/DiWheelDrive/userthread.cpp b/devices/DiWheelDrive/userthread.cpp
index d9ffe41..86dfada 100644
--- a/devices/DiWheelDrive/userthread.cpp
+++ b/devices/DiWheelDrive/userthread.cpp
@@ -1,4 +1,5 @@
 #include "userthread.hpp"
+#include "amiro/Constants.h"
 #include "amiro_map.hpp"
 #include "global.hpp"
 #include "linefollow.hpp"
@@ -273,6 +274,13 @@ UserThread::main()
   for (uint8_t led = 0; led < 8; ++led) {
     global.robot.setLightColor(led, Color(Color::BLACK));
   }
+
+  // State Variables
+  ut_states prevState = ut_states::UT_IDLE;
+  ut_states currentState = ut_states::UT_INACTIVE;
+  ut_states newState = ut_states::UT_INACTIVE;
+
+
   running = false;
   LineFollowStrategy lStrategy = LineFollowStrategy::EDGE_RIGHT;
   LineFollow lf(&global);
@@ -289,10 +297,10 @@ UserThread::main()
 
     if (accel_z < -900 /*-0.9g*/) {
       // Start line following when AMiRo is rotated
-      if(currentState == states::INACTIVE){
-        newState = states::FOLLOW_LINE;
+      if(currentState == ut_states::UT_INACTIVE){
+        newState = ut_states::UT_FOLLOW_LINE;
       }else{
-        newState = states::IDLE;
+        newState = ut_states::UT_IDLE;
       }
       lightAllLeds(Color::GREEN);
       this->sleep(1000);
@@ -304,31 +312,31 @@ UserThread::main()
       // running = true;
       switch(global.lfStrategy){
       case msg_content::MSG_START:
-        newState = states::CALIBRATION_CHECK;
+        newState = ut_states::UT_CALIBRATION_CHECK;
         break;
       case msg_content::MSG_STOP:
-        newState = states::IDLE;
+        newState = ut_states::UT_IDLE;
         break;
       case msg_content::MSG_EDGE_RIGHT:
-        // newState = states::FOLLOW_LINE;
+        // newState = ut_states::UT_FOLLOW_LINE;
         lStrategy = LineFollowStrategy::EDGE_RIGHT;
         break;
       case  msg_content::MSG_EDGE_LEFT:
-        // newState = states::FOLLOW_LINE;
+        // newState = ut_states::UT_FOLLOW_LINE;
         lStrategy = LineFollowStrategy::EDGE_LEFT;
         break;
       case msg_content::MSG_FUZZY:
-        // newState = states::FOLLOW_LINE;
+        // newState = ut_states::UT_FOLLOW_LINE;
         lStrategy = LineFollowStrategy::FUZZY;
         break;
       case msg_content::MSG_DOCK:
-        newState = states::DETECT_STATION;
+        newState = ut_states::UT_DETECT_STATION;
         break;
       case msg_content::MSG_UNDOCK:
-        newState = states::RELEASE;
+        newState = ut_states::UT_RELEASE;
         break;
       case msg_content::MSG_CHARGE:
-        newState = states::CHARGING;
+        newState = ut_states::UT_CHARGING;
         break;
       case msg_content::MSG_RESET_ODOMETRY:
         global.odometry.resetPosition();
@@ -336,26 +344,31 @@ UserThread::main()
       case msg_content::MSG_CALIBRATE_BLACK:
         proxCalib.calibrateBlack = true;
         // global.odometry.resetPosition();
-        newState = states::CALIBRATION;
+        newState = ut_states::UT_CALIBRATION;
         break;
       case msg_content::MSG_CALIBRATE_WHITE:
         proxCalib.calibrateBlack = false;
-        newState = states::CALIBRATION;
+        newState = ut_states::UT_CALIBRATION;
         break;
       case msg_content::MSG_TEST_MAP_STATE:
-        newState = states::TEST_MAP_STATE;
+        if (AMIRO_MAP_AUTO_TRACKING){
+          newState = ut_states::UT_TEST_MAP_AUTO_STATE;
+        }else {
+          newState = ut_states::UT_TEST_MAP_STATE;
+        }
+
         break;
 
       default:
-        newState = states::IDLE;
+        newState = ut_states::UT_IDLE;
         break;
       }
     }
     // newState = currentState;
 
     // 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();
+    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++){
       rProx[i] = global.robot.getProximityRingValue(i);
     }
@@ -365,20 +378,20 @@ UserThread::main()
     // int FL = global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_LEFT].getProximityScaledWoOffset();
     // int FR = global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_RIGHT].getProximityScaledWoOffset();
     switch(currentState){
-      case states::INACTIVE:
+      case ut_states::UT_INACTIVE:
         // Dummy state to deactivate every interaction
       break;
       // ---------------------------------------
-      case states::CALIBRATION_CHECK:
+      case ut_states::UT_CALIBRATION_CHECK:
         // global.robot.calibrate();
         if(global.linePID.BThresh >= global.linePID.WThresh){
-          newState = states::CALIBRATION_ERROR;
+          newState = ut_states::UT_CALIBRATION_ERROR;
         }else{
-          newState = states::FOLLOW_LINE;
+          newState = ut_states::UT_FOLLOW_LINE;
         }
       break;
       // ---------------------------------------
-      case states::CALIBRATION:
+      case ut_states::UT_CALIBRATION:
         /* Calibrate the global thresholds for black or white.
             This values will be used by the line follow object
         */
@@ -402,10 +415,10 @@ UserThread::main()
         }
           chprintf((BaseSequentialStream*)&global.sercanmux1, "Black: %d, White: %d!\n", global.linePID.BThresh, global.linePID.WThresh);
 
-        newState = states::IDLE;
+        newState = ut_states::UT_IDLE;
       break;
       // ---------------------------------------
-      case states::IDLE:
+      case ut_states::UT_IDLE:
         global.motorcontrol.setMotorEnable(true);
         setRpmSpeed(stop);
         if(/* checkPinVoltage() && */ checkPinEnabled()){
@@ -416,10 +429,10 @@ UserThread::main()
         utCount.whiteCount = 0;
         utCount.ringProxCount = 0;
         utCount.errorCount = 0;
-        newState = states::INACTIVE;
+        newState = ut_states::UT_INACTIVE;
       break;
       // ---------------------------------------
-      case states::FOLLOW_LINE:
+      case ut_states::UT_FOLLOW_LINE:
       // Set correct forward speed to every strategy
         if (global.forwardSpeed != global.rpmForward[0]){
           global.forwardSpeed = global.rpmForward[0];
@@ -434,7 +447,7 @@ UserThread::main()
           if(utCount.whiteCount >= WHITE_DETETION_TIMEOUT){
             setRpmSpeed(stop);
             utCount.whiteCount = 0;
-            newState = states::WHITE_DETECTION_ERROR;
+            newState = ut_states::UT_WHITE_DETECTION_ERROR;
           }
         }else{
           utCount.whiteCount = 0;
@@ -454,10 +467,10 @@ UserThread::main()
             // // BaseThread::sleep(8000);
             // checkForMotion();
             this->utCount.whiteCount = 0;
-            newState = states::TURN;
+            newState = ut_states::UT_TURN;
             // lf.promptStrategyChange(LineFollowStrategy::EDGE_LEFT);
           } else {
-            newState = states::PROXY_DETECTION_ERROR;
+            newState = ut_states::UT_PROXY_DETECTION_ERROR;
           }
         }
 
@@ -470,7 +483,7 @@ UserThread::main()
 
       break;
       // ---------------------------------------
-    case states::TURN:{
+    case ut_states::UT_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();
@@ -492,14 +505,14 @@ UserThread::main()
       }else {
         if((utCount.whiteCount == 1) && (blackSensor <= global.linePID.BThresh)){
           utCount.whiteCount = 0;
-          newState = states::FOLLOW_LINE;
+          newState = ut_states::UT_FOLLOW_LINE;
           setRpmSpeed(stop);
         }
       }
       break;
     }
       // ---------------------------------------
-    case states::DETECT_STATION:
+    case ut_states::UT_DETECT_STATION:
 
         if (global.forwardSpeed != DETECTION_SPEED){
           global.forwardSpeed = DETECTION_SPEED;
@@ -521,11 +534,11 @@ UserThread::main()
           global.distcontrol.setTargetPosition(0, ROTATION_180, ROTATION_DURATION);
           // BaseThread::sleep(8000);
           checkForMotion();
-          newState = states::CORRECT_POSITIONING;
+          newState = ut_states::UT_CORRECT_POSITIONING;
         }
       break;
       // ---------------------------------------
-    case states::CORRECT_POSITIONING:
+    case ut_states::UT_CORRECT_POSITIONING:
         if (global.forwardSpeed != CHARGING_SPEED){
           global.forwardSpeed = CHARGING_SPEED;
         }
@@ -538,13 +551,13 @@ UserThread::main()
         utCount.stateTime++;
         if (utCount.stateTime >= DOCKING_CORRECTION_TIMEOUT){
           utCount.stateTime = 0;
-          newState = states::REVERSE;
+          newState = ut_states::UT_REVERSE;
           setRpmSpeed(stop);
           checkForMotion();
         }
       break;
       // ---------------------------------------
-    case states::REVERSE:
+    case ut_states::UT_REVERSE:
         if(lf.getStrategy() != LineFollowStrategy::REVERSE){
           lf.setStrategy(LineFollowStrategy::REVERSE);
         }
@@ -559,29 +572,29 @@ UserThread::main()
           // setRpmSpeed(stop);
           // checkForMotion();
           utCount.stateTime = 0;
-          newState = states::PUSH_BACK;
+          newState = ut_states::UT_PUSH_BACK;
         }else if ((devCor.currentDeviation <= -MAX_DEVIATION_FACTOR) && ((rProx[0] > DEVIATION_DIST_THRESH) || (rProx[7] > DEVIATION_DIST_THRESH))){
           // Case R
           utCount.stateTime = 0;
           setRpmSpeed(stop);
           devCor.RCase = true;
           lightAllLeds(Color::YELLOW);
-          newState = states::DEVIATION_CORRECTION;
+          newState = ut_states::UT_DEVIATION_CORRECTION;
         }else if ((devCor.currentDeviation >= MAX_DEVIATION_FACTOR) && ((rProx[0] > DEVIATION_DIST_THRESH) || (rProx[7] > DEVIATION_DIST_THRESH))){
           // Case L
           utCount.stateTime = 0;
           setRpmSpeed(stop);
           devCor.RCase = false;
           lightAllLeds(Color::WHITE);
-          newState = states::DEVIATION_CORRECTION;
+          newState = ut_states::UT_DEVIATION_CORRECTION;
         }else if (utCount.stateTime >= REVERSE_DOCKING_TIMEOUT){
           setRpmSpeed(stop);
           utCount.stateTime = 0;
           utCount.errorCount++;
           if (utCount.errorCount >= DOCKING_ERROR_THRESH){
-            newState = states::DOCKING_ERROR;
+            newState = ut_states::UT_DOCKING_ERROR;
           }else{
-            newState = states::CORRECT_POSITIONING;
+            newState = ut_states::UT_CORRECT_POSITIONING;
           }
         }
 
@@ -593,7 +606,7 @@ UserThread::main()
         // setRpmSpeed(rpmSpeed);
       break;
       // ---------------------------------------
-    case states::DEVIATION_CORRECTION:
+    case ut_states::UT_DEVIATION_CORRECTION:
         // if(lf.getStrategy() != LineFollowStrategy::REVERSE){
         //   lf.setStrategy(LineFollowStrategy::REVERSE);
         // }
@@ -619,12 +632,12 @@ UserThread::main()
           setRpmSpeed(rpmSpeed);
           if(((devCor.currentDeviation >= -5) && (devCor.currentDeviation <= 5))){
             utCount.stateTime = 0;
-            newState = states::REVERSE;
+            newState = ut_states::UT_REVERSE;
             setRpmSpeed(stop);
           }
         }else{
           utCount.stateTime = 0;
-          newState = states::REVERSE;
+          newState = ut_states::UT_REVERSE;
           setRpmSpeed(stop);
         }
 
@@ -633,11 +646,11 @@ UserThread::main()
 
         // if (utCount.stateTime > PUSH_BACK_TIMEOUT){
         //   utCount.stateTime = 0;
-        //   newState = states::CHECK_POSITIONING;
+        //   newState = ut_states::UT_CHECK_POSITIONING;
         // }
       break;
       // ---------------------------------------
-    case states::PUSH_BACK:
+    case ut_states::UT_PUSH_BACK:
         if(lf.getStrategy() != LineFollowStrategy::REVERSE){
           lf.setStrategy(LineFollowStrategy::REVERSE);
         }
@@ -647,32 +660,32 @@ UserThread::main()
         utCount.stateTime++;
         if (utCount.stateTime > PUSH_BACK_TIMEOUT){
           utCount.stateTime = 0;
-          newState = states::CHECK_POSITIONING;
+          newState = ut_states::UT_CHECK_POSITIONING;
         }
       break;
       // ---------------------------------------
-    case states::CHECK_POSITIONING:
+    case ut_states::UT_CHECK_POSITIONING:
         setRpmSpeed(stop);
         checkForMotion();
         if(checkDockingSuccess()){
-          newState = states::CHECK_VOLTAGE;
+          newState = ut_states::UT_CHECK_VOLTAGE;
         }else{
           utCount.errorCount++;
-          newState = states::CORRECT_POSITIONING;
+          newState = ut_states::UT_CORRECT_POSITIONING;
           if (utCount.errorCount >= DOCKING_ERROR_THRESH){
-              newState = states::DOCKING_ERROR;
+              newState = ut_states::UT_DOCKING_ERROR;
             }
         }
       break;
       // ---------------------------------------
-    case states::CHECK_VOLTAGE:
+    case ut_states::UT_CHECK_VOLTAGE:
         if(!checkPinEnabled()){
           global.robot.requestCharging(1);
         } else {
           if(checkPinVoltage()){
             // Pins are under voltage -> correctly docked
 
-            newState = states::CHARGING;
+            newState = ut_states::UT_CHARGING;
           }else{
             utCount.errorCount++;
             // No voltage on pins -> falsely docked
@@ -681,19 +694,19 @@ UserThread::main()
             global.robot.requestCharging(0);
             // TODO: Soft release when docking falsely
             if((rProx[0] >= PROX_MAX_VAL) && (rProx[7] >= PROX_MAX_VAL)){
-              newState = states::RELEASE_TO_CORRECT;
+              newState = ut_states::UT_RELEASE_TO_CORRECT;
             } else {
-              newState = states::RELEASE_TO_CORRECT; //states::CORRECT_POSITIONING;
+              newState = ut_states::UT_RELEASE_TO_CORRECT; //ut_states::UT_CORRECT_POSITIONING;
             }
 
             if (utCount.errorCount > DOCKING_ERROR_THRESH){
-              newState = states::DOCKING_ERROR;
+              newState = ut_states::UT_DOCKING_ERROR;
             }
           }
         }
       break;
       // ---------------------------------------
-    case states::RELEASE_TO_CORRECT:
+    case ut_states::UT_RELEASE_TO_CORRECT:
 
         global.distcontrol.setTargetPosition(0, ROTATION_20, ROTATION_DURATION);
         checkForMotion();
@@ -706,10 +719,10 @@ UserThread::main()
 
         global.distcontrol.setTargetPosition(1500, 0, ROTATION_DURATION);
         checkForMotion();
-        newState = states::CORRECT_POSITIONING;
+        newState = ut_states::UT_CORRECT_POSITIONING;
       break;
       // ---------------------------------------
-    case states::CHARGING:
+    case ut_states::UT_CHARGING:
         global.motorcontrol.setMotorEnable(false);
         utCount.errorCount = 0;
         // Formulate Request to enable charging
@@ -721,7 +734,7 @@ UserThread::main()
         }
       break;
       // ---------------------------------------
-    case states::RELEASE:
+    case ut_states::UT_RELEASE:
 
       if (global.forwardSpeed != DETECTION_SPEED){
           global.rpmForward[0] = DETECTION_SPEED;
@@ -744,7 +757,7 @@ UserThread::main()
           // global.distcontrol.setTargetPosition(5000, 0, ROTATION_DURATION);
           // checkForMotion();
           lStrategy = LineFollowStrategy::EDGE_RIGHT;
-          newState = states::FOLLOW_LINE;
+          newState = ut_states::UT_FOLLOW_LINE;
             // whiteBuf = -100;
           // lf.followLine(rpmSpeed);
           // setRpmSpeed(rpmSpeed);
@@ -752,35 +765,48 @@ UserThread::main()
         // lightAllLeds(Color::BLACK);
       break;
       // ---------------------------------------
-    case states::DOCKING_ERROR:
-      newState = states::RELEASE;
+    case ut_states::UT_DOCKING_ERROR:
+      newState = ut_states::UT_RELEASE;
       break;
       // ---------------------------------------
-    case states::REVERSE_TIMEOUT_ERROR:
-      newState = states::IDLE;
+    case ut_states::UT_REVERSE_TIMEOUT_ERROR:
+      newState = ut_states::UT_IDLE;
       break;
       // ---------------------------------------
-    case states::CALIBRATION_ERROR:
-      newState = states::IDLE;
+    case ut_states::UT_CALIBRATION_ERROR:
+      newState = ut_states::UT_IDLE;
       break;
       // ---------------------------------------
-    case states::WHITE_DETECTION_ERROR:
-      newState = states::IDLE;
+    case ut_states::UT_WHITE_DETECTION_ERROR:
+      newState = ut_states::UT_IDLE;
       break;
       // ---------------------------------------
-    case states::PROXY_DETECTION_ERROR:
-      newState = states::IDLE;
+    case ut_states::UT_PROXY_DETECTION_ERROR:
+      newState = ut_states::UT_IDLE;
       break;
       // ---------------------------------------
-    case states::NO_CHARGING_POWER_ERROR:
-      newState = states::IDLE;
+    case ut_states::UT_NO_CHARGING_POWER_ERROR:
+      newState = ut_states::UT_IDLE;
       break;
       // ---------------------------------------
-    case states::UNKNOWN_STATE_ERROR:
-        newState = states::IDLE;
+    case ut_states::UT_UNKNOWN_STATE_ERROR:
+        newState = ut_states::UT_IDLE;
       break;
       // ---------------------------------------
-    case states::TEST_MAP_STATE:{
+    case ut_states::UT_TEST_MAP_AUTO_STATE:
+      chprintf((BaseSequentialStream *)&global.sercanmux1,
+               "Start autotracking: \n");
+
+      chprintf((BaseSequentialStream *)&global.sercanmux1,
+               "NodeCount: %d\n", map.getNodeCount());
+      for (int i=0; i<map.getNodeCount(); i++) {
+        chprintf((BaseSequentialStream *)&global.sercanmux1,
+                 "ID: %d, l: %d, r: %d \n", map.getNodeList()[i].id,
+                 map.getNodeList()[i].left, map.getNodeList()[i].right);
+      }
+      newState = UT_IDLE;
+      break;
+    case ut_states::UT_TEST_MAP_STATE:{
       // Test suit for amiro map
 
 
@@ -942,17 +968,21 @@ UserThread::main()
                "Total: %d, Passed: %d, Failed: %d\n", global.tcase + 1, passed,
                failed);
 
-      newState = states::IDLE;
+      newState = ut_states::UT_IDLE;
       break;
     }
       // --------------------------------------------------
     default:
-      newState = states::UNKNOWN_STATE_ERROR;
+      newState = ut_states::UT_UNKNOWN_STATE_ERROR;
       break;
     }
 
     // In case a new state is set:
     // 1. Record the state transition
+    if (AMIRO_MAP_AUTO_TRACKING) {
+      // map.trackUpdate(WL, WR, lStrategy, currentState);
+    }
+
     if (currentState != newState){
 
       global.stateTransitionCounter++;
@@ -969,52 +999,52 @@ UserThread::main()
 
       // 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;}
+      if (newState == ut_states::UT_IDLE)
+        {global.stateTracker[ut_states::UT_IDLE] += 1;}
+      else if (newState == ut_states::UT_FOLLOW_LINE)
+        {global.stateTracker[ut_states::UT_FOLLOW_LINE] += 1;}
+      else if (newState == ut_states::UT_DETECT_STATION)
+        {global.stateTracker[ut_states::UT_DETECT_STATION] += 1;}
+      else if (newState == ut_states::UT_REVERSE)
+        {global.stateTracker[ut_states::UT_REVERSE] += 1;}
+      else if (newState == ut_states::UT_PUSH_BACK)
+        {global.stateTracker[ut_states::UT_PUSH_BACK] += 1;}
+      else if (newState == ut_states::UT_CHECK_POSITIONING)
+        {global.stateTracker[ut_states::UT_CHECK_POSITIONING] += 1;}
+      else if (newState == ut_states::UT_CHECK_VOLTAGE)
+        {global.stateTracker[ut_states::UT_CHECK_VOLTAGE] += 1;}
+      else if (newState == ut_states::UT_CHARGING)
+        {global.stateTracker[ut_states::UT_CHARGING] += 1;}
+      else if (newState == ut_states::UT_RELEASE)
+        {global.stateTracker[ut_states::UT_RELEASE] += 1;}
+      else if (newState == ut_states::UT_RELEASE_TO_CORRECT)
+        {global.stateTracker[ut_states::UT_RELEASE_TO_CORRECT] += 1;}
+      else if (newState == ut_states::UT_CORRECT_POSITIONING)
+        {global.stateTracker[ut_states::UT_CORRECT_POSITIONING] += 1;}
+      else if (newState == ut_states::UT_TURN)
+        {global.stateTracker[ut_states::UT_TURN] += 1;}
+      else if (newState == ut_states::UT_INACTIVE)
+        {global.stateTracker[ut_states::UT_INACTIVE] += 1;}
+      else if (newState == ut_states::UT_CALIBRATION)
+        {global.stateTracker[ut_states::UT_CALIBRATION] += 1;}
+      else if (newState == ut_states::UT_CALIBRATION_CHECK)
+        {global.stateTracker[ut_states::UT_CALIBRATION_CHECK] += 1;}
+      else if (newState == ut_states::UT_DEVIATION_CORRECTION)
+        {global.stateTracker[ut_states::UT_DEVIATION_CORRECTION] += 1;}
+      else if (newState == ut_states::UT_DOCKING_ERROR)
+        {global.stateTracker[16+(-ut_states::UT_DOCKING_ERROR)] += 1;}
+      else if (newState == ut_states::UT_REVERSE_TIMEOUT_ERROR)
+        {global.stateTracker[16+(-ut_states::UT_REVERSE_TIMEOUT_ERROR)] += 1;}
+      else if (newState == ut_states::UT_CALIBRATION_ERROR)
+        {global.stateTracker[16+(-ut_states::UT_CALIBRATION_ERROR)] += 1;}
+      else if (newState == ut_states::UT_WHITE_DETECTION_ERROR)
+        {global.stateTracker[16+(-ut_states::UT_WHITE_DETECTION_ERROR)] += 1;}
+      else if (newState == ut_states::UT_PROXY_DETECTION_ERROR)
+        {global.stateTracker[16+(-ut_states::UT_PROXY_DETECTION_ERROR)] += 1;}
+      else if (newState == ut_states::UT_NO_CHARGING_POWER_ERROR)
+        {global.stateTracker[16+(-ut_states::UT_NO_CHARGING_POWER_ERROR)] += 1;}
+      else if (newState == ut_states::UT_UNKNOWN_STATE_ERROR)
+        {global.stateTracker[16+(-ut_states::UT_UNKNOWN_STATE_ERROR)] += 1;}
     }
 
     // Keep track of last state and set the new state for next iteration
