commit e404e6c07cc5931bd4eec21cc7123e179ddeafff
Author: galberding <galberding@techfak.uni-bielefeld.de>
Date:   Sun Jun 21 13:34:10 2020 +0200

    Stop tracking state transitions, move test suit from main to userthread

diff --git a/devices/DiWheelDrive/userthread.cpp b/devices/DiWheelDrive/userthread.cpp
index a6b1cc5..1912a74 100644
--- a/devices/DiWheelDrive/userthread.cpp
+++ b/devices/DiWheelDrive/userthread.cpp
@@ -1,10 +1,9 @@
-// #include "userthread.hpp"
 #include "global.hpp"
 #include <cmath>
 #include "linefollow.hpp"
 #include "userthread.hpp"
-// #include <cmath>
-// #include "global.hpp"
+#include "amiro_map.hpp"
+
 using namespace amiro;
 
 extern Global global;
@@ -248,6 +247,12 @@ int32_t UserThread::meanDeviation(uint16_t a, uint16_t b){
   return devCor.currentDeviation;
 }
 
+void setAttributes(uint8_t (&map)[MAX_NODES][NODE_ATTRIBUTES],
+                          uint8_t id, uint8_t l, uint8_t r, uint8_t att) {
+  map[id][0] = l;
+  map[id][1] = r;
+  map[id][2] = att;
+}
 
 UserThread::UserThread() :
   chibios_rt::BaseStaticThread<USER_THREAD_STACK_SIZE>()
@@ -272,6 +277,7 @@ UserThread::main()
   running = false;
   LineFollowStrategy lStrategy = LineFollowStrategy::EDGE_RIGHT;
   LineFollow lf(&global);
+  AmiroMap map(&global);
   /*
    * LOOP
    */
@@ -298,47 +304,51 @@ UserThread::main()
       global.msgReceived = false;
       // running = true;
       switch(global.lfStrategy){
-        case msg_content::MSG_START:
-          newState = states::CALIBRATION_CHECK;
+      case msg_content::MSG_START:
+        newState = states::CALIBRATION_CHECK;
         break;
-        case msg_content::MSG_STOP:
-          newState = states::IDLE;
+      case msg_content::MSG_STOP:
+        newState = states::IDLE;
         break;
-        case msg_content::MSG_EDGE_RIGHT:
-          // newState = states::FOLLOW_LINE;
-          lStrategy = LineFollowStrategy::EDGE_RIGHT;
+      case msg_content::MSG_EDGE_RIGHT:
+        // newState = states::FOLLOW_LINE;
+        lStrategy = LineFollowStrategy::EDGE_RIGHT;
         break;
-        case  msg_content::MSG_EDGE_LEFT:
-          // newState = states::FOLLOW_LINE;
-          lStrategy = LineFollowStrategy::EDGE_LEFT;
+      case  msg_content::MSG_EDGE_LEFT:
+        // newState = states::FOLLOW_LINE;
+        lStrategy = LineFollowStrategy::EDGE_LEFT;
         break;
-        case msg_content::MSG_FUZZY:
-          // newState = states::FOLLOW_LINE;
-          lStrategy = LineFollowStrategy::FUZZY;
+      case msg_content::MSG_FUZZY:
+        // newState = states::FOLLOW_LINE;
+        lStrategy = LineFollowStrategy::FUZZY;
         break;
-        case msg_content::MSG_DOCK:
-          newState = states::DETECT_STATION;
+      case msg_content::MSG_DOCK:
+        newState = states::DETECT_STATION;
         break;
-        case msg_content::MSG_UNDOCK:
-          newState = states::RELEASE;
+      case msg_content::MSG_UNDOCK:
+        newState = states::RELEASE;
+        break;
+      case msg_content::MSG_CHARGE:
+        newState = states::CHARGING;
         break;
-        case msg_content::MSG_CHARGE:
-          newState = states::CHARGING;
+      case msg_content::MSG_RESET_ODOMETRY:
+        global.odometry.resetPosition();
         break;
-        case msg_content::MSG_RESET_ODOMETRY:
-          global.odometry.resetPosition();
+      case msg_content::MSG_CALIBRATE_BLACK:
+        proxCalib.calibrateBlack = true;
+        // global.odometry.resetPosition();
+        newState = states::CALIBRATION;
         break;
-        case msg_content::MSG_CALIBRATE_BLACK:
-          proxCalib.calibrateBlack = true;
-          // global.odometry.resetPosition();
-          newState = states::CALIBRATION;
+      case msg_content::MSG_CALIBRATE_WHITE:
+        proxCalib.calibrateBlack = false;
+        newState = states::CALIBRATION;
         break;
-        case msg_content::MSG_CALIBRATE_WHITE:
-          proxCalib.calibrateBlack = false;
-          newState = states::CALIBRATION;
+      case msg_content::MSG_TEST_MAP_STATE:
+        newState = states::TEST_MAP_STATE;
         break;
-        default:
-          newState = states::IDLE;
+
+      default:
+        newState = states::IDLE;
         break;
       }
     }
@@ -490,7 +500,7 @@ UserThread::main()
       break;
     }
       // ---------------------------------------
-      case states::DETECT_STATION:
+    case states::DETECT_STATION:
 
         if (global.forwardSpeed != DETECTION_SPEED){
           global.forwardSpeed = DETECTION_SPEED;
@@ -516,7 +526,7 @@ UserThread::main()
         }
       break;
       // ---------------------------------------
-      case states::CORRECT_POSITIONING:
+    case states::CORRECT_POSITIONING:
         if (global.forwardSpeed != CHARGING_SPEED){
           global.forwardSpeed = CHARGING_SPEED;
         }
@@ -535,7 +545,7 @@ UserThread::main()
         }
       break;
       // ---------------------------------------
-      case states::REVERSE:
+    case states::REVERSE:
         if(lf.getStrategy() != LineFollowStrategy::REVERSE){
           lf.setStrategy(LineFollowStrategy::REVERSE);
         }
@@ -584,7 +594,7 @@ UserThread::main()
         // setRpmSpeed(rpmSpeed);
       break;
       // ---------------------------------------
-      case states::DEVIATION_CORRECTION:
+    case states::DEVIATION_CORRECTION:
         // if(lf.getStrategy() != LineFollowStrategy::REVERSE){
         //   lf.setStrategy(LineFollowStrategy::REVERSE);
         // }
@@ -628,7 +638,7 @@ UserThread::main()
         // }
       break;
       // ---------------------------------------
-      case states::PUSH_BACK:
+    case states::PUSH_BACK:
         if(lf.getStrategy() != LineFollowStrategy::REVERSE){
           lf.setStrategy(LineFollowStrategy::REVERSE);
         }
@@ -642,7 +652,7 @@ UserThread::main()
         }
       break;
       // ---------------------------------------
-      case states::CHECK_POSITIONING:
+    case states::CHECK_POSITIONING:
         setRpmSpeed(stop);
         checkForMotion();
         if(checkDockingSuccess()){
@@ -656,7 +666,7 @@ UserThread::main()
         }
       break;
       // ---------------------------------------
-      case states::CHECK_VOLTAGE:
+    case states::CHECK_VOLTAGE:
         if(!checkPinEnabled()){
           global.robot.requestCharging(1);
         } else {
@@ -684,7 +694,7 @@ UserThread::main()
         }
       break;
       // ---------------------------------------
-      case states::RELEASE_TO_CORRECT:
+    case states::RELEASE_TO_CORRECT:
 
         global.distcontrol.setTargetPosition(0, ROTATION_20, ROTATION_DURATION);
         checkForMotion();
@@ -700,7 +710,7 @@ UserThread::main()
         newState = states::CORRECT_POSITIONING;
       break;
       // ---------------------------------------
-      case states::CHARGING:
+    case states::CHARGING:
         global.motorcontrol.setMotorEnable(false);
         utCount.errorCount = 0;
         // Formulate Request to enable charging
@@ -712,7 +722,7 @@ UserThread::main()
         }
       break;
       // ---------------------------------------
-      case states::RELEASE:
+    case states::RELEASE:
       if (global.forwardSpeed != DETECTION_SPEED){
           global.rpmForward[0] = DETECTION_SPEED;
         }
@@ -742,93 +752,146 @@ UserThread::main()
         // lightAllLeds(Color::BLACK);
       break;
       // ---------------------------------------
-      case states::DOCKING_ERROR:
-        newState = states::RELEASE;
+    case states::DOCKING_ERROR:
+      newState = states::RELEASE;
       break;
       // ---------------------------------------
-      case states::REVERSE_TIMEOUT_ERROR:
-        newState = states::IDLE;
+    case states::REVERSE_TIMEOUT_ERROR:
+      newState = states::IDLE;
       break;
       // ---------------------------------------
-      case states::CALIBRATION_ERROR:
-        newState = states::IDLE;
+    case states::CALIBRATION_ERROR:
+      newState = states::IDLE;
       break;
       // ---------------------------------------
-      case states::WHITE_DETECTION_ERROR:
-        newState = states::IDLE;
+    case states::WHITE_DETECTION_ERROR:
+      newState = states::IDLE;
       break;
       // ---------------------------------------
-      case states::PROXY_DETECTION_ERROR:
-        newState = states::IDLE;
+    case states::PROXY_DETECTION_ERROR:
+      newState = states::IDLE;
       break;
       // ---------------------------------------
-      case states::NO_CHARGING_POWER_ERROR:
-        newState = states::IDLE;
+    case states::NO_CHARGING_POWER_ERROR:
+      newState = states::IDLE;
       break;
       // ---------------------------------------
-      case states::UNKNOWN_STATE_ERROR:
+    case states::UNKNOWN_STATE_ERROR:
         newState = states::IDLE;
       break;
       // ---------------------------------------
-      default:
-        newState = states::UNKNOWN_STATE_ERROR;
-      break;
-      }
-      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;}
+    case states::TEST_MAP_STATE:{
+      /* Test suit for amiro map */
+
+
+      setAttributes(global.testmap, 0, 1, 2, 1);
+      setAttributes(global.testmap, 1, 2, 2, 0);
+      setAttributes(global.testmap, 2, 1, 0, 0);
+      setAttributes(global.testmap, 3, 0, 0, 0xff);
+      // AmiroMap map = AmiroMap(&global);
+
+    //   // --------------------------------------------------
+
+      global.tcase = 0;
+      map.initialize();
+      global.testres[global.tcase] = map.get_state()->valid;
+
+        global.tcase++; // 1
+      setAttributes(global.testmap, 0, 1, 2, 0xff);
+      map.initialize();
+      global.testres[global.tcase] = !map.get_state()->valid;
+
+      global.tcase++; // 2
+      setAttributes(global.testmap, 0, 1, 2, 0);
+      setAttributes(global.testmap, 2, 1, 0, 1);
+      map.initialize();
+      global.testres[global.tcase] = map.get_state()->current == 2;
+
+      global.tcase++; // 3
+      setAttributes(global.testmap, 3, 0, 0, 0);
+      setAttributes(global.testmap, 4, 0, 0, 0xff);
+      map.initialize();
+      global.testres[global.tcase] = !map.get_state()->valid;
+
+      int failed = 0;
+      int passed = 0;
+      for (int i = 0; i <= global.tcase; i++) {
+        if (global.testres[i]) {
+          passed++;
+          chprintf((BaseSequentialStream *)&global.sercanmux1,
+                   "Test %d Passed!\n", i);
+        } else {
+          failed++;
+          chprintf((BaseSequentialStream *)&global.sercanmux1,
+                   "Test %d Failed\n", i);
+        }
       }
-      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++;
-      // }
+      chprintf((BaseSequentialStream *)&global.sercanmux1,
+               "Total: %d, Passed: %d, Failed: %d\n", global.tcase + 1, passed,
+               failed);
+
+      newState = states::IDLE;
+      break;
+    }
+      // --------------------------------------------------
+    default:
+      newState = states::UNKNOWN_STATE_ERROR;
+      break;
+    }
+
+    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;
+
     this->sleep(CAN::UPDATE_PERIOD);
   }
 
