Revision d314ad6f devices/DiWheelDrive/linefollow.hpp

View differences:

devices/DiWheelDrive/linefollow.hpp
10 10 
namespace amiro {
11 11
12 12 
  enum LineFollowStrategy{
13 
  EDGE_LEFT,
14 
  TRANSITION_L_R,
15 
  TRANSITION_R_L,
16 
  EDGE_RIGHT,
13 
  EDGE_LEFT,      // driving on the left edge of a black line
14 
  TRANSITION_L_R, // Transition from left to right edge
15 
  TRANSITION_R_L, // transition from right to left edge
16 
  EDGE_RIGHT,     // driving on the right edge of a black line
17 17 
  REVERSE,
18 
  MIDDLE,
18 
  MIDDLE,         // not working, use FUZZY instead
19 19 
  FUZZY,
20 20 
  NONE
21 21 
  };
......
30 30 
{
31 31 
public:
32 32 

  		




  33
  
  
    
  // TODO: Documentation


  		




  34
  33
  
    
  int biggestDiff = 0;


  		




  35
  34
  
    
  Global *global;


  		




  36
  35
  
    
  LineFollow(Global *global);


  		




  37
  36
  
    
  LineFollow(Global *global, LineFollowStrategy strategy);


  		




  
  37
  
    
  /**


  		




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


  		




  
  39
  
    
   * 


  		




  
  40
  
    
   * @param rpmSpeed speed that will be determained


  		




  
  41
  
    
   * @return white flag, 1 if white is detected  


  		




  
  42
  
    
   */


  		




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


  		




  
  44
  
    

  		




  
  45
  
    
  /**


  		




  
  46
  
    
   * Setter for LineFollowStrategy which triggers transition behavior.


  		




  
  47
  
    
   * 


  		




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


  		




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


  		




  
  50
  
    
   * 


  		




  
  51
  
    
   * @param strategy 


  		




  
  52
  
    
   */


  		




  39
  53
  
    
  void setStrategy(LineFollowStrategy strategy);


  		




  40
  54
  
    
  void promptStrategyChange(LineFollowStrategy strategy);


  		




  41
  55
  
    
  LineFollowStrategy getStrategy();


  		




  ......




  63
  77
  
    
  const int greyStartRising = blackStartFalling; // Where grey starts rising


  		




  64
  78
  
    
  const int greyOff = whiteOn; // Where grey is completely off again


  		




  65
  79
  
    

  		




  66
  
  
    
  private:


  		




  67
  
  
    
    int delta();


  		




  68
  
  
    
    int getError();


  		




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


  		




  70
  
  
    
    int getPidCorrectionSpeed();


  		




  71
  
  
    
    void lineFollowing(int (&proximity)[4], int (&rpmFuzzyCtrl)[2]);


  		




  72
  
  
    
    // void defuzz(colorMember (&member)[4], int (&rpmFuzzyCtrl)[2]);


  		




  73
  
  
    
    Color memberToLed(colorMember member);


  		




  74
  
  
    
    void defuzzyfication(colorMember (&member)[4], int (&rpmFuzzyCtrl)[2]);


  		




  75
  
  
    
    colorMember getMember(float (&fuzzyValue)[3]);


  		




  76
  
  
    
    void fuzzyfication(int sensorValue, float (&fuzziedValue)[3]);


  		




  77
  
  
    
    void copyRpmSpeed(const int (&source)[2], int (&target)[2]);


  		




  78
  
  
    

  		




  79
  
  
    
    char whiteFlag = 0;


  		




  80
  
  
    
    LineFollowStrategy strategy = LineFollowStrategy::EDGE_RIGHT;


  		




  81
  
  
    
    float K_p = 0.002;


  		




  82
  
  
    
    float K_i = 0;


  		




  83
  
  
    
    float K_d = 0;


  		




  84
  
  
    
    int accumHist = 0;


  		




  85
  
  
    
    float oldError = 0;


  		




  86
  
  
    
    int trans = 0;


  		




  87
  
  
    
    int vcnl4020AmbientLight[4] = {0};


  		




  88
  
  
    
    int vcnl4020Proximity[4] = {0};


  		




  
  80
  
    
private:


  		




  
  81
  
    
  /**


  		




  
  82
  
    
   * Calculate the error from front sensors.


  		




  
  83
  
    
   */


  		




  
  84
  
    
  int getError();


  		




  
  85
  
    

  		




  
  86
  
    
  /**


  		




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


  		




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


  		




  
  89
  
    
   * 


  		




  
  90
  
    
   * @param FL value of left front sensor


  		




  
  91
  
    
   * @param FR value of right front sensor


  		




  
  92
  
    
   * @param targetL left threshold 


  		




  
  93
  
    
   * @param targetR right threshold


  		




  
  94
  
    
   */


  		




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


  		




  
  96
  
    

  		




  
  97
  
    
  /**


  		




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


  		




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


  		




  
  100
  
    
   * correction speed.


  		




  
  101
  
    
   */


  		




  
  102
  
    
  int getPidCorrectionSpeed();


  		




  
  103
  
    

  		




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


  		




  
  105
  
    
  void lineFollowing(int (&proximity)[4], int (&rpmFuzzyCtrl)[2]);


  		




  
  106
  
    
  Color memberToLed(colorMember member);


  		




  
  107
  
    
  void defuzzyfication(colorMember (&member)[4], int (&rpmFuzzyCtrl)[2]);


  		




  
  108
  
    
  colorMember getMember(float (&fuzzyValue)[3]);


  		




  
  109
  
    
  void fuzzyfication(int sensorValue, float (&fuzziedValue)[3]);


  		




  
  110
  
    
  void copyRpmSpeed(const int (&source)[2], int (&target)[2]);


  		




  
  111
  
    
  int vcnl4020AmbientLight[4] = {0};


  		




  
  112
  
    
  int vcnl4020Proximity[4] = {0};


  		




  
  113
  
    
  // -----------------------------------------


  		




  
  114
  
    
  LineFollowStrategy strategy = LineFollowStrategy::EDGE_RIGHT;


  		




  
  115
  
    
  char whiteFlag = 0;


  		




  
  116
  
    
  int trans = 0;


  		




  
  117
  
    

  		




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


  		




  
  119
  
    
  float K_p = 0.002;


  		




  
  120
  
    
  float K_i = 0;


  		




  
  121
  
    
  float K_d = 0;


  		




  
  122
  
    
  int accumHist = 0;


  		




  
  123
  
    
  float oldError = 0;


  		




  
  124
  
    
  // ----------------------------------------


  		




  89
  125
  
    
};


  		




  90
  126
  
    

  		




  91
  127
  
    

  		












Also available in:  Unified diff