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
