Revision 1b3adcdd devices/DiWheelDrive/linefollow2.hpp

View differences:

devices/DiWheelDrive/linefollow2.hpp
6 6
#include <amiroosconf.h>
7 7

  
8 8
namespace amiro {
9
  enum class LineFollowStrategy{
10
  EDGE_LEFT,
11
  EDGE_RIGHT,
12
  MIDDLE,
13
  FUZZY
14
};
15

  
16
enum colorMember : uint8_t {
17
	BLACK=0,
18
	GREY=1,
19
	WHITE=2
20
};
9 21

  
10 22
class LineFollow
11 23
{
12 24
public:
13
  void printSensorData();
14
  void stableFollow(int vcnl4020Proximity[4], int (&rpmFuzzyCtrl)[2], Global *global);
15
  float SetPoint = 0x4000; // (0x1800+0x2800) >> 8
16
  float Kp = 0.001;
17
  float Ki = 0.00001;
18
  float Kd = 0.5;
19
  int accSum = 0;
20
  float oldError = 0;
25

  
26
  
21 27
  int biggestDiff = 0;
22 28
  Global *global;
23 29
  LineFollow(Global *global);
30
  LineFollow(Global *global, LineFollowStrategy strategy);
24 31
  // void calibrateZiegler(float KCrit, int rpmSpeed[2]);
25
  int followLeftEdge(int rpmSpeed[2]);
26
  int followRightEdge(int rpmSpeed[2]);
32
  int followLine(int (&rpmSpeed)[2]);
33
  // int followLeftEdge(int rpmSpeed[2]);
34
  // int followRightEdge(int rpmSpeed[2]);
35
  // int followMiddle(int rpmSpeed[2]);
36
  void setStrategy(LineFollowStrategy strategy);
37
  LineFollowStrategy getStrategy();
38
  void setGains(float Kp, float Ki, float Kd);
39

  
40

  
41
  const int rpmTurnLeft[2] = {-10, 10};
42
  const int rpmTurnRight[2] = {rpmTurnLeft[1],rpmTurnLeft[0]};
43
  const int rpmHalt[2] = {0, 0};
44
  // Definition of the fuzzyfication function
45
  //  | Membership
46
  // 1|_B__   G    __W__
47
  //  |    \  /\  /
48
  //  |     \/  \/
49
  //  |_____/\__/\______ Sensor values
50
  // SEE MATLAB SCRIPT "fuzzyRule.m" for adjusting the values
51
  // All values are "raw sensor values"
52
  /* Use these values for white ground surface (e.g. paper) */
53

  
54
  const int blackStartFalling = 0x1000; // Where the black curve starts falling
55
  const int blackOff = 0x1800; // Where no more black is detected
56
  const int whiteStartRising = 0x2800; // Where the white curve starts rising
57
  const int whiteOn = 0x6000; // Where the white curve has reached the maximum value
58
  const int greyMax = (whiteOn + blackStartFalling) / 2; // Where grey has its maximum
59
  const int greyStartRising = blackStartFalling; // Where grey starts rising
60
  const int greyOff = whiteOn; // Where grey is completely off again
27 61

  
28 62
  private:
29 63
    int delta();
30 64
    int getError();
31 65
    int getPidCorrectionSpeed();
66
    void lineFollowing(int (&proximity)[4], int (&rpmFuzzyCtrl)[2]);
67
    // void defuzz(colorMember (&member)[4], int (&rpmFuzzyCtrl)[2]);
68
    Color memberToLed(colorMember member);
69
    void defuzzyfication(colorMember (&member)[4], int (&rpmFuzzyCtrl)[2]);
70
    colorMember getMember(float (&fuzzyValue)[3]);
71
    void fuzzyfication(int sensorValue, float (&fuzziedValue)[3]);
72
    void copyRpmSpeed(const int (&source)[2], int (&target)[2]);
32 73

  
33 74
    char whiteFlag = 0;
34
    
35

  
75
    LineFollowStrategy strategy = LineFollowStrategy::EDGE_RIGHT;
76
    float Kp = 0.003;
77
    float Ki = 0;
78
    float Kd = 0;
79
    int accumHist = 0;
80
    float oldError = 0;
81
    int vcnl4020AmbientLight[4] = {0};
82
    int vcnl4020Proximity[4] = {0};
36 83
};
37 84

  
85

  
86

  
38 87
} // end of namespace amiro
39 88

  
40 89
#endif // AMIRO_LINEFOLLOWING_H

Also available in: Unified diff