AMiRo-OS

#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]);

}

#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