AMiRo-OS

#ifndef AMIRO_LINEFOLLOWING_H

#define AMIRO_LINEFOLLOWING_H

#include <ch.hpp>

#include "global.hpp"

#include <amiroosconf.h>

namespace amiro {

  enum class LineFollowStrategy{

  EDGE_LEFT,

  EDGE_RIGHT,

  DOCKING,

  MIDDLE,

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

  // void calibrateZiegler(float KCrit, int rpmSpeed[2]);

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

  // int followLeftEdge(int rpmSpeed[2]);

  // int followRightEdge(int rpmSpeed[2]);

  // int followMiddle(int rpmSpeed[2]);

  void setStrategy(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 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 Kp = 0.002;

    float Ki = 0;

    float Kd = 0;

    int accumHist = 0;

    float oldError = 0;

    int vcnl4020AmbientLight[4] = {0};

    int vcnl4020Proximity[4] = {0};

};

} // end of namespace amiro

#endif // AMIRO_LINEFOLLOWING_H