commit af93a91c89b03c4043bdfd9a16e24877c11daaa4
Author: galberding <galberding@techfak.uni-bielefeld.de>
Date:   Wed Oct 2 16:16:49 2019 +0200

    Add first part of ziegler method, refine shell comands to parse output via python

diff --git a/devices/DiWheelDrive/main.cpp b/devices/DiWheelDrive/main.cpp
index bae5864..9ff3acc 100644
--- a/devices/DiWheelDrive/main.cpp
+++ b/devices/DiWheelDrive/main.cpp
@@ -17,6 +17,8 @@
 #include <chprintf.h>
 #include <shell.h>
 
+#include "linefollow2.hpp" 
+
 using namespace chibios_rt;
 
 Global global;
@@ -715,18 +717,24 @@ void calibrateLineSensores(BaseSequentialStream *chp, int argc, char *argv[]) {
     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 (uint8_t led = 0; led < 8; ++led) {
+    global.robot.setLightColor(led, Color(Color::BLACK));
+  }
 
   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();
     }
-
+    global.robot.setLightColor(j % 8, Color(Color::BLACK));
+    global.robot.setLightColor(j+1 % 8, Color(Color::WHITE));
     int delta = abs(vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT]
                   - vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT]);
     // Update proximity thresh
     if (delta > maxDelta) {
+      for (uint8_t led = 0; led < 8; ++led) {
+        global.robot.setLightColor(led, Color(Color::GREEN));
+      }
       maxDelta = delta;
       proxyL = vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT];
       proxyR = vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT];
@@ -756,18 +764,19 @@ void calibrateLineSensores(BaseSequentialStream *chp, int argc, char *argv[]) {
 
 
 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;
+  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;
  
   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++) {
@@ -775,7 +784,7 @@ void proxySensorData(BaseSequentialStream *chp, int argc, char *argv[]) {
         vcnl4020Proximity[i] = global.vcnl4020[i].getProximityScaledWoOffset();
     }
 
-    int delta = abs(vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT]
+    uint16_t delta = (vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT]
                   - vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT]);
     // // Update proximity thresh
     // if (delta > maxDelta) {
@@ -784,27 +793,58 @@ void proxySensorData(BaseSequentialStream *chp, int argc, char *argv[]) {
     //   proxyR = vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT];
     // }
 
-    if (vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT] > proxyR && vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT] > proxyL ){
-      delta *= -1;
-    }
+    // 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", 
+    chprintf(chp,"WL:%d,FL:%d,FR:%d,WR:%d,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);
+  // chprintf(chp,"Summary: MaxDelta: %d, FL: %x, FR: %d\n", maxDelta, proxyL, proxyR);
   return;
 }
 
 
 
+void zieglerMeth2(BaseSequentialStream *chp, int argc, char *argv[]) {
+  int vcnl4020AmbientLight[4];
+  int vcnl4020Proximity[4];
+  int rpmSpeed[2] = {0};
+  int steps = 0;
+  int proxyL = global.threshProxyL;
+  int proxyR = global.threshProxyR;
+  int maxDelta = 0;
+  float KCrit = 0.0f;
+  LineFollow lf(&global);
+ 
+  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>");
+    return;
+  }
+  // global.motorcontrol.setTargetRPM(rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] * 1000000, rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] * 1000000);
+  
+  for(int s=0; s < steps; s++){
+    chprintf(chp,"S:%d,",s);
+    lf.calibrateZiegler(KCrit, rpmSpeed);
+
+    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);
+}
+
+
 static const ShellCommand commands[] = {
   {"shutdown", shellRequestShutdown},
   {"wakeup", shellRequestWakeup},
@@ -832,7 +872,9 @@ static const ShellCommand commands[] = {
   {"motor_getGains", shellRequestMotorGetGains},
   {"motor_resetGains", shellRequestMotorResetGains},
   {"dev_proxi_sensor_data", proxySensorData},
-  {"calibrate_line", calibrateLineSensores},
+  {"dev_ziegler2", zieglerMeth2},
+  // TODO: Stop user process from execution to finish/force calibration before anything starts
+  {"calibrate_line", calibrateLineSensores}, 
   {NULL, NULL}
 };
 
