commit 88afb834f7822624d54f01e715fb818dfa65f5af
Author: galberding <galberding@techfak.uni-bielefeld.de>
Date:   Wed Oct 9 10:25:43 2019 +0200

    New error function

diff --git a/devices/DiWheelDrive/global.hpp b/devices/DiWheelDrive/global.hpp
index cb6d7ba..489eb3b 100644
--- a/devices/DiWheelDrive/global.hpp
+++ b/devices/DiWheelDrive/global.hpp
@@ -171,8 +171,22 @@ public:
   int rpmHardRight[2] = {rpmHardLeft[1],rpmHardLeft[0]};
 
   // Line Following thresholds set due to calibration
-  int threshProxyL = 0;
-  int threshProxyR = 0;
+  // MaxDelta: 18676, FL: 4289, FR: 22965
+  int threshProxyL = 2168;
+  int threshProxyR = 24406;
+
+  // Buffer for sensor values
+  struct sensorRecord
+  {
+    int FL = 0;
+    int FR = 0;
+    int delta = 0;
+    int error = 0;
+  };
+  
+  int sensSamples = 0;
+  sensorRecord senseRec[1000];
+  
 
 public:
   Global() :
diff --git a/devices/DiWheelDrive/linefollow2.cpp b/devices/DiWheelDrive/linefollow2.cpp
index 0190c35..feb3944 100644
--- a/devices/DiWheelDrive/linefollow2.cpp
+++ b/devices/DiWheelDrive/linefollow2.cpp
@@ -19,9 +19,9 @@ int LineFollow::delta(){
     delta = abs(abs(global->threshProxyL-global->threshProxyR) - abs(FL-FR));
 
     if (FR > global->threshProxyR && FL > global->threshProxyL ){
-        return delta * -1;
+        return delta ;
     }else {
-        return delta;
+        return delta* -1;
     }
     return  delta;
 }
@@ -53,15 +53,28 @@ void LineFollow::stableFollow(int vcnl4020Proximity[4], int (&rpmFuzzyCtrl)[2],
 
 }
 
-
+int error(){
+    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;
+}
 
 void LineFollow::calibrateZiegler(float KCrit, int rpmSpeed[2]){
     int targetSpeedL = 5;
     int targetSpeedR = 5;
-
-    int correctionSpeed = (int) (KCrit * delta());   
-    
-    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]);
+    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
diff --git a/devices/DiWheelDrive/main.cpp b/devices/DiWheelDrive/main.cpp
index 9ff3acc..f607cc0 100644
--- a/devices/DiWheelDrive/main.cpp
+++ b/devices/DiWheelDrive/main.cpp
@@ -754,7 +754,7 @@ void calibrateLineSensores(BaseSequentialStream *chp, int argc, char *argv[]) {
     // 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);
+  chprintf(chp,"Sensors Calibrated: MaxDelta: %d, FL: %d, FR: %d\n", maxDelta, proxyL, proxyR);
 
   global.threshProxyL = proxyL;
   global.threshProxyR = proxyR;
@@ -821,6 +821,7 @@ void zieglerMeth2(BaseSequentialStream *chp, int argc, char *argv[]) {
   int proxyR = global.threshProxyR;
   int maxDelta = 0;
   float KCrit = 0.0f;
+  global.sensSamples = 0;
   LineFollow lf(&global);
  
   if (argc == 2){
@@ -832,6 +833,8 @@ void zieglerMeth2(BaseSequentialStream *chp, int argc, char *argv[]) {
     chprintf(chp, "Usage: dev_ziegler2 <K_crit> <steps>");
     return;
   }
+  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);
   
   for(int s=0; s < steps; s++){
@@ -845,6 +848,69 @@ 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;
+  int speed = 0;
+  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();
+    }
+
+    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;
+    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],
+    //               vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT] - vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT]);
+    global.motorcontrol.setTargetRPM(speed * 1000000, -speed * 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++){
+    chprintf(chp,"FL:%d,FR:%d,Delta:%d\n",global.senseRec[j].FL, global.senseRec[j].FR, global.senseRec[j].delta);
+  }
+
+}
+
+
 static const ShellCommand commands[] = {
   {"shutdown", shellRequestShutdown},
   {"wakeup", shellRequestWakeup},
@@ -875,6 +941,8 @@ 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},
+  {"print_record", printMove},
   {NULL, NULL}
 };
 
