commit 22b85da1838c69caa5a689bd60a6ab9b0302a96c
Author: galberding <galberding@techfak.uni-bielefeld.de>
Date:   Wed Oct 9 16:40:26 2019 +0200

    Implement different line following strategies and stopping if sensor detects white

diff --git a/devices/DiWheelDrive/global.hpp b/devices/DiWheelDrive/global.hpp
index 489eb3b..949c091 100644
--- a/devices/DiWheelDrive/global.hpp
+++ b/devices/DiWheelDrive/global.hpp
@@ -169,11 +169,27 @@ public:
   int rpmHardLeft[2] = {5,20};
   int rpmSoftRight[2] = {rpmSoftLeft[1],rpmSoftLeft[0]};
   int rpmHardRight[2] = {rpmHardLeft[1],rpmHardLeft[0]};
+  
 
   // Line Following thresholds set due to calibration
   // MaxDelta: 18676, FL: 4289, FR: 22965
-  int threshProxyL = 2168;
-  int threshProxyR = 24406;
+  // Thresh FL: 5241, FR: 25528
+  int threshProxyL = 5241;
+  int threshProxyR = 25528;
+  int threshWhite = 78000;
+
+  // PID for line following:
+  float K_p = 0.0f;
+  float K_i = 0.0f;
+  float K_d = 0.0f;
+
+  // Integral part
+  int accumHist = 0;
+  int oldError = 0;
+
+// Debugging stuff --------------
+  int forwardSpeed = 10;
+  int enableRecord = 0;
 
   // Buffer for sensor values
   struct sensorRecord
@@ -186,8 +202,9 @@ public:
   
   int sensSamples = 0;
   sensorRecord senseRec[1000];
+  sensorRecord maxDist;
   
-
+// -----------------------------
 public:
   Global() :
     HW_I2C1(&I2CD1), HW_I2C2(&I2CD2),
diff --git a/devices/DiWheelDrive/linefollow2.cpp b/devices/DiWheelDrive/linefollow2.cpp
index feb3944..4fbd65f 100644
--- a/devices/DiWheelDrive/linefollow2.cpp
+++ b/devices/DiWheelDrive/linefollow2.cpp
@@ -2,15 +2,17 @@
 #include "linefollow2.hpp" 
 #include <cmath>
 
-
+// Trash
 void LineFollow::printSensorData(){
     chprintf((BaseSequentialStream*) &SD1, "Test!");
 }
 
+
 LineFollow::LineFollow(Global *global){
     this->global = global;
 }
 
+// trash
 int LineFollow::delta(){
     int delta = 0;
     int FL = global->vcnl4020[constants::DiWheelDrive::PROX_FRONT_LEFT].getProximityScaledWoOffset();
@@ -26,7 +28,7 @@ int LineFollow::delta(){
     return  delta;
 }
 
-
+// old and trash
 void LineFollow::stableFollow(int vcnl4020Proximity[4], int (&rpmFuzzyCtrl)[2], Global *global){
     int targetSensor = 0x38;
     int actualSensorL = vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT] ;
@@ -53,28 +55,85 @@ void LineFollow::stableFollow(int vcnl4020Proximity[4], int (&rpmFuzzyCtrl)[2],
 
 }
 
-int error(){
+/**
+ * Calculate the error from front proxi sensors and fixed threshold values for those sensors.
+ */
+int LineFollow::getError(){
+    
     int FL = global->vcnl4020[constants::DiWheelDrive::PROX_FRONT_LEFT].getProximityScaledWoOffset();
     int FR = global->vcnl4020[constants::DiWheelDrive::PROX_FRONT_RIGHT].getProximityScaledWoOffset();
     int targetL = global->threshProxyL;
     int targetR = global->threshProxyR;
-    return FL -targetL + FR - targetR;
+    int error = FL -targetL + FR - targetR;
+
+    if (FL+FR > global->threshWhite){
+        whiteFlag = 1;
+    }else{
+        whiteFlag = 0;
+    }
+    return error;
+}
+
+/**
+ * Follow strategy for left edge. 
+ */
+int LineFollow::followLeftEdge(int rpmSpeed[2]){
+
+    int correctionSpeed = getPidCorrectionSpeed();
+    chprintf((BaseSequentialStream*) &SD1, "Correction: %d, thresh: %d\n",correctionSpeed,  global->threshWhite);
+
+    rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] =   global->forwardSpeed + correctionSpeed;
+
+    rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] = global->forwardSpeed - correctionSpeed;
+    return whiteFlag;
+}
+
+/**
+ * Follow strategy for right edge. 
+ */
+int LineFollow::followRightEdge(int rpmSpeed[2]){
+
+    int correctionSpeed = getPidCorrectionSpeed();
+    chprintf((BaseSequentialStream*) &SD1, "Correction: %d, thresh: %d\n",correctionSpeed,  global->threshWhite);
+
+    rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] =   global->forwardSpeed - correctionSpeed;
+
+    rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] = global->forwardSpeed + correctionSpeed;
+    return whiteFlag;
+}
+
+/**
+ * Pid controller which returns a corrections speed.
+ */
+int LineFollow::getPidCorrectionSpeed(){
+    int error = getError();
+    int sloap = error - global->oldError;
+    int correctionSpeed = (int) (global->K_p*error + global->K_i*global->accumHist + global->K_d*sloap);
+    global->oldError = error;
+    global->accumHist += error;
+    if (abs(error) > global->maxDist.error){
+        global->maxDist.error = error;
+    }
+    return correctionSpeed;
 }
 
-void LineFollow::calibrateZiegler(float KCrit, int rpmSpeed[2]){
-    int targetSpeedL = 5;
-    int targetSpeedR = 5;
-    int delta_ = error();
-    int correctionSpeed = (int) (KCrit * delta_);   
-    // global->senseRec[global->sensSamples].FL = FL;
-    // global->senseRec[global->sensSamples].FR = FR;
-    global->senseRec[global->sensSamples].error = delta_;
-    global->senseRec[global->sensSamples].FL = global->vcnl4020[constants::DiWheelDrive::PROX_FRONT_LEFT].getProximityScaledWoOffset();;
-    global->senseRec[global->sensSamples].FR = global->vcnl4020[constants::DiWheelDrive::PROX_FRONT_RIGHT].getProximityScaledWoOffset();;
-    global->sensSamples++
-
-
-    rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] =   -1*correctionSpeed;
-    rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] =  correctionSpeed;
-    // chprintf((BaseSequentialStream*) &SD1, "CS:%d,LW:%d,RW:%d\n", correctionSpeed, rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL], rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL]);
-}
\ No newline at end of file
+// trash
+// void LineFollow::calibrateZiegler(float KCrit, int rpmSpeed[2]){
+//     int targetSpeedL = 5;
+//     int targetSpeedR = 5;
+//     int delta_ = error();
+//     int correctionSpeed = (int) (KCrit * delta_);   
+//     if (global->enableRecord){
+//         global->senseRec[global->sensSamples].error = delta_;
+//         global->senseRec[global->sensSamples].FL = global->vcnl4020[constants::DiWheelDrive::PROX_FRONT_LEFT].getProximityScaledWoOffset();
+//         global->senseRec[global->sensSamples].FR = global->vcnl4020[constants::DiWheelDrive::PROX_FRONT_RIGHT].getProximityScaledWoOffset();
+//         global->sensSamples++;
+//         }
+//     if (abs(delta_) > global->maxDist.error){
+//         global->maxDist.error = delta_;
+//     }
+
+//     rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] =   global->forwardSpeed + -1*correctionSpeed;
+//     rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] = global->forwardSpeed + correctionSpeed;
+//     chprintf((BaseSequentialStream*) &SD1, "CS:%d,LW:%d,RW:%d\n", correctionSpeed, rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL], rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL]);
+// }
\ No newline at end of file
diff --git a/devices/DiWheelDrive/linefollow2.hpp b/devices/DiWheelDrive/linefollow2.hpp
index af0596e..64ff120 100644
--- a/devices/DiWheelDrive/linefollow2.hpp
+++ b/devices/DiWheelDrive/linefollow2.hpp
@@ -15,17 +15,23 @@ public:
   float SetPoint = 0x4000; // (0x1800+0x2800) >> 8
   float Kp = 0.001;
   float Ki = 0.00001;
-  float Kd = 0.5
-  ;
+  float Kd = 0.5;
   int accSum = 0;
   float oldError = 0;
   int biggestDiff = 0;
   Global *global;
   LineFollow(Global *global);
-  void calibrateZiegler(float KCrit, int rpmSpeed[2]);
+  // void calibrateZiegler(float KCrit, int rpmSpeed[2]);
+  int followLeftEdge(int rpmSpeed[2]);
+  int followRightEdge(int rpmSpeed[2]);
 
   private:
     int delta();
+    int getError();
+    int getPidCorrectionSpeed();
+
+    char whiteFlag = 0;
+    
 
 };
 
diff --git a/devices/DiWheelDrive/main.cpp b/devices/DiWheelDrive/main.cpp
index f607cc0..58d5031 100644
--- a/devices/DiWheelDrive/main.cpp
+++ b/devices/DiWheelDrive/main.cpp
@@ -708,6 +708,8 @@ void calibrateLineSensores(BaseSequentialStream *chp, int argc, char *argv[]) {
     int proxyL = 0;
     int proxyR = 0;
     int maxDelta = 0;
+    int sensorL = 0;
+    int sensorR = 0;
  
   if (argc == 1){
     chprintf(chp, "Test %i rounds \n", atoi(argv[0]));
@@ -739,6 +741,8 @@ void calibrateLineSensores(BaseSequentialStream *chp, int argc, char *argv[]) {
       proxyL = vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT];
       proxyR = vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT];
     }
+    sensorL += global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_LEFT].getProximityScaledWoOffset();
+    sensorR += global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_RIGHT].getProximityScaledWoOffset();
 
     // if (vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT] > proxyR && vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT] > proxyL ){
     //   delta *= -1;
@@ -754,10 +758,11 @@ void calibrateLineSensores(BaseSequentialStream *chp, int argc, char *argv[]) {
     // sleep(CAN::UPDATE_PERIOD);
     BaseThread::sleep(CAN::UPDATE_PERIOD);
   }
-  chprintf(chp,"Sensors Calibrated: MaxDelta: %d, FL: %d, FR: %d\n", maxDelta, proxyL, proxyR);
+  
 
-  global.threshProxyL = proxyL;
-  global.threshProxyR = proxyR;
+  global.threshProxyL = sensorL / rounds;
+  global.threshProxyR = sensorR / rounds;
+  chprintf(chp,"Thresh FL: %d, FR: %d\n",  global.threshProxyL, global.threshProxyR);
   return;
 }
 
@@ -770,7 +775,9 @@ void proxySensorData(BaseSequentialStream *chp, int argc, char *argv[]) {
   uint16_t proxyL = global.threshProxyL;
   uint16_t proxyR = global.threshProxyR;
   uint16_t maxDelta = 0;
- 
+  
+  int sensorL = 0;
+  int sensorR = 0;
   if (argc == 1){
     chprintf(chp, "Test %i rounds \n", atoi(argv[0]));
     rounds = atoi(argv[0]);
@@ -783,7 +790,7 @@ void proxySensorData(BaseSequentialStream *chp, int argc, char *argv[]) {
         vcnl4020AmbientLight[i] = global.vcnl4020[i].getAmbientLight();
         vcnl4020Proximity[i] = global.vcnl4020[i].getProximityScaledWoOffset();
     }
-
+    
     uint16_t delta = (vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT]
                   - vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT]);
     // // Update proximity thresh
@@ -810,6 +817,13 @@ void proxySensorData(BaseSequentialStream *chp, int argc, char *argv[]) {
   return;
 }
 
+// Either 0 to disable record or > 0 to enable it
+void setRecord(BaseSequentialStream *chp, int argc, char *argv[]){
+  if (argc == 1){
+    chprintf(chp, "Set recording to %d\n", atoi(argv[0]));
+    global.enableRecord = atoi(argv[0]);
+  }
+}
 
 
 void zieglerMeth2(BaseSequentialStream *chp, int argc, char *argv[]) {
@@ -822,28 +836,57 @@ void zieglerMeth2(BaseSequentialStream *chp, int argc, char *argv[]) {
   int maxDelta = 0;
   float KCrit = 0.0f;
   global.sensSamples = 0;
+  global.maxDist.error = 0;
   LineFollow lf(&global);
+  int led = 0;
  
   if (argc == 2){
     chprintf(chp, "KCrti %f\n", atof(argv[0]));
     chprintf(chp, "Steps %i\n", atoi(argv[1]));
     KCrit = atof(argv[0]);
     steps = atoi(argv[1]);
-  } else{
-    chprintf(chp, "Usage: dev_ziegler2 <K_crit> <steps>");
+  } else if (argc == 3){
+    chprintf(chp, "KCrti %f\n", atof(argv[0]));
+    chprintf(chp, "Steps %i\n", atoi(argv[1]));
+    KCrit = atof(argv[0]);
+    steps = atoi(argv[1]);
+    global.forwardSpeed = atoi(argv[2]);
+    
+  }else{
+    chprintf(chp, "Usage: dev_ziegler2 <K_crit> <steps> (<speed>)");
     return;
   }
+  global.K_p = KCrit;
+  for(led=0; led<8; led++){
+        global.robot.setLightColor(led, Color(Color::BLACK));
+  }
   chprintf((BaseSequentialStream*)&global.sercanmux1, "Recodring starts in five seconds...\n");
   BaseThread::sleep(MS2ST(5000));
   // global.motorcontrol.setTargetRPM(rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] * 1000000, rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] * 1000000);
-  
+  int checkWhite = 0;
+  int it_switch = steps / 2;
   for(int s=0; s < steps; s++){
-    chprintf(chp,"S:%d,",s);
-    lf.calibrateZiegler(KCrit, rpmSpeed);
+    // chprintf(chp,"S:%d,",s);
+    if(global.threshWhite)
+    if(s < it_switch){
 
-    global.motorcontrol.setTargetRPM(rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] * 1000000, rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] * 1000000);
+      checkWhite = lf.followRightEdge(rpmSpeed);
+    }else{
+      checkWhite = lf.followLeftEdge(rpmSpeed);
+    }
+    if(checkWhite){
+      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);
+    }
+    
+    
     BaseThread::sleep(CAN::UPDATE_PERIOD);
   }
+
   global.motorcontrol.setTargetRPM(0,0);
 }
 
@@ -884,7 +927,6 @@ void recordMove(BaseSequentialStream *chp, int argc, char *argv[]){
 
     global.senseRec[j].FL = FL;
     global.senseRec[j].FR = FR;
-    global.senseRec[j].delta = delta;
     // 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],
@@ -905,8 +947,10 @@ void recordMove(BaseSequentialStream *chp, int argc, char *argv[]){
 void printMove(BaseSequentialStream *chp, int argc, char *argv[]){
 
   for (int j=0; j<global.sensSamples;j++){
-    chprintf(chp,"FL:%d,FR:%d,Delta:%d\n",global.senseRec[j].FL, global.senseRec[j].FR, global.senseRec[j].delta);
+    chprintf(chp,"FL:%d,FR:%d,Delta:%d,Error:%d\n",global.senseRec[j].FL, global.senseRec[j].FR, global.senseRec[j].delta, global.senseRec[j].error);
   }
+  chprintf(chp,"MaxDist: FL:%d,FR:%d,Delta:%d,Error:%d\n",global.maxDist.FL, global.maxDist.FR, global.maxDist.delta, global.maxDist.error);
+
 
 }
 
@@ -943,6 +987,7 @@ static const ShellCommand commands[] = {
   {"calibrate_line", calibrateLineSensores}, 
   {"record_move_l", recordMove},
   {"print_record", printMove},
+  {"setRecord", setRecord},
   {NULL, NULL}
 };
 
