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/linefollow2.cpp b/devices/DiWheelDrive/linefollow2.cpp
index fd16596..0190c35 100644
--- a/devices/DiWheelDrive/linefollow2.cpp
+++ b/devices/DiWheelDrive/linefollow2.cpp
@@ -11,51 +11,20 @@ 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);
-    
-//     // chprintf((BaseSequentialStream*) &SD1, "Proximity: WL:0x%04X FL:0x%04X FR:0x%04X WR:0x%04X\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]);
-//     // global->motorcontrol.printGains();
-//     // chprintf((BaseSequentialStream*) &SD1, "Speed -- Left: %d, Right: %d\n", global->motorcontrol.getCurrentRPMLeft(), global->motorcontrol.getCurrentRPMRight());
-
-
-//     // float speedL = global->motorcontrol.getCurrentRPMLeft();
-//     // float speedR = global->motorcontrol.getCurrentRPMRight();
-//     // chprintf((BaseSequentialStream*) &SD1, "After motor request SP: %f,\n", SetPoint);
-//     // Process value
-//     float processV = static_cast< float >((vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT] + vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT]));
-//     // chprintf((BaseSequentialStream*) &SD1, "PV: %f,\n", processV);
-//     // chprintf((BaseSequentialStream*) &SD1, "After PV SP: %f,\n", SetPoint);
-//     float error = SetPoint - processV;
-//     float d_term = old_error - error;
-//     // chprintf((BaseSequentialStream*) &SD1, "After Error SP: %f,\n", SetPoint);
-//     // chprintf((BaseSequentialStream*) &SD1, "Error: %f,\n", error);
-//     acc_sum = 0.5 * acc_sum + error;
-//     int correctionSpeed = static_cast< int >(Kp * error + Ki*acc_sum + Kd*d_term);
-//     old_error = error;
-//     chprintf((BaseSequentialStream*) &SD1, "Error: %f,\n", error);
-//     chprintf((BaseSequentialStream*) &SD1, "Dterm: %f,\n", d_term);
-//     chprintf((BaseSequentialStream*) &SD1, "Iterm: %f,\n", acc_sum);
-//     chprintf((BaseSequentialStream*) &SD1, "New Speed: %d,\n", correctionSpeed);
-//     // chprintf((BaseSequentialStream*) &SD1, "New Speed: %f, Sum: %f, SP: %f, processV: %f, K_p: %f, K_i: %f \n", correctionSpeed, acc_sum, SetPoint, processV, Kp, Ki);
-
-//     // int forward = 15;
-//     int speedL = global->rpmForward[constants::DiWheelDrive::LEFT_WHEEL] - correctionSpeed;
-//     int speedR = global->rpmForward[constants::DiWheelDrive::RIGHT_WHEEL] + correctionSpeed;
-
-//     // if (l_speed )
-
-//     rpmFuzzyCtrl[constants::DiWheelDrive::LEFT_WHEEL] = speedL;
-//     rpmFuzzyCtrl[constants::DiWheelDrive::RIGHT_WHEEL] = speedR;
-
-//     chprintf((BaseSequentialStream*) &SD1, "Speed L: %d, R: %d\n", speedL, speedR);
-
-// }
+int LineFollow::delta(){
+    int delta = 0;
+    int FL = global->vcnl4020[constants::DiWheelDrive::PROX_FRONT_LEFT].getProximityScaledWoOffset();
+    int FR = global->vcnl4020[constants::DiWheelDrive::PROX_FRONT_RIGHT].getProximityScaledWoOffset();
+
+    delta = abs(abs(global->threshProxyL-global->threshProxyR) - abs(FL-FR));
+
+    if (FR > global->threshProxyR && FL > global->threshProxyL ){
+        return delta * -1;
+    }else {
+        return delta;
+    }
+    return  delta;
+}
 
 
 void LineFollow::stableFollow(int vcnl4020Proximity[4], int (&rpmFuzzyCtrl)[2], Global *global){
@@ -84,89 +53,15 @@ void LineFollow::stableFollow(int vcnl4020Proximity[4], int (&rpmFuzzyCtrl)[2],
 
 }
 
-int calculateError(){
-    
-}
 
-void calibrateZiegler(int (&rpmFuzzyCtrl)[2], Global *global){
 
-}
+void LineFollow::calibrateZiegler(float KCrit, int rpmSpeed[2]){
+    int targetSpeedL = 5;
+    int targetSpeedR = 5;
 
-// void LineFollow::followLineSeperateSensors2(int vcnl4020Proximity[4], int (&rpmFuzzyCtrl)[2], Global *global){
-    
-//     chprintf((BaseSequentialStream*) &SD1, "SP: %d,\n", SetPoint);
-//     int targetSensorL = 0x10;
-//     int targetSensorR = 0x28;
-    
-//     float actualSpeedL = 20;
-//     float actualSpeedR = 20;
-
-//     // if(actualSpeedL == 0){
-//     //     actualSpeedL = 1;
-//     // }
-//     // if(actualSpeedR == 0){
-//     //     actualSpeedR = 1;
-//     // }
-
-//     // Shift sensor values to prevent overflow in following calculation
-//     int actualSensorL = vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT] >> 8;
-//     int actualSensorR = vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT] >> 8;
-//     chprintf((BaseSequentialStream*) &SD1, "Sensor L: %d, R: %d\n", actualSensorL, actualSensorR);
-
-//     int targetSpeedL = global->rpmForward[constants::DiWheelDrive::LEFT_WHEEL];
-//     int targetSpeedR = global->rpmForward[constants::DiWheelDrive::RIGHT_WHEEL];
-
-//     int setPointL = targetSensorL;
-//     int setPointR = targetSensorR;
-//     chprintf((BaseSequentialStream*) &SD1, "SetPoint L: %d, R: %d\n",setPointL, setPointR );
-
-//     int processValueL =  actualSensorL;
-//     int processValueR =  actualSensorR;
-//     chprintf((BaseSequentialStream*) &SD1, "ProcessValue L: %d, R: %d\n",processValueL, processValueR );
-
-//     int errorL = setPointL - processValueL;
-//     int errorR = setPointR - processValueR;
-
-//     // This will howfully decrease the overall speed when sensors deviate much 
-//         // errorL /= targetSensorL+actualSensorL;
-//         // errorR /= targetSensorR+actualSensorR;
-//     chprintf((BaseSequentialStream*) &SD1, "Error L: %d, R: %d\n",errorL, errorR);
-
-//     // int newSpeedL =  
-//     rpmFuzzyCtrl[constants::DiWheelDrive::LEFT_WHEEL] = errorL;
-//     rpmFuzzyCtrl[constants::DiWheelDrive::RIGHT_WHEEL] = errorR;
+    int correctionSpeed = (int) (KCrit * delta());   
     
-//     int correction_speedL = (int) (Kp * errorL);   
-//     int correction_speedR = (int) (Kp * errorR);   
-//     chprintf((BaseSequentialStream*) &SD1, "Speed L: %d, R: %d\n",correction_speedL, correction_speedR);
-
-//     // // chprintf((BaseSequentialStream*) &SD1, "After motor request SP: %f,\n", SetPoint);
-//     // // Process value
-//     // float processV = static_cast< float >((vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT] + vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT]));
-//     // // chprintf((BaseSequentialStream*) &SD1, "PV: %f,\n", processV);
-//     // // chprintf((BaseSequentialStream*) &SD1, "After PV SP: %f,\n", SetPoint);
-//     // float error = SetPoint - processV;
-//     // float d_term = old_error - error;
-//     // // chprintf((BaseSequentialStream*) &SD1, "After Error SP: %f,\n", SetPoint);
-//     // // chprintf((BaseSequentialStream*) &SD1, "Error: %f,\n", error);
-//     // acc_sum = 0.5 * acc_sum + error;
-//     // int new_speed = static_cast< int >(Kp * error + Ki*acc_sum + Kd*d_term);
-//     // old_error = error;
-//     // chprintf((BaseSequentialStream*) &SD1, "Error: %f,\n", error);
-//     // chprintf((BaseSequentialStream*) &SD1, "Dterm: %f,\n", d_term);
-//     // chprintf((BaseSequentialStream*) &SD1, "Iterm: %f,\n", acc_sum);
-//     // chprintf((BaseSequentialStream*) &SD1, "New Speed: %d,\n", new_speed);
-//     // // chprintf((BaseSequentialStream*) &SD1, "New Speed: %f, Sum: %f, SP: %f, processV: %f, K_p: %f, K_i: %f \n", new_speed, acc_sum, SetPoint, processV, Kp, Ki);
-
-//     // // int forward = 15;
-//     // // int l_speed = forward - new_speed;
-//     // // int r_speed = forward + new_speed;
-
-//     // // if (l_speed )
-
-//     rpmFuzzyCtrl[constants::DiWheelDrive::LEFT_WHEEL] = targetSpeedL + correction_speedL;
-//     rpmFuzzyCtrl[constants::DiWheelDrive::RIGHT_WHEEL] = targetSpeedR + correction_speedR;
-
-//     // chprintf((BaseSequentialStream*) &SD1, "Speed L: %d, R: %d\n", rpmFuzzyCtrl[constants::DiWheelDrive::LEFT_WHEEL], rpmFuzzyCtrl[constants::DiWheelDrive::RIGHT_WHEEL]);
-
-// }
\ No newline at end of file
+    rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] = targetSpeedL + correctionSpeed;
+    rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] = targetSpeedR - 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 89d5379..af0596e 100644
--- a/devices/DiWheelDrive/linefollow2.hpp
+++ b/devices/DiWheelDrive/linefollow2.hpp
@@ -22,6 +22,10 @@ public:
   int biggestDiff = 0;
   Global *global;
   LineFollow(Global *global);
+  void calibrateZiegler(float KCrit, int rpmSpeed[2]);
+
+  private:
+    int delta();
 
 };
 
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}
 };
 
diff --git a/devices/DiWheelDrive/userthread.cpp b/devices/DiWheelDrive/userthread.cpp
index bb7bd00..4339c53 100644
--- a/devices/DiWheelDrive/userthread.cpp
+++ b/devices/DiWheelDrive/userthread.cpp
@@ -462,7 +462,7 @@ UserThread::main()
 	//   lineFollownew
 	//else
             // lineFollowing(vcnl4020Proximity, rpmFuzzyCtrl, &global);
-            // setRpmSpeed(rpmFuzzyCtrl);
+            setRpmSpeed(rpmFuzzyCtrl);
         }
 
 		// this->sleep(US2ST(5));
