commit 181f2892bf5f8277059ecc4dc4ec51b7b66b012e
Author: galberding <galberding@techfak.uni-bielefeld.de>
Date:   Tue Nov 19 16:06:25 2019 +0100

    Add final state maschine for userthread, it contains functionality for standard line following as well as the docking procedure

diff --git a/devices/DiWheelDrive/userthread.cpp b/devices/DiWheelDrive/userthread.cpp
index 5f76b90..27cccd3 100644
--- a/devices/DiWheelDrive/userthread.cpp
+++ b/devices/DiWheelDrive/userthread.cpp
@@ -1,87 +1,61 @@
 #include "userthread.hpp"
-
 #include "global.hpp"
-
 #include "linefollow2.hpp" 
 
 using namespace amiro;
 
 extern Global global;
 
-enum class states{
-  FOLLOW_LINE,
-  OCCUPY,
-  RELEASE,
-  CHARGING
-};
-
-enum msg_content{
-  STOP, 
-  START,
-  EDGE_LEFT,
-  EDGE_RIGHT,
-  FUZZY,
-  DOCKING
-};
-
-// User thread state:
-states utState = states::FOLLOW_LINE;
-
 // a buffer for the z-value of the accelerometer
 int16_t accel_z;
 bool running = false;
 
-int vcnl4020AmbientLight[4] = {0};
-int vcnl4020Proximity[4] = {0};
 
-
-// Set the speed by the array
-void setRpmSpeed(const int (&rpmSpeed)[2]) {
+/**
+ * Set speed.
+ * 
+ * @param rpmSpeed speed for left and right wheel in rounds/min
+ */
+void UserThread::setRpmSpeed(const int (&rpmSpeed)[2]) {
   global.motorcontrol.setTargetRPM(rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] * 1000000, rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] * 1000000);
 }
 
-void lightOneLed(Color color, int idx){
+void UserThread::lightOneLed(Color color, int idx){
   global.robot.setLightColor(idx, Color(color));
 }
 
-void lightAllLeds(Color color){
+void UserThread::lightAllLeds(Color color){
   int led = 0;
   for(led=0; led<8; led++){
         lightOneLed(color, led);
       }
 }
 
-/**
- * Get the wanted strategy from the global object.
- */
-LineFollowStrategy determineStrategy(){
-  switch(global.lfStrategy){
-    case 0:
-      return LineFollowStrategy::EDGE_RIGHT;
-      break;
-    case 1:
-      return LineFollowStrategy::EDGE_LEFT;
-      break;
-    case 2:
-      return LineFollowStrategy::FUZZY;
-      break;
-    case 3:
-      return LineFollowStrategy::DOCKING;
-    default:
-      break;
+void UserThread::showChargingState(){
+  uint8_t numLeds = global.robot.getPowerStatus().state_of_charge / 12;
+  Color color = Color::GREEN;
+  if (numLeds <= 2){
+    color = Color::RED;
+  }else if(numLeds <= 6){
+    color = Color::YELLOW;
+  }
+  for (int i=0; i<numLeds; i++){
+    lightOneLed(color, i);
+    this->sleep(300);
   }
-  return LineFollowStrategy::NONE;
+  this->sleep(1000);
+  lightAllLeds(Color::BLACK);
 }
 
 /**
  * Blocks as long as the position changes.
  */
-void checkForMotion(UserThread *ut){
+void UserThread::checkForMotion(){
   int motion = 1;
   int led = 0;
   types::position oldPos = global.odometry.getPosition();
   while(motion){
-    ut->sleep(500);
+    this->sleep(500);
     types::position tmp = global.odometry.getPosition();
     motion = abs(oldPos.x - tmp.x)+ abs(oldPos.y - tmp.y)+abs(oldPos.z - tmp.z);
     oldPos = tmp;
@@ -91,53 +65,40 @@ void checkForMotion(UserThread *ut){
   }
 }
 
+bool UserThread::checkPinVoltage(){
+  return global.ltc4412.isPluggedIn(); 
+}
 
-/**
- * Turns MotorControl off and checks if position remains stable.
- */
-int checkDockingSuccess(UserThread *ut){
-  // global.motorcontrol.setTargetRPM(0,0);
-  checkForMotion(ut);
-  int led = 0;
+bool UserThread::checkPinEnabled(){
+  return global.ltc4412.isEnabled();
+}
+
+int UserThread::checkDockingSuccess(){
+  // setRpmSpeed(stop);
+  checkForMotion();
   int success = 0;
   global.odometry.resetPosition();
   types::position start = global.startPos = global.odometry.getPosition();
   global.motorcontrol.toggleMotorEnable();
-  ut->sleep(1000);
-  types::position stop = global.endPos = global.odometry.getPosition();
-  global.motorcontrol.toggleMotorEnable();
+  this->sleep(1000);
+  types::position stop_ = global.endPos = global.odometry.getPosition();
+  
   // Amiro moved, docking was not successful
-  if ((start.x + stop.x) || (start.y + stop.y)){
-    for(led=0; led<8; led++){
-      global.robot.setLightColor(led, Color(Color::RED));
-    }
+  if ((start.x + stop_.x) || (start.y + stop_.y)){
+    lightAllLeds(Color::RED);
+    // Enable Motor again if docking was not successful
+    global.motorcontrol.toggleMotorEnable();
     success = 0;
   }else{
-    for(led=0; led<8; led++){
-      global.robot.setLightColor(led, Color(Color::GREEN));
-    }
+    lightAllLeds(Color::GREEN);
     success = 1;
   }
   
-  ut->sleep(500);
-  for(led=0; led<8; led++){
-          global.robot.setLightColor(led, Color(Color::BLACK));
-  }
+  this->sleep(500);
+  lightAllLeds(Color::BLACK);
   return success;
 }
 
-void correctPosition(UserThread *ut, LineFollow &lf, int steps){
-  int checkWhite = 0;
-  int rpmSpeed[2] ={0};
-  lf.setStrategy(LineFollowStrategy::EDGE_LEFT);
-  for (int correction=0; correction<steps; correction++){
-    checkWhite = lf.followLine(rpmSpeed);
-    setRpmSpeed(rpmSpeed);
-    ut->sleep(CAN::UPDATE_PERIOD);
-  }
-}
-
-
 UserThread::UserThread() :
   chibios_rt::BaseStaticThread<USER_THREAD_STACK_SIZE>()
 {
@@ -153,10 +114,15 @@ UserThread::main()
   /*
    * SETUP
    */
+  // User thread state:
+states utState = states::IDLE;
+states newState;
+
+   int whiteBuf = 0;
+   int correctionStep = 0;
   int rpmSpeed[2] = {0};
   int stop[2] = {0};
-  int proxyWheelThresh = 18000;
-  LineFollowStrategy lStrategy;
+  LineFollowStrategy lStrategy = LineFollowStrategy::FUZZY;
   for (uint8_t led = 0; led < 8; ++led) {
     global.robot.setLightColor(led, Color(Color::BLACK));
   }
@@ -167,131 +133,204 @@ UserThread::main()
    */
   while (!this->shouldTerminate())
   {
-        /*
-         * read accelerometer z-value
-         */
-        accel_z = global.lis331dlh.getAccelerationForce(LIS331DLH::AXIS_Z);
-
-        /*
-         * evaluate the accelerometer
-         */
+    /*
+    * read accelerometer z-value
+    */
+    accel_z = global.lis331dlh.getAccelerationForce(LIS331DLH::AXIS_Z);
+        
+    if (accel_z < -900 /*-0.9g*/) { 
+      // Start line following when AMiRo is rotated
+      if(utState == states::IDLE){
+        utState = states::FOLLOW_LINE;
+      }else{
+        utState = states::IDLE;
+      }
+      lightAllLeds(Color::GREEN);
+      this->sleep(1000);
+      lightAllLeds(Color::BLACK);
 
-  //Features over can: (Line Following)
-  //SetKP
-  //SetKI
-  //SetKd ?
-  //SetSleepTime
-  //SeForwardSpeed? or SetMaxSpeed
-  //DriveOnLeftLine
-  //DriveOnRightLine
-  
-  //if accel_z<-900
-    //send can event (one time)
-    // Line following is started if amiro roteted
-    if (accel_z < -900 /*-0.9g*/) { //new: (can.allowLineFollowOnTurn && accel_z < 900) || can.startLineFollow optional feature
-        // set the front LEDs to blue for one second
-        lightAllLeds(Color::GREEN);
-        this->sleep(1000);
-        lightAllLeds(Color::BLACK);
-        running = !running;
     // If message was received handle it here:
     } else if(global.msgReceived){
-
       global.msgReceived = false;
-      running = true;
+      // running = true;
       switch(global.lfStrategy){
         case msg_content::START:
+          utState = states::FOLLOW_LINE;
           break;
         case msg_content::STOP:
-          running = false;
+          utState = states::IDLE;
           break;
         case msg_content::EDGE_RIGHT:
+          // utState = states::FOLLOW_LINE;
           lStrategy = LineFollowStrategy::EDGE_RIGHT;
           break;
         case  msg_content::EDGE_LEFT:
+          // utState = states::FOLLOW_LINE;
           lStrategy = LineFollowStrategy::EDGE_LEFT;
           break;
         case msg_content::FUZZY:
+          // utState = states::FOLLOW_LINE;
           lStrategy = LineFollowStrategy::FUZZY;
           break;
-        case msg_content::DOCKING:
-          utState = states::OCCUPY;
-          lStrategy = LineFollowStrategy::EDGE_RIGHT;
+        case msg_content::DOCK:
+          utState = states::DETECT_STATION;
+          break;
+        case msg_content::UNDOCK:
+          utState = states::RELEASE;
+          break;
+        case msg_content::CHARGE:
+          utState = states::CHARGING;
           break;
         default:
+          utState = states::IDLE;
           break;
       }
-    
     }
+    newState = utState;
 
-    if(running){
-      switch(utState){
-        case states::FOLLOW_LINE:{
-          // LineFollowStrategy lStrategy = determineStrategy();
-          if(lStrategy == LineFollowStrategy::DOCKING){
-            utState = states::OCCUPY;
-            break;
-          }
+    // Get sensor data 
+    int WL = global.vcnl4020[constants::DiWheelDrive::PROX_WHEEL_LEFT].getProximityScaledWoOffset();
+    int WR = global.vcnl4020[constants::DiWheelDrive::PROX_WHEEL_RIGHT].getProximityScaledWoOffset();
+    // int FL = global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_LEFT].getProximityScaledWoOffset();
+    // int FR = global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_RIGHT].getProximityScaledWoOffset();
+    switch(utState){
+      case states::IDLE:
+        if (!global.motorcontrol.getMotorEnable()){
+            global.motorcontrol.toggleMotorEnable();
+        }
+        setRpmSpeed(stop);
+        if(/* checkPinVoltage() && */ checkPinEnabled()){
+          global.robot.requestCharging(0);
+        }
+        
+        break;
+      // ---------------------------------------
+      case states::FOLLOW_LINE:
+      // Set correct forward speed to every strategy
+        if (global.forwardSpeed != global.rpmForward[0]){
+          global.forwardSpeed = global.rpmForward[0];
+        }
+        
+        if(lf.getStrategy() != lStrategy){
           lf.setStrategy(lStrategy);
-          lf.followLine(rpmSpeed);
-          setRpmSpeed(rpmSpeed);
-          break;
         }
-        case states::OCCUPY:{
-          // Get Wheel proxy sensors
-          int WL = global.vcnl4020[constants::DiWheelDrive::PROX_WHEEL_LEFT].getProximityScaledWoOffset();
-          int WR = global.vcnl4020[constants::DiWheelDrive::PROX_WHEEL_LEFT].getProximityScaledWoOffset();
-          // Check for black
-          if ((WL+WR) < proxyWheelThresh){ /*TODO: Only check for one sensor */
-            global.motorcontrol.setTargetRPM(0,0);
 
-            // Stop when in docking state and take further actions
-            if(lf.getStrategy() == LineFollowStrategy::DOCKING){
-              if(checkDockingSuccess(this)){
-                utState = states::CHARGING;
-                break;
-              }else{
-                correctPosition(this, lf, 250);
-                lf.setStrategy(LineFollowStrategy::DOCKING);
-                // break;
-              }
-            }else{
-              checkForMotion(this);
-              // 180° Rotation 
-              global.distcontrol.setTargetPosition(0, 3141592, 10000);
-              // BaseThread::sleep(8000);
-              checkForMotion(this);
-              correctPosition(this, lf, 250);
-              // break;
-              lf.setStrategy(LineFollowStrategy::DOCKING);
-              
-            }
+        //TODO: Check if white is detected and stop threshold is reached
+        if(lf.followLine(rpmSpeed)){
+          
+          if(whiteBuf >= WHITE_COUNT_THRESH){
+            setRpmSpeed(stop);
+            newState = states::IDLE;
+          }else{
+            whiteBuf++;
           }
-
-          // Set correct following
-          // lf.setStrategy(LineFollowStrategy::EDGE_RIGHT);
-          lf.followLine(rpmSpeed);
+        }else{
+          whiteBuf = 0;
           setRpmSpeed(rpmSpeed);
-          
-          break;
         }
-        case states::RELEASE:{
-          break;
+        // lf.followLine(rpmSpeed);
+        // setRpmSpeed(rpmSpeed);
+        
+        break;
+      // ---------------------------------------
+      case states::DETECT_STATION:
+        if (global.forwardSpeed != CHARGING_SPEED){
+          global.forwardSpeed = CHARGING_SPEED;
         }
-        case states::CHARGING:{
+        if(lf.getStrategy() != LineFollowStrategy::EDGE_RIGHT){
+          lf.setStrategy(LineFollowStrategy::EDGE_RIGHT);
+        }
+
+        lf.followLine(rpmSpeed);
+        setRpmSpeed(rpmSpeed);
+        // // Detect marker before docking station
+        if ((WL+WR) < PROXY_WHEEL_THRESH){
           setRpmSpeed(stop);
-          break;
+          checkForMotion();
+          // 180° Rotation 
+          global.distcontrol.setTargetPosition(0, ROTATION_180, ROTATION_DURATION);
+          // BaseThread::sleep(8000);
+          checkForMotion();
+          newState = states::CORRECT_POSITIONING;
         }
-        default:{
-          break;
+        break;
+      // ---------------------------------------
+      case states::CORRECT_POSITIONING:
+        if(lf.getStrategy() != LineFollowStrategy::EDGE_LEFT){
+          lf.promptStrategyChange(LineFollowStrategy::EDGE_LEFT);
         }
-      }
+        lf.followLine(rpmSpeed);
+        setRpmSpeed(rpmSpeed);
 
-    }else{
-      // Stop
-        setRpmSpeed(stop);
-      }
+        correctionStep++;
+        if (correctionStep >= MAX_CORRECTION_STEPS){
+          correctionStep = 0;
+          newState = states::REVERSE;
+          setRpmSpeed(stop);
+          checkForMotion();
+        }
+        break;
+      // ---------------------------------------
+      case states::REVERSE:
+        if(lf.getStrategy() != LineFollowStrategy::REVERSE){
+          lf.setStrategy(LineFollowStrategy::REVERSE);
+        }
+        lf.followLine(rpmSpeed);
+        setRpmSpeed(rpmSpeed);
 
+        if ((WL+WR) < PROXY_WHEEL_THRESH){
+          setRpmSpeed(stop);
+          checkForMotion();
+          newState = states::CHECK_POSITIONING;
+        }
+        break;
+      // ---------------------------------------
+      case states::CHECK_POSITIONING:
+        if(checkDockingSuccess()/* && checkVoltage() */){
+          newState = states::CHARGING;
+        }else{
+          newState = states::CORRECT_POSITIONING;
+        }
+        break;
+      // ---------------------------------------
+      case states::CHARGING:
+        if (global.motorcontrol.getMotorEnable()){
+          global.motorcontrol.toggleMotorEnable();
+        }
+        // Formulate Request to enable charging
+        if(/* checkPinVoltage() && */ !checkPinEnabled()){
+          global.robot.requestCharging(1);
+        }
+        if(checkPinEnabled()){
+          showChargingState();
+        }
+        break;
+      
+      // ---------------------------------------
+      case states::RELEASE:
+        if(/* checkPinVoltage() && */ checkPinEnabled()){
+          global.robot.requestCharging(0);
+        }
+
+        if(checkPinEnabled()){
+          showChargingState();
+        }else{
+          if (!global.motorcontrol.getMotorEnable()){
+            global.motorcontrol.toggleMotorEnable();
+          }
+          //Rotate -20° to free from magnet
+          global.distcontrol.setTargetPosition(0, ROTATION_20, ROTATION_DURATION);
+          checkForMotion();
+          lStrategy = LineFollowStrategy::EDGE_RIGHT;
+          newState = states::FOLLOW_LINE;
+        }
+        lightAllLeds(Color::BLACK);
+        break;
+      
+      default:
+        break;
+      }
+      utState = newState;
     this->sleep(CAN::UPDATE_PERIOD);
   }
 
