commit fbcb25cc0d7006b83cecc8c2520f92ce908af3f0
Author: galberding <galberding@techfak.uni-bielefeld.de>
Date:   Tue Jan 7 10:58:42 2020 +0100

    Add check for frontal object, rotate if frontal object is detected

diff --git a/devices/DiWheelDrive/userthread.cpp b/devices/DiWheelDrive/userthread.cpp
index b505c05..52efae2 100644
--- a/devices/DiWheelDrive/userthread.cpp
+++ b/devices/DiWheelDrive/userthread.cpp
@@ -69,6 +69,24 @@ void UserThread::checkForMotion(){
   }
 }
 
+bool UserThread::checkFrontalObject(){
+  uint32_t thresh = 1000;
+  uint32_t prox;
+  for(int i=0; i<8; i++){
+    prox = global.robot.getProximityRingValue(i);
+    if((i == 3) || (i == 4)){
+      if(prox < thresh){
+        return false;
+      }
+    }else{
+      if(prox > thresh){
+        return false;
+      }
+    }
+  }
+  return true;
+}
+
 bool UserThread::checkPinVoltage(){
   return global.ltc4412.isPluggedIn(); 
 }
@@ -128,11 +146,10 @@ UserThread::main()
    * SETUP
    */
   // User thread state:
-states utState = states::IDLE;
-states newState;
 
-   int whiteBuf = 0;
-   int proxyBuf = 0;
+
+  //  int whiteBuf = 0;
+  //  int proxyBuf = 0;
   //  int reverseBuf = 0;
    int correctionStep = 0;
   //  int dist_count = 0;
@@ -161,9 +178,9 @@ states newState;
     if (accel_z < -900 /*-0.9g*/) { 
       // Start line following when AMiRo is rotated
       if(utState == states::IDLE){
-        utState = states::FOLLOW_LINE;
+        newState = states::FOLLOW_LINE;
       }else{
-        utState = states::IDLE;
+        newState = states::IDLE;
       }
       lightAllLeds(Color::GREEN);
       this->sleep(1000);
@@ -175,42 +192,42 @@ states newState;
       // running = true;
       switch(global.lfStrategy){
         case msg_content::START:
-          utState = states::FOLLOW_LINE;
+          newState = states::FOLLOW_LINE;
           break;
         case msg_content::STOP:
-          utState = states::IDLE;
+          newState = states::IDLE;
           break;
         case msg_content::EDGE_RIGHT:
-          // utState = states::FOLLOW_LINE;
+          // newState = states::FOLLOW_LINE;
           lStrategy = LineFollowStrategy::EDGE_RIGHT;
           break;
         case  msg_content::EDGE_LEFT:
-          // utState = states::FOLLOW_LINE;
+          // newState = states::FOLLOW_LINE;
           lStrategy = LineFollowStrategy::EDGE_LEFT;
           break;
         case msg_content::FUZZY:
-          // utState = states::FOLLOW_LINE;
+          // newState = states::FOLLOW_LINE;
           lStrategy = LineFollowStrategy::FUZZY;
           break;
         case msg_content::DOCK:
-          utState = states::DETECT_STATION;
+          newState = states::DETECT_STATION;
           break;
         case msg_content::UNDOCK:
-          utState = states::RELEASE;
+          newState = states::RELEASE;
           break;
         case msg_content::CHARGE:
-          utState = states::CHARGING;
+          newState = states::CHARGING;
           break;
         default:
-          utState = states::IDLE;
+          newState = states::IDLE;
           break;
       }
     }
-    newState = utState;
+    // newState = utState;
 
     // 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);
     }
@@ -223,9 +240,15 @@ states newState;
         if(/* checkPinVoltage() && */ checkPinEnabled()){
           global.robot.requestCharging(0);
         }
-        whiteBuf = 0;
-        proxyBuf = 0;
+        utCount.whiteCount = 0;
+        utCount.proxyCount = 0;
         utCount.errorCount = 0;
+        utCount.idleCount++;
+        if (utCount.idleCount > 10){
+          utCount.idleCount = 0;
+        // chprintf((BaseSequentialStream*)&global.sercanmux1, "Transmit state %d\n", newState);
+        global.robot.transmitState(utState);
+      }
         break;
       // ---------------------------------------
       case states::FOLLOW_LINE:
@@ -238,28 +261,33 @@ states newState;
           lf.setStrategy(lStrategy);
         }
 
-        //TODO: Check if white is detected and stop threshold is reached
         if(lf.followLine(rpmSpeed)){
-          whiteBuf++;
-          if(whiteBuf >= WHITE_COUNT_THRESH){
+          utCount.whiteCount++;
+          if(utCount.whiteCount >= WHITE_COUNT_THRESH){
             rpmSpeed[0] = stop[0];
             rpmSpeed[1] = stop[1];
             newState = states::IDLE;
           }
         }else{
-          whiteBuf = 0;
+          utCount.whiteCount = 0;
           // setRpmSpeed(rpmSpeed);
         }
 
         if(getProxyRingSum() > PROXY_RING_THRESH){
-          proxyBuf++;
-          if(proxyBuf > WHITE_COUNT_THRESH){
-            newState = states::IDLE;
+          utCount.proxyCount++;
+          if(utCount.proxyCount > WHITE_COUNT_THRESH){
             rpmSpeed[0] = stop[0];
             rpmSpeed[1] = stop[1];
+            // newState = states::IDLE;
+            if (continue_on_obstacle){
+              newState = states::TURN;
+              utCount.proxyCount = 0;
+            }else{
+              newState = states::IDLE;
+            }
           }
         }else{
-            proxyBuf = 0;
+            utCount.proxyCount = 0;
         }
 
         if ((rProx[3] + rProx[4]) > RING_PROX_FRONT_THRESH){
@@ -267,18 +295,6 @@ states newState;
           rpmSpeed[1] = stop[1];
         }
 
-        // if (needDistance){
-        //   whiteBuf = 0;
-        //   proxyBuf = 0;
-        //   dist_count++;
-        //   if(dist_count > DIST_THRESH){
-        //     dist_count = 0;
-        //     needDistance = false;
-        //   }
-        // }
-        // whiteBuf = 0;
-        // proxyBuf = 0;
-        // lf.followLine(rpmSpeed);
         if (lf.getStrategy() == LineFollowStrategy::FUZZY){
           setRpmSpeedFuzzy(rpmSpeed);
         }else{
@@ -288,6 +304,20 @@ states newState;
         
         break;
       // ---------------------------------------
+      case states::TURN:
+        checkForMotion();
+        // Check if only front sensors are active 
+        if(checkFrontalObject()){
+          global.distcontrol.setTargetPosition(0, ROTATION_180, ROTATION_DURATION);
+          // BaseThread::sleep(8000);
+          checkForMotion();
+          newState = states::FOLLOW_LINE;
+          lf.promptStrategyChange(LineFollowStrategy::EDGE_LEFT);
+        }else{
+          newState = states::IDLE;
+        }
+        break;
+      // ---------------------------------------
       case states::DETECT_STATION:
         if (global.forwardSpeed != DETECTION_SPEED){
           global.forwardSpeed = DETECTION_SPEED;
@@ -323,9 +353,9 @@ states newState;
         lf.followLine(rpmSpeed);
         setRpmSpeed(rpmSpeed);
 
-        correctionStep++;
-        if (correctionStep >= MAX_CORRECTION_STEPS){
-          correctionStep = 0;
+        utCount.stepCount++;
+        if (utCount.stepCount >= MAX_CORRECTION_STEPS){
+          utCount.stepCount = 0;
           newState = states::REVERSE;
           setRpmSpeed(stop);
           checkForMotion();
@@ -352,7 +382,7 @@ states newState;
         if((rProx[0] >= PROX_MAX_VAL) || (rProx[7] >= PROX_MAX_VAL)){
           // setRpmSpeed(stop);
           // checkForMotion();
-          utCount.reverseCount = 0;
+          utCount.stepCount = 0;
           newState = states::PUSH_BACK;
         }
         break;
@@ -364,8 +394,8 @@ states newState;
         lf.followLine(rpmSpeed);
         setRpmSpeed(rpmSpeed);
 
-        utCount.reverseCount++;
-        if (utCount.reverseCount > PUSH_BACK_COUNT){
+        utCount.stepCount++;
+        if (utCount.stepCount > PUSH_BACK_COUNT){
           newState = states::CHECK_POSITIONING;
         }
         break;
@@ -435,8 +465,8 @@ states newState;
       // ---------------------------------------
       case states::ERROR:
         utCount.errorCount = 0;
-        lStrategy = LineFollowStrategy::EDGE_RIGHT;
-        newState = states::FOLLOW_LINE;
+        // lStrategy = LineFollowStrategy::EDGE_RIGHT;
+        newState = states::RELEASE;
         break;
       // ---------------------------------------
       case states::CHARGING:
@@ -485,6 +515,7 @@ states newState;
         break;
       }
       if (utState != newState){
+        chprintf((BaseSequentialStream*)&global.sercanmux1, "Transmit state %d\n", newState);
         global.robot.transmitState(newState);
       }
       utState = newState;
