AMiRo-OS

#ifndef AMIRO_LINEFOLLOWING_H

#define AMIRO_LINEFOLLOWING_H

#include <ch.hpp>

#include "global.hpp"

#include <amiroosconf.h>

#define RAND_TRESH 16000

#define MAX_CORRECTED_SPEED 1000000*100

namespace amiro {

  enum LineFollowStrategy{

  EDGE_LEFT,      // driving on the left edge of a black line

  TRANSITION_L_R, // Transition from left to right edge

  TRANSITION_R_L, // transition from right to left edge

  EDGE_RIGHT,     // driving on the right edge of a black line

  REVERSE,

  MIDDLE,         // not working, use FUZZY instead

  FUZZY,

  NONE

  };

enum colorMember : uint8_t {

        BLACK=0,

        GREY=1,

        WHITE=2

};

class LineFollow

{

public:

  int biggestDiff = 0;

  Global *global;

  LineFollow(Global *global);

  LineFollow(Global *global, LineFollowStrategy strategy);

  /**

   * Entry method which should be called to follow a line.

   * 

   * @param rpmSpeed speed that will be determained

   * @return white flag, 1 if white is detected  

   */

  int followLine(int (&rpmSpeed)[2]);

  /**

   * Setter for LineFollowStrategy which triggers transition behavior.

   * 

   * To prevent random turns while changeing the strategy a transition state is set.

   * In case the strategy needs to be switched immediately use promptStrategyChange().

   * 

   * @param strategy 

   */

  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:

  /**

   * Calculate the error from front sensors.

   */

  int getError();

  /**

   * Error calculation while changing from one EDGE_* strategy to another.

   * This prevents the AMiRo from random turns while switching strategies.

   * 

   * @param FL value of left front sensor

   * @param FR value of right front sensor

   * @param targetL left threshold 

   * @param targetR right threshold

   */

  int transitionError(int FL, int FR, int targetL, int targetR);

  /**

   * Use the error according to the strategy to calculate the correction speed.

   * Currently only the P part of the PID controller is used to calculate the 

   * correction speed.

   */

  int getPidCorrectionSpeed();

  // Fuzzy line following methods--------------

  void lineFollowing(int (&proximity)[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]);

  int vcnl4020AmbientLight[4] = {0};

  int vcnl4020Proximity[4] = {0};

  // -----------------------------------------

  LineFollowStrategy strategy = LineFollowStrategy::EDGE_RIGHT;

  char whiteFlag = 0;

  int trans = 0;

  // PID controller components ---------------

  int32_t K_p = 1900;

  float K_i = 0.15;

  float K_d = 0.6;

  int32_t accumHist = 0;

  int32_t oldError = 0;

  // ----------------------------------------

};

} // end of namespace amiro

#endif // AMIRO_LINEFOLLOWING_H