commit 25388c2f213b094b00213738f5aca6c2980f9474
Author: Georg Alberding <galberding@techfak.uni-bielefeld.de>
Date:   Thu Sep 26 12:57:26 2019 +0200

    Constants added

diff --git a/devices/DiWheelDrive/linefollow2.cpp b/devices/DiWheelDrive/linefollow2.cpp
index afc38fd..fe963dd 100644
--- a/devices/DiWheelDrive/linefollow2.cpp
+++ b/devices/DiWheelDrive/linefollow2.cpp
@@ -7,48 +7,148 @@ void LineFollow::printSensorData(){
     chprintf((BaseSequentialStream*) &SD1, "Test!");
 }
 
-void LineFollow::followLine(int vcnl4020Proximity[4], int (&rpmFuzzyCtrl)[2], Global *global){
+// void LineFollow::followLine(int vcnl4020Proximity[4], int (&rpmFuzzyCtrl)[2], Global *global){
     
-    chprintf((BaseSequentialStream*) &SD1, "SP: %d,\n", SetPoint);
+//     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 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] = 15  + new_speed;
-    rpmFuzzyCtrl[constants::DiWheelDrive::RIGHT_WHEEL] = 15  - new_speed;
-
-    chprintf((BaseSequentialStream*) &SD1, "Speed L: %d, R: %d\n", rpmFuzzyCtrl[constants::DiWheelDrive::LEFT_WHEEL], rpmFuzzyCtrl[constants::DiWheelDrive::RIGHT_WHEEL]);
+//     // 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);
+
+// }
+
+
+void LineFollow::stableFollow(int vcnl4020Proximity[4], int (&rpmFuzzyCtrl)[2], Global *global){
+    int targetSensor = 0x38;
+    int actualSensorL = vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT] >> 8;
+    int actualSensorR = vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT] >> 8;
+    int targetSpeedL = global->rpmForward[constants::DiWheelDrive::LEFT_WHEEL];
+    int targetSpeedR = global->rpmForward[constants::DiWheelDrive::RIGHT_WHEEL];
+
+    int error = targetSensor - (actualSensorL + actualSensorR);
+
+    accSum += error;
+    int dTerm = error - oldError;
+
+    int correctionSpeed = (int) (Kp * error + Ki * accSum + Kd * dTerm);   
+    chprintf((BaseSequentialStream*) &SD1, "Correction Speed: %d\n", correctionSpeed);
+    rpmFuzzyCtrl[constants::DiWheelDrive::LEFT_WHEEL] = targetSpeedL + correctionSpeed;
+    rpmFuzzyCtrl[constants::DiWheelDrive::RIGHT_WHEEL] = targetSpeedR - correctionSpeed;
 
 }
+
+
+// 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 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
diff --git a/devices/DiWheelDrive/linefollow2.hpp b/devices/DiWheelDrive/linefollow2.hpp
index 5b27193..ac80326 100644
--- a/devices/DiWheelDrive/linefollow2.hpp
+++ b/devices/DiWheelDrive/linefollow2.hpp
@@ -11,13 +11,14 @@ class LineFollow
 {
 public:
   void printSensorData();
-  void followLine(int vcnl4020Proximity[4], int (&rpmFuzzyCtrl)[2], Global *global);
+  void stableFollow(int vcnl4020Proximity[4], int (&rpmFuzzyCtrl)[2], Global *global);
   float SetPoint = 0x4000; // (0x1800+0x2800) >> 8
   float Kp = 0.001;
   float Ki = 0.00001;
-  float Kd = 0.1;
-  float acc_sum = 0;
-  float old_error = 0;
+  float Kd = 0.5
+  ;
+  int accSum = 0;
+  float oldError = 0;
 
 };
 
diff --git a/devices/DiWheelDrive/userthread.cpp b/devices/DiWheelDrive/userthread.cpp
index 9215d64..35c8c58 100644
--- a/devices/DiWheelDrive/userthread.cpp
+++ b/devices/DiWheelDrive/userthread.cpp
@@ -452,7 +452,7 @@ UserThread::main()
                 vcnl4020AmbientLight[i] = global.vcnl4020[i].getAmbientLight();
                 vcnl4020Proximity[i] = global.vcnl4020[i].getProximityScaledWoOffset();
             }
-			lf.followLine(vcnl4020Proximity, rpmFuzzyCtrl, &global);
+			lf.stableFollow(vcnl4020Proximity, rpmFuzzyCtrl, &global);
             // chprintf((BaseSequentialStream*) &SD1, "0x%04X 0x%04X 0x%04X 0x%04X\n",
             //         vcnl4020Proximity[constants::DiWheelDrive::PROX_WHEEL_LEFT],
             //         vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT],
@@ -462,7 +462,7 @@ UserThread::main()
 	//   lineFollownew
 	//else
             // lineFollowing(vcnl4020Proximity, rpmFuzzyCtrl, &global);
-            // setRpmSpeed(rpmFuzzyCtrl);
+            setRpmSpeed(rpmFuzzyCtrl);
         }
 
 		// this->sleep(US2ST(5));
diff --git a/include/amiro/Constants.h b/include/amiro/Constants.h
index 3673285..bd6d30d 100644
--- a/include/amiro/Constants.h
+++ b/include/amiro/Constants.h
@@ -63,6 +63,7 @@ namespace CAN {
   const uint32_t BRIGHTNESS_ID             = 0x40;
   inline constexpr uint32_t COLOR_ID(uint32_t index)             {return 0x38 | ((index) & 0x7);}
   inline constexpr uint32_t PROXIMITY_RING_ID(uint32_t index)    {return 0x30 | ((index) & 0x7);}
+  const uint32_t SET_LINE_FOLLOW_SPEED     = 0x23;
   const uint32_t SET_KINEMATIC_CONST_ID    = 0x22;
   const uint32_t TARGET_POSITION_ID        = 0x21;
   const uint32_t ACTUAL_SPEED_ID           = 0x20;
