commit 1b3adcdd278abb4e6daba0478081040b9fbbcb86
Author: galberding <galberding@techfak.uni-bielefeld.de>
Date:   Thu Oct 10 13:35:37 2019 +0200

    Implement differnet line following strategies

diff --git a/devices/DiWheelDrive/main.cpp b/devices/DiWheelDrive/main.cpp
index 58d5031..a4f54cf 100644
--- a/devices/DiWheelDrive/main.cpp
+++ b/devices/DiWheelDrive/main.cpp
@@ -702,7 +702,7 @@ void shellRequestMotorResetGains(BaseSequentialStream *chp, int argc, char *argv
  * 
  * */
 void calibrateLineSensores(BaseSequentialStream *chp, int argc, char *argv[]) {
-    int vcnl4020AmbientLight[4];
+    // int vcnl4020AmbientLight[4];
     int vcnl4020Proximity[4];
     int rounds = 1;
     int proxyL = 0;
@@ -725,7 +725,7 @@ void calibrateLineSensores(BaseSequentialStream *chp, int argc, char *argv[]) {
 
   for (int j = 0; j < rounds; j++) {
     for (int i = 0; i < 4; i++) {
-        vcnl4020AmbientLight[i] = global.vcnl4020[i].getAmbientLight();
+        // vcnl4020AmbientLight[i] = global.vcnl4020[i].getAmbientLight();
         vcnl4020Proximity[i] = global.vcnl4020[i].getProximityScaledWoOffset();
     }
     global.robot.setLightColor(j % 8, Color(Color::BLACK));
@@ -769,15 +769,15 @@ void calibrateLineSensores(BaseSequentialStream *chp, int argc, char *argv[]) {
 
 
 void proxySensorData(BaseSequentialStream *chp, int argc, char *argv[]) {
-  uint16_t vcnl4020AmbientLight[4];
+  // uint16_t vcnl4020AmbientLight[4];
   uint16_t vcnl4020Proximity[4];
   uint16_t rounds = 1;
-  uint16_t proxyL = global.threshProxyL;
-  uint16_t proxyR = global.threshProxyR;
-  uint16_t maxDelta = 0;
+  // uint16_t proxyL = global.threshProxyL;
+  // uint16_t proxyR = global.threshProxyR;
+  // uint16_t maxDelta = 0;
   
-  int sensorL = 0;
-  int sensorR = 0;
+  // int sensorL = 0;
+  // int sensorR = 0;
   if (argc == 1){
     chprintf(chp, "Test %i rounds \n", atoi(argv[0]));
     rounds = atoi(argv[0]);
@@ -787,7 +787,7 @@ void proxySensorData(BaseSequentialStream *chp, int argc, char *argv[]) {
 
   for (int j = 0; j < rounds; j++) {
     for (int i = 0; i < 4; i++) {
-        vcnl4020AmbientLight[i] = global.vcnl4020[i].getAmbientLight();
+        // vcnl4020AmbientLight[i] = global.vcnl4020[i].getAmbientLight();
         vcnl4020Proximity[i] = global.vcnl4020[i].getProximityScaledWoOffset();
     }
     
@@ -827,12 +827,12 @@ void setRecord(BaseSequentialStream *chp, int argc, char *argv[]){
 
 
 void zieglerMeth2(BaseSequentialStream *chp, int argc, char *argv[]) {
-  int vcnl4020AmbientLight[4];
-  int vcnl4020Proximity[4];
+  // int vcnl4020AmbientLight[4];
+  // int vcnl4020Proximity[4];
   int rpmSpeed[2] = {0};
   int steps = 0;
-  int proxyL = global.threshProxyL;
-  int proxyR = global.threshProxyR;
+  // int proxyL = global.threshProxyL;
+  // int proxyR = global.threshProxyR;
   int maxDelta = 0;
   float KCrit = 0.0f;
   global.sensSamples = 0;
@@ -865,20 +865,24 @@ void zieglerMeth2(BaseSequentialStream *chp, int argc, char *argv[]) {
   // global.motorcontrol.setTargetRPM(rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] * 1000000, rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] * 1000000);
   int checkWhite = 0;
   int it_switch = steps / 2;
+  // lf.setStrategie(LineFollowStrategie::MIDDLE);
   for(int s=0; s < steps; s++){
+    
+    checkWhite = lf.followLine(rpmSpeed);
     // chprintf(chp,"S:%d,",s);
-    if(global.threshWhite)
-    if(s < it_switch){
-
-      checkWhite = lf.followRightEdge(rpmSpeed);
-    }else{
-      checkWhite = lf.followLeftEdge(rpmSpeed);
-    }
+    // if(global.threshWhite)
+    // if(s < it_switch){
+    //   lf.setStrategie(LineFollowStrategie::EDGE_LEFT);
+    //   checkWhite = lf.followLine(rpmSpeed);
+    // }else{
+    //   lf.setStrategie(LineFollowStrategie::EDGE_RIGHT);
+    //   checkWhite = lf.followLine(rpmSpeed);
+    // }
     if(checkWhite){
+      global.motorcontrol.setTargetRPM(0,0);
       for(led=0; led<8; led++){
         global.robot.setLightColor(led, Color(Color::RED));
       }
-      global.motorcontrol.setTargetRPM(0,0);
     }else{
       global.motorcontrol.setTargetRPM(rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] * 1000000, rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] * 1000000);
     }
@@ -891,59 +895,64 @@ void zieglerMeth2(BaseSequentialStream *chp, int argc, char *argv[]) {
 }
 
 
-
-void recordMove(BaseSequentialStream *chp, int argc, char *argv[]){
-  // int vcnl4020AmbientLight[4];
-  int vcnl4020Proximity[4];
-  int rpmSpeed[2] = {0};
-  int steps = 0;
+void followLine(BaseSequentialStream *chp, int argc, char *argv[]){
+  int steps = 1000;
   int speed = 0;
+  int strategy = 0;
+  int led = 0;
+  int checkWhite = 0;
+  int rpmSpeed[2] = {0};
+  LineFollow lf(&global);
   if (argc == 1){
       chprintf(chp, "%i steps \n", atoi(argv[0]));
       steps = atoi(argv[0]);
-      speed = 30;
-  }else if (argc == 2){
-    steps = atoi(argv[0]);
-    speed = atoi(argv[1]);
-  }else{
-    chprintf(chp, "No steps given!\n");
-    return;
-  }
-  global.sensSamples = steps;
-  chprintf((BaseSequentialStream*)&global.sercanmux1, "Recodring starts in five seconds...\n");
-  BaseThread::sleep(MS2ST(5000));
-  // int sensSamples = 0;
-  // sensorRecord senseRec[1000];
-
-  for (int j = 0; j < steps; j++) {
-    for (int i = 0; i < 4; i++) {
-        // vcnl4020AmbientLight[i] = global.vcnl4020[i].getAmbientLight();
-        vcnl4020Proximity[i] = global.vcnl4020[i].getProximityScaledWoOffset();
+    }else if (argc == 2){
+      steps = atoi(argv[0]);
+      speed = atoi(argv[1]);
+    }else if (argc == 3){
+      steps = atoi(argv[0]);
+      speed = atoi(argv[1]);
+      strategy = atoi(argv[2]);
+    }else{
+      chprintf(chp, "Use: followLine <steps> <speed> <strategy>\n");
+      return;
+    }
+    global.forwardSpeed = speed;
+    switch (strategy)
+    {
+    case 0:
+      lf.setStrategy(amiro::LineFollowStrategy::EDGE_RIGHT);
+      break;
+    case 1:
+      lf.setStrategy(amiro::LineFollowStrategy::EDGE_LEFT);
+      break;
+    case 2:
+      lf.setStrategy(amiro::LineFollowStrategy::FUZZY);
+      break;
+    default:
+      break;
     }
 
-    int FL = global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_LEFT].getProximityScaledWoOffset();
-    int FR = global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_RIGHT].getProximityScaledWoOffset();
-    
 
-    global.senseRec[j].FL = FL;
-    global.senseRec[j].FR = FR;
-    // chprintf(chp,"FL: 0x%x, FR: 0x%x, Delta: %d, ProxyL: %x, ProxyR: %x, MaxDelta: %d\n", 
-    //               vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT],
-    //               vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT],
-    //               vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT] - vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT]);
-    global.motorcontrol.setTargetRPM(speed * 1000000, -speed * 1000000);
-    BaseThread::sleep(CAN::UPDATE_PERIOD);
+    for(int s=0; s < steps; s++){
+    
+      checkWhite = lf.followLine(rpmSpeed);
+      if(checkWhite){
+        global.motorcontrol.setTargetRPM(0,0);
+        for(led=0; led<8; led++){
+          global.robot.setLightColor(led, Color(Color::RED));
+        }
+      }else{
+        global.motorcontrol.setTargetRPM(rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] * 1000000, rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] * 1000000);
+      }
+      
+      BaseThread::sleep(CAN::UPDATE_PERIOD);
   }
+
   global.motorcontrol.setTargetRPM(0,0);
-  for(int k=0; k<8;k++){
-    global.robot.setLightColor(k, Color(Color::WHITE));
-  }
-  BaseThread::sleep(MS2ST(1000));
-  for(int k=0; k<8;k++){
-    global.robot.setLightColor(k, Color(Color::BLACK));
-  }
 }
 
+
 void printMove(BaseSequentialStream *chp, int argc, char *argv[]){
 
   for (int j=0; j<global.sensSamples;j++){
@@ -985,9 +994,10 @@ static const ShellCommand commands[] = {
   {"dev_ziegler2", zieglerMeth2},
   // TODO: Stop user process from execution to finish/force calibration before anything starts
   {"calibrate_line", calibrateLineSensores}, 
-  {"record_move_l", recordMove},
+  // {"record_move", recordMove},
   {"print_record", printMove},
   {"setRecord", setRecord},
+  {"followLine", followLine},
   {NULL, NULL}
 };
 
