commit 726fdc72a9b924ee4bf84ceda9c3050a70b66d52
Author: galberding <galberding@techfak.uni-bielefeld.de>
Date:   Tue Nov 19 16:14:04 2019 +0100

    Add line following

diff --git a/devices/DiWheelDrive/linefollow.cpp b/devices/DiWheelDrive/linefollow.cpp
new file mode 100644
index 0000000..28f9b3a
--- /dev/null
+++ b/devices/DiWheelDrive/linefollow.cpp
@@ -0,0 +1,419 @@
+#include "global.hpp"
+#include "linefollow2.hpp" 
+#include <cmath>
+
+
+
+LineFollow::LineFollow(Global *global){
+    this->global = global;
+}
+LineFollow::LineFollow(Global *global, LineFollowStrategy strategy){
+    this->global = global;
+    this-> strategy = strategy;
+}
+
+
+int LineFollow::transitionError(int FL, int FR, int targetL, int targetR){
+    // global->robot.setLightColor(0, Color::RED);
+    // global->robot.setLightColor(7, Color::RED);
+    int error = 0;
+
+    switch (this->strategy)
+    {
+    case LineFollowStrategy::TRANSITION_R_L:
+        error = -(FL -targetL + FR - targetR + this->trans);
+        break;
+    case LineFollowStrategy::TRANSITION_L_R:
+        error = (FL -targetL + FR - targetR + this->trans);
+        break;
+    default:
+        break;
+    }
+    this->trans += 400;
+    if(FL+FR <= RAND_TRESH){
+      // global->robot.setLightColor(0, Color::GREEN);
+      // global->robot.setLightColor(7, Color::GREEN);
+      switch (this->strategy)
+      {
+        case LineFollowStrategy::TRANSITION_R_L:
+            this->strategy = LineFollowStrategy::EDGE_LEFT;
+            break;
+        case LineFollowStrategy::TRANSITION_L_R:
+            this->strategy = LineFollowStrategy::EDGE_RIGHT;
+            break;
+        default:
+            break;
+      }
+      this->trans = 0;
+    }
+  return error;
+}
+
+/**
+ * Calculate the error from front proxi sensors and fixed threshold values for those sensors.
+ */
+int LineFollow::getError(){
+  // global->robot.setLightColor(3, Color::YELLOW);
+    // Get actual sensor data of both front sensors
+    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;
+    int error = 0;
+    switch (this->strategy)
+    {
+    case LineFollowStrategy::EDGE_RIGHT:
+        error = -(FL -targetL + FR - targetR);
+        break;
+    case LineFollowStrategy::EDGE_LEFT:
+        error = (FL -targetL + FR - targetR);
+        break;
+    case LineFollowStrategy::MIDDLE:
+        // Assume that the smallest value means driving in the middle
+        targetL = targetR = !(targetL<targetR)?targetR:targetL;
+        error = (FL -targetL + FR - targetR);
+        break;
+    case LineFollowStrategy::TRANSITION_L_R: case LineFollowStrategy::TRANSITION_R_L:
+      error = transitionError(FL, FR, targetL, targetR);
+      break;
+    default:
+        break;
+    }
+    // Debugging stuff ------
+    // if (global->enableRecord){
+    //     global->senseRec[global->sensSamples].error = error;
+    //     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++;
+    //     }
+    // ----------------------
+    // Register white values
+    if (FL+FR > global->threshWhite){
+        whiteFlag = 1;
+    }else{
+        whiteFlag = 0;
+    }
+    return error;
+}
+
+
+
+
+/**
+ * Depending on the strategy different behaviours will be triggered. 
+ *  FUZZY - standard tracking of black area 
+ *  REVERSE - drive back
+ *  @param: rpmSpeed motor speed 
+ */
+int LineFollow::followLine(int (&rpmSpeed)[2]){
+  
+    int correctionSpeed = 0;
+    switch (this->strategy)
+    {
+    case LineFollowStrategy::FUZZY:
+      for (int i = 0; i < 4; i++) {
+          vcnl4020AmbientLight[i] = global->vcnl4020[i].getAmbientLight();
+          vcnl4020Proximity[i] = global->vcnl4020[i].getProximityScaledWoOffset();
+      }
+      lineFollowing(vcnl4020Proximity, rpmSpeed);
+      break;
+
+    case LineFollowStrategy::REVERSE:
+      correctionSpeed = -getPidCorrectionSpeed();
+      rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] = -global->forwardSpeed;
+
+      rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] = -global->forwardSpeed;
+      
+      break;
+
+    default:
+      correctionSpeed = getPidCorrectionSpeed();
+      // chprintf((BaseSequentialStream*) &SD1, "Correction: %d, thresh: %d\n",correctionSpeed,  global->threshWhite);
+
+      rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] =   global->forwardSpeed + correctionSpeed;
+
+      rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] = global->forwardSpeed - correctionSpeed;
+      break;
+    }
+    return whiteFlag;
+}
+
+
+/**
+ * Pid controller which returns a corrections speed.
+ */
+int LineFollow::getPidCorrectionSpeed(){
+    int error = getError();
+    int sloap = oldError - error ;
+    // int correctionSpeed = (int) (global->K_p*error + Ki*accumHist - global->K_d*sloap);
+    int correctionSpeed = (int) (K_p*error + K_i*accumHist - K_d*sloap);
+    oldError = error;
+    // accumHist += (int) (0.01 * error);
+    if (abs(error) > global->maxDist.error){
+        global->maxDist.error = error;
+    }
+    return correctionSpeed;
+}
+
+
+void LineFollow::setStrategy(LineFollowStrategy strategy){
+
+  if(this->strategy == LineFollowStrategy::TRANSITION_R_L || this->strategy == LineFollowStrategy::TRANSITION_L_R){
+    return;
+  }
+
+  switch(strategy){
+    case LineFollowStrategy::EDGE_LEFT:
+      if((this->strategy == LineFollowStrategy::EDGE_RIGHT) || (this->strategy == LineFollowStrategy::TRANSITION_R_L)){
+        this->strategy = LineFollowStrategy::TRANSITION_R_L;
+      }else{
+        // In case of fuzzy or reverse
+        this->strategy = strategy;
+      }
+      break;
+    case LineFollowStrategy::EDGE_RIGHT:
+    if((this->strategy == LineFollowStrategy::EDGE_LEFT) || (this->strategy == LineFollowStrategy::TRANSITION_L_R)){
+        this->strategy = LineFollowStrategy::TRANSITION_L_R;
+      }else{
+        // In case of fuzzy or reverse
+        this->strategy = strategy;
+      }
+      break;
+    default:
+    // From Fuzzy or Reverse state should work to transition automatically
+      this->strategy = strategy;
+      break;
+  }
+  // this->strategy = strategy;
+}
+
+void LineFollow::promptStrategyChange(LineFollowStrategy strategy){
+  this->strategy = strategy;
+}
+
+LineFollowStrategy LineFollow::getStrategy(){
+      return this->strategy;
+}
+void LineFollow::setGains(float Kp, float Ki, float Kd){
+    this->K_p = Kp;
+    this->K_i = Ki;
+    this->K_d = Kd;
+}
+
+
+
+
+
+// Legacy code, fuzzy following-----------------------------------------
+// Line following by a fuzzy controler
+void LineFollow::lineFollowing(int (&proximity)[4], int (&rpmFuzzyCtrl)[2]) {
+  // FUZZYFICATION
+  // First we need to get the fuzzy value for our 3 values {BLACK, GREY, WHITE}
+  float leftWheelFuzzyMemberValues[3], leftFrontFuzzyMemberValues[3], rightFrontFuzzyMemberValues[3], rightWheelFuzzyMemberValues[3];
+  fuzzyfication(proximity[constants::DiWheelDrive::PROX_WHEEL_LEFT], leftWheelFuzzyMemberValues);
+  fuzzyfication(proximity[constants::DiWheelDrive::PROX_FRONT_LEFT], leftFrontFuzzyMemberValues);
+  fuzzyfication(proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT], rightFrontFuzzyMemberValues);
+  fuzzyfication(proximity[constants::DiWheelDrive::PROX_WHEEL_RIGHT], rightWheelFuzzyMemberValues);
+
+  // INFERENCE RULE DEFINITION
+  // Get the member for each sensor
+  colorMember member[4];
+  member[constants::DiWheelDrive::PROX_WHEEL_LEFT] = getMember(leftWheelFuzzyMemberValues);
+  member[constants::DiWheelDrive::PROX_FRONT_LEFT] = getMember(leftFrontFuzzyMemberValues);
+  member[constants::DiWheelDrive::PROX_FRONT_RIGHT] = getMember(rightFrontFuzzyMemberValues);
+  member[constants::DiWheelDrive::PROX_WHEEL_RIGHT] = getMember(rightWheelFuzzyMemberValues);
+
+  // visualize sensors via LEDs
+  global->robot.setLightColor(constants::LightRing::LED_WNW, memberToLed(member[constants::DiWheelDrive::PROX_WHEEL_LEFT]));
+  global->robot.setLightColor(constants::LightRing::LED_NNW, memberToLed(member[constants::DiWheelDrive::PROX_FRONT_LEFT]));
+  global->robot.setLightColor(constants::LightRing::LED_NNE, memberToLed(member[constants::DiWheelDrive::PROX_FRONT_RIGHT]));
+  global->robot.setLightColor(constants::LightRing::LED_ENE, memberToLed(member[constants::DiWheelDrive::PROX_WHEEL_RIGHT]));
+
+  // chprintf((BaseSequentialStream*) &SD1, "Left: BLACK: %f, GREY: %f, WHITE: %f\r\n", leftWheelFuzzyMemberValues[BLACK], leftWheelFuzzyMemberValues[GREY], leftWheelFuzzyMemberValues[WHITE]);
+  // chprintf((BaseSequentialStream*) &SD1, "Right: BLACK: %f, GREY: %f, WHITE: %f\r\n", rightFuzzyMemberValues[BLACK], rightFuzzyMemberValues[GREY], rightFuzzyMemberValues[WHITE]);
+
+  // DEFUZZYFICATION
+  defuzzyfication(member, rpmFuzzyCtrl);
+  // defuzz(member, rpmFuzzyCtrl);
+}
+
+
+Color LineFollow::memberToLed(colorMember member) {
+  switch (member) {
+    case BLACK:
+      return Color(Color::GREEN);
+    case GREY:
+      return Color(Color::YELLOW);
+    case WHITE:
+      return Color(Color::RED);
+    default:
+      return Color(Color::WHITE);
+  }
+}
+
+void LineFollow::defuzzyfication(colorMember (&member)[4], int (&rpmFuzzyCtrl)[2]) {
+    whiteFlag = 0;
+  // all sensors are equal
+  if (member[constants::DiWheelDrive::PROX_WHEEL_LEFT] == member[constants::DiWheelDrive::PROX_FRONT_LEFT] &&
+      member[constants::DiWheelDrive::PROX_FRONT_LEFT] == member[constants::DiWheelDrive::PROX_FRONT_RIGHT] &&
+      member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == member[constants::DiWheelDrive::PROX_WHEEL_RIGHT]) {
+    // something is wrong -> stop
+    copyRpmSpeed(rpmHalt, rpmFuzzyCtrl);
+  // both front sensor detect a line
+  } else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == BLACK &&
+      member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == BLACK) {
+    // straight
+    copyRpmSpeed(global->rpmForward, rpmFuzzyCtrl);
+  // exact one front sensor detects a line
+  } else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == BLACK ||
+             member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == BLACK) {
+    // soft correction
+    if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == GREY) {
+      // soft right
+      copyRpmSpeed(global->rpmSoftRight, rpmFuzzyCtrl);
+    } else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == WHITE) {
+      // hard right
+      copyRpmSpeed(global->rpmHardRight, rpmFuzzyCtrl);
+    } else if (member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == GREY) {
+      // soft left
+      copyRpmSpeed(global->rpmSoftLeft, rpmFuzzyCtrl);
+    } else if (member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == WHITE) {
+      // hard left
+      copyRpmSpeed(global->rpmHardLeft, rpmFuzzyCtrl);
+    }
+  // both wheel sensors detect a line
+  } else if (member[constants::DiWheelDrive::PROX_WHEEL_LEFT] == BLACK &&
+             member[constants::DiWheelDrive::PROX_WHEEL_RIGHT] == BLACK) {
+    // something is wrong -> stop
+    copyRpmSpeed(rpmHalt, rpmFuzzyCtrl);
+  // exactly one wheel sensor detects a line
+  } else if (member[constants::DiWheelDrive::PROX_WHEEL_LEFT] == BLACK ||
+             member[constants::DiWheelDrive::PROX_WHEEL_RIGHT] == BLACK) {
+    if (member[constants::DiWheelDrive::PROX_WHEEL_LEFT] == BLACK) {
+      // turn left
+      copyRpmSpeed(rpmTurnLeft, rpmFuzzyCtrl);
+    } else if (member[constants::DiWheelDrive::PROX_WHEEL_RIGHT] == BLACK) {
+      // turn right
+      copyRpmSpeed(rpmTurnRight, rpmFuzzyCtrl);
+    }
+  // both front sensors may detect a line
+  } else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == GREY &&
+             member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == GREY) {
+    if (member[constants::DiWheelDrive::PROX_WHEEL_LEFT] == GREY) {
+      // turn left
+      copyRpmSpeed(rpmTurnLeft, rpmFuzzyCtrl);
+    } else if (member[constants::DiWheelDrive::PROX_WHEEL_RIGHT] == GREY) {
+      // turn right
+      copyRpmSpeed(rpmTurnRight, rpmFuzzyCtrl);
+    }
+  // exactly one front sensor may detect a line
+  } else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == GREY ||
+             member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == GREY) {
+    if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == GREY) {
+      // turn left
+      copyRpmSpeed(rpmTurnLeft, rpmFuzzyCtrl);
+    } else if (member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == GREY) {
+      // turn right
+      copyRpmSpeed(rpmTurnRight, rpmFuzzyCtrl);
+    }
+  // both wheel sensors may detect a line
+  } else if (member[constants::DiWheelDrive::PROX_WHEEL_LEFT] == GREY &&
+             member[constants::DiWheelDrive::PROX_WHEEL_RIGHT] == GREY) {
+    // something is wrong -> stop
+    copyRpmSpeed(rpmHalt, rpmFuzzyCtrl);
+  // exactly one wheel sensor may detect a line
+  } else if (member[constants::DiWheelDrive::PROX_WHEEL_LEFT] == GREY ||
+             member[constants::DiWheelDrive::PROX_WHEEL_RIGHT] == GREY) {
+    if (member[constants::DiWheelDrive::PROX_WHEEL_LEFT] == GREY) {
+      // turn left
+      copyRpmSpeed(rpmTurnLeft, rpmFuzzyCtrl);
+    } else if (member[constants::DiWheelDrive::PROX_WHEEL_RIGHT] == GREY) {
+      // turn right
+      copyRpmSpeed(rpmTurnRight, rpmFuzzyCtrl);
+    }
+  // no sensor detects anything 
+  } else {
+    // line is lost -> stop
+    whiteFlag = 1;
+    copyRpmSpeed(rpmHalt, rpmFuzzyCtrl);
+  }
+    chprintf((BaseSequentialStream*) &SD1, "Fuzzy Speed: Left: %d, Right: %d\n", rpmFuzzyCtrl[0], rpmFuzzyCtrl[1]);
+  return;
+}
+
+colorMember LineFollow::getMember(float (&fuzzyValue)[3]) {
+  colorMember member;
+
+  if (fuzzyValue[BLACK] > fuzzyValue[GREY])
+    if (fuzzyValue[BLACK] > fuzzyValue[WHITE])
+      member = BLACK;
+    else
+      member = WHITE;
+  else
+    if (fuzzyValue[GREY] > fuzzyValue[WHITE])
+      member = GREY;
+    else
+      member = WHITE;
+
+  return member;
+}
+
+// Fuzzyfication of the sensor values
+void LineFollow::fuzzyfication(int sensorValue, float (&fuzziedValue)[3]) {
+  if (sensorValue < blackStartFalling ) {
+    // Only black value
+    fuzziedValue[BLACK] = 1.0f;
+    fuzziedValue[GREY] = 0.0f;
+    fuzziedValue[WHITE] = 0.0f;
+  } else if (sensorValue > whiteOn ) {
+    // Only white value
+    fuzziedValue[BLACK] = 0.0f;
+    fuzziedValue[GREY] = 0.0f;
+    fuzziedValue[WHITE] = 1.0f;
+  } else if ( sensorValue < greyMax) {
+    // Some greyisch value between black and grey
+
+    // Black is going down
+    if ( sensorValue > blackOff) {
+      fuzziedValue[BLACK] = 0.0f;
+    } else {
+      fuzziedValue[BLACK] = static_cast<float>(sensorValue-blackOff) / (blackStartFalling-blackOff);
+    }
+
+    // Grey is going up
+    if ( sensorValue < greyStartRising) {
+      fuzziedValue[GREY] = 0.0f;
+    } else {
+      fuzziedValue[GREY] = static_cast<float>(sensorValue-greyStartRising) / (greyMax-greyStartRising);
+    }
+
+    // White is absent
+    fuzziedValue[WHITE] = 0.0f;
+
+  } else if ( sensorValue >= greyMax) {
+    // Some greyisch value between grey white
+
+    // Black is absent
+    fuzziedValue[BLACK] = 0.0f;
+
+    // Grey is going down
+    if ( sensorValue < greyOff) {
+      fuzziedValue[GREY] = static_cast<float>(sensorValue-greyOff) / (greyMax-greyOff);
+    } else {
+      fuzziedValue[GREY] = 0.0f;
+    }
+
+    // White is going up
+    if ( sensorValue < whiteStartRising) {
+      fuzziedValue[WHITE] = 0.0f;
+    } else {
+      fuzziedValue[WHITE] = static_cast<float>(sensorValue-whiteStartRising) / (whiteOn-whiteStartRising);
+    }
+  }
+}
+
+void LineFollow::copyRpmSpeed(const int (&source)[2], int (&target)[2]) {
+  target[constants::DiWheelDrive::LEFT_WHEEL] = source[constants::DiWheelDrive::LEFT_WHEEL];
+  target[constants::DiWheelDrive::RIGHT_WHEEL] = source[constants::DiWheelDrive::RIGHT_WHEEL];
+  // chprintf((BaseSequentialStream*) &SD1, "Speed left: %d, Speed right: %d\r\n", target[0], target[1]);
+}
\ No newline at end of file
diff --git a/devices/DiWheelDrive/linefollow.hpp b/devices/DiWheelDrive/linefollow.hpp
new file mode 100644
index 0000000..b1f8323
--- /dev/null
+++ b/devices/DiWheelDrive/linefollow.hpp
@@ -0,0 +1,95 @@
+#ifndef AMIRO_LINEFOLLOWING_H
+#define AMIRO_LINEFOLLOWING_H
+
+#include <ch.hpp>
+#include "global.hpp"
+#include <amiroosconf.h>
+
+#define RAND_TRESH 16000
+
+namespace amiro {
+  
+  enum LineFollowStrategy{
+  EDGE_LEFT,
+  TRANSITION_L_R,
+  TRANSITION_R_L,
+  EDGE_RIGHT,
+  REVERSE,
+  MIDDLE,
+  FUZZY,
+  NONE
+  };
+
+enum colorMember : uint8_t {
+	BLACK=0,
+	GREY=1,
+	WHITE=2
+};
+
+class LineFollow
+{
+public:
+
+  // TODO: Documentation
+  int biggestDiff = 0;
+  Global *global;
+  LineFollow(Global *global);
+  LineFollow(Global *global, LineFollowStrategy strategy);
+  int followLine(int (&rpmSpeed)[2]);
+  void setStrategy(LineFollowStrategy strategy);
+  void promptStrategyChange(LineFollowStrategy strategy);
+  LineFollowStrategy getStrategy();
+  void setGains(float Kp, float Ki, float Kd);
+
+
+  const int rpmTurnLeft[2] = {-10, 10};
+  const int rpmTurnRight[2] = {rpmTurnLeft[1],rpmTurnLeft[0]};
+  const int rpmHalt[2] = {0, 0};
+  // Definition of the fuzzyfication function
+  //  | Membership
+  // 1|_B__   G    __W__
+  //  |    \  /\  /
+  //  |     \/  \/
+  //  |_____/\__/\______ Sensor values
+  // SEE MATLAB SCRIPT "fuzzyRule.m" for adjusting the values
+  // All values are "raw sensor values"
+  /* Use these values for white ground surface (e.g. paper) */
+
+  const int blackStartFalling = 0x1000; // Where the black curve starts falling
+  const int blackOff = 0x1800; // Where no more black is detected
+  const int whiteStartRising = 0x2800; // Where the white curve starts rising
+  const int whiteOn = 0x6000; // Where the white curve has reached the maximum value
+  const int greyMax = (whiteOn + blackStartFalling) / 2; // Where grey has its maximum
+  const int greyStartRising = blackStartFalling; // Where grey starts rising
+  const int greyOff = whiteOn; // Where grey is completely off again
+
+  private:
+    int delta();
+    int getError();
+    int transitionError(int FL, int FR, int targetL, int targetR);
+    int getPidCorrectionSpeed();
+    void lineFollowing(int (&proximity)[4], int (&rpmFuzzyCtrl)[2]);
+    // void defuzz(colorMember (&member)[4], int (&rpmFuzzyCtrl)[2]);
+    Color memberToLed(colorMember member);
+    void defuzzyfication(colorMember (&member)[4], int (&rpmFuzzyCtrl)[2]);
+    colorMember getMember(float (&fuzzyValue)[3]);
+    void fuzzyfication(int sensorValue, float (&fuzziedValue)[3]);
+    void copyRpmSpeed(const int (&source)[2], int (&target)[2]);
+
+    char whiteFlag = 0;
+    LineFollowStrategy strategy = LineFollowStrategy::EDGE_RIGHT;
+    float K_p = 0.002;
+    float K_i = 0;
+    float K_d = 0;
+    int accumHist = 0;
+    float oldError = 0;
+    int trans = 0;
+    int vcnl4020AmbientLight[4] = {0};
+    int vcnl4020Proximity[4] = {0};
+};
+
+
+
+} // end of namespace amiro
+
+#endif // AMIRO_LINEFOLLOWING_H
