commit 12463563e3f19bd29fab1deb81b0f6221e8acf35
Author: galberding <galberding@techfak.uni-bielefeld.de>
Date:   Tue Oct 1 15:37:27 2019 +0200

    Implement adaptive threshhold for line Following. AMiRo can now be calibrated and the sensor values with the maximum thesh will be set as guiding thresholds

diff --git a/devices/DiWheelDrive/global.hpp b/devices/DiWheelDrive/global.hpp
index 2d75bc9..cb6d7ba 100644
--- a/devices/DiWheelDrive/global.hpp
+++ b/devices/DiWheelDrive/global.hpp
@@ -170,6 +170,10 @@ public:
   int rpmSoftRight[2] = {rpmSoftLeft[1],rpmSoftLeft[0]};
   int rpmHardRight[2] = {rpmHardLeft[1],rpmHardLeft[0]};
 
+  // Line Following thresholds set due to calibration
+  int threshProxyL = 0;
+  int threshProxyR = 0;
+
 public:
   Global() :
     HW_I2C1(&I2CD1), HW_I2C2(&I2CD2),
diff --git a/devices/DiWheelDrive/linefollow2.cpp b/devices/DiWheelDrive/linefollow2.cpp
index 84e4bd7..fd16596 100644
--- a/devices/DiWheelDrive/linefollow2.cpp
+++ b/devices/DiWheelDrive/linefollow2.cpp
@@ -7,6 +7,10 @@ void LineFollow::printSensorData(){
     chprintf((BaseSequentialStream*) &SD1, "Test!");
 }
 
+LineFollow::LineFollow(Global *global){
+    this->global = global;
+}
+
 // void LineFollow::followLine(int vcnl4020Proximity[4], int (&rpmFuzzyCtrl)[2], Global *global){
     
 //     chprintf((BaseSequentialStream*) &SD1, "SP: %d,\n", SetPoint);
@@ -80,6 +84,13 @@ void LineFollow::stableFollow(int vcnl4020Proximity[4], int (&rpmFuzzyCtrl)[2],
 
 }
 
+int calculateError(){
+    
+}
+
+void calibrateZiegler(int (&rpmFuzzyCtrl)[2], Global *global){
+
+}
 
 // void LineFollow::followLineSeperateSensors2(int vcnl4020Proximity[4], int (&rpmFuzzyCtrl)[2], Global *global){
     
diff --git a/devices/DiWheelDrive/linefollow2.hpp b/devices/DiWheelDrive/linefollow2.hpp
index cd65d12..89d5379 100644
--- a/devices/DiWheelDrive/linefollow2.hpp
+++ b/devices/DiWheelDrive/linefollow2.hpp
@@ -20,6 +20,8 @@ public:
   int accSum = 0;
   float oldError = 0;
   int biggestDiff = 0;
+  Global *global;
+  LineFollow(Global *global);
 
 };
 
diff --git a/devices/DiWheelDrive/main.cpp b/devices/DiWheelDrive/main.cpp
index 5cdfae7..bae5864 100644
--- a/devices/DiWheelDrive/main.cpp
+++ b/devices/DiWheelDrive/main.cpp
@@ -690,6 +690,121 @@ void shellRequestMotorResetGains(BaseSequentialStream *chp, int argc, char *argv
   return;
 }
 
+
+/**
+ * Calibrate the thresholds for left and right sensor to get the maximum threshold and to
+ * be able to detect the correction direction.
+ * In this case it is expected that the FL-Sensor sould be in the white part of the edge and the FR-Sensor in the black one.
+ * 
+ * Note: invert the threshs to drive on the other edge.
+ * 
+ * */
+void calibrateLineSensores(BaseSequentialStream *chp, int argc, char *argv[]) {
+    int vcnl4020AmbientLight[4];
+    int vcnl4020Proximity[4];
+    int rounds = 1;
+    int proxyL = 0;
+    int proxyR = 0;
+    int maxDelta = 0;
+ 
+  if (argc == 1){
+    chprintf(chp, "Test %i rounds \n", atoi(argv[0]));
+    rounds = atoi(argv[0]);
+    
+  }else{
+    chprintf(chp, "Usage: calbrate_line_sensors [1,n]\nThis will calibrate the thresholds for the left and right sensor\naccording to the maximum delta value recorded.\n");
+    return;
+  }
+
+
+  for (int j = 0; j < rounds; j++) {
+    for (int i = 0; i < 4; i++) {
+        vcnl4020AmbientLight[i] = global.vcnl4020[i].getAmbientLight();
+        vcnl4020Proximity[i] = global.vcnl4020[i].getProximityScaledWoOffset();
+    }
+
+    int delta = abs(vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT]
+                  - vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT]);
+    // Update proximity thresh
+    if (delta > maxDelta) {
+      maxDelta = delta;
+      proxyL = vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT];
+      proxyR = vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT];
+    }
+
+    // if (vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT] > proxyR && vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT] > proxyL ){
+    //   delta *= -1;
+    // }
+
+    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],
+                  delta,
+                  proxyL,
+                  proxyR,
+                  maxDelta);
+    // sleep(CAN::UPDATE_PERIOD);
+    BaseThread::sleep(CAN::UPDATE_PERIOD);
+  }
+  chprintf(chp,"Sensors Calibrated: MaxDelta: %d, FL: 0x%x, FR: 0x%d\n", maxDelta, proxyL, proxyR);
+
+  global.threshProxyL = proxyL;
+  global.threshProxyR = proxyR;
+  return;
+}
+
+
+
+void proxySensorData(BaseSequentialStream *chp, int argc, char *argv[]) {
+  int vcnl4020AmbientLight[4];
+  int vcnl4020Proximity[4];
+  int rounds = 1;
+  int proxyL = global.threshProxyL;
+  int proxyR = global.threshProxyR;
+  int maxDelta = 0;
+ 
+  if (argc == 1){
+    chprintf(chp, "Test %i rounds \n", atoi(argv[0]));
+    rounds = atoi(argv[0]);
+    
+  }
+
+  for (int j = 0; j < rounds; j++) {
+    for (int i = 0; i < 4; i++) {
+        vcnl4020AmbientLight[i] = global.vcnl4020[i].getAmbientLight();
+        vcnl4020Proximity[i] = global.vcnl4020[i].getProximityScaledWoOffset();
+    }
+
+    int delta = abs(vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT]
+                  - vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT]);
+    // // Update proximity thresh
+    // if (delta > maxDelta) {
+    //   maxDelta = delta;
+    //   proxyL = vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT];
+    //   proxyR = vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT];
+    // }
+
+    if (vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT] > proxyR && vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT] > proxyL ){
+      delta *= -1;
+    }
+
+    chprintf(chp,"WL: %x, FL: %x, FR: %x, WR: %x, ProxyL: %x, ProxyR: %x, Delta: %d\n", 
+                  vcnl4020Proximity[constants::DiWheelDrive::PROX_WHEEL_LEFT],
+                  vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT],
+                  vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT],
+                  vcnl4020Proximity[constants::DiWheelDrive::PROX_WHEEL_RIGHT],
+                  proxyL,
+                  proxyR,
+                  delta);
+    // sleep(CAN::UPDATE_PERIOD);
+    BaseThread::sleep(CAN::UPDATE_PERIOD);
+  }
+  chprintf(chp,"Summary: MaxDelta: %d, FL: %x, FR: %d\n", maxDelta, proxyL, proxyR);
+  return;
+}
+
+
+
 static const ShellCommand commands[] = {
   {"shutdown", shellRequestShutdown},
   {"wakeup", shellRequestWakeup},
@@ -716,6 +831,8 @@ static const ShellCommand commands[] = {
   {"motor_calibrate", shellRequestMotorCalibrate},
   {"motor_getGains", shellRequestMotorGetGains},
   {"motor_resetGains", shellRequestMotorResetGains},
+  {"dev_proxi_sensor_data", proxySensorData},
+  {"calibrate_line", calibrateLineSensores},
   {NULL, NULL}
 };
 
diff --git a/devices/DiWheelDrive/userthread.cpp b/devices/DiWheelDrive/userthread.cpp
index a5deb9b..bb7bd00 100644
--- a/devices/DiWheelDrive/userthread.cpp
+++ b/devices/DiWheelDrive/userthread.cpp
@@ -394,7 +394,7 @@ UserThread::main()
 		global.robot.setLightColor(led, Color(Color::BLACK));
     }
     running = false;
-	LineFollow lf;
+	LineFollow lf(&global);
 	/*
 	 * LOOP
 	 */
