Statistics
| Branch: | Tag: | Revision:

amiro-os / devices / DiWheelDrive / linefollow.hpp @ e0c5d17a

History | View | Annotate | Download (4 KB)

1
#ifndef AMIRO_LINEFOLLOWING_H
2
#define AMIRO_LINEFOLLOWING_H
3

    
4
#include <ch.hpp>
5
#include "global.hpp"
6
#include <amiroosconf.h>
7

    
8
#define RAND_TRESH 16000
9
#define MAX_CORRECTED_SPEED 1000000*100
10

    
11
namespace amiro {
12
  
13
  enum LineFollowStrategy{
14
  EDGE_LEFT,      // driving on the left edge of a black line
15
  TRANSITION_L_R, // Transition from left to right edge
16
  TRANSITION_R_L, // transition from right to left edge
17
  EDGE_RIGHT,     // driving on the right edge of a black line
18
  REVERSE,
19
  MIDDLE,         // not working, use FUZZY instead
20
  FUZZY,
21
  NONE
22
  };
23

    
24
enum colorMember : uint8_t {
25
        BLACK=0,
26
        GREY=1,
27
        WHITE=2
28
};
29

    
30
class LineFollow
31
{
32
public:
33

    
34
  int biggestDiff = 0;
35
  Global *global;
36
  LineFollow(Global *global);
37
  LineFollow(Global *global, LineFollowStrategy strategy);
38
  /**
39
   * Entry method which should be called to follow a line.
40
   * 
41
   * @param rpmSpeed speed that will be determained
42
   * @return white flag, 1 if white is detected  
43
   */
44
  int followLine(int (&rpmSpeed)[2]);
45

    
46
  /**
47
   * Setter for LineFollowStrategy which triggers transition behavior.
48
   * 
49
   * To prevent random turns while changeing the strategy a transition state is set.
50
   * In case the strategy needs to be switched immediately use promptStrategyChange().
51
   * 
52
   * @param strategy 
53
   */
54
  void setStrategy(LineFollowStrategy strategy);
55
  void promptStrategyChange(LineFollowStrategy strategy);
56
  LineFollowStrategy getStrategy();
57
  void setGains(float Kp, float Ki, float Kd);
58

    
59

    
60
  const int rpmTurnLeft[2] = {-10, 10};
61
  const int rpmTurnRight[2] = {rpmTurnLeft[1],rpmTurnLeft[0]};
62
  const int rpmHalt[2] = {0, 0};
63
  // Definition of the fuzzyfication function
64
  //  | Membership
65
  // 1|_B__   G    __W__
66
  //  |    \  /\  /
67
  //  |     \/  \/
68
  //  |_____/\__/\______ Sensor values
69
  // SEE MATLAB SCRIPT "fuzzyRule.m" for adjusting the values
70
  // All values are "raw sensor values"
71
  /* Use these values for white ground surface (e.g. paper) */
72

    
73
  const int blackStartFalling = 0x1000; // Where the black curve starts falling
74
  const int blackOff = 0x1800; // Where no more black is detected
75
  const int whiteStartRising = 0x2800; // Where the white curve starts rising
76
  const int whiteOn = 0x6000; // Where the white curve has reached the maximum value
77
  const int greyMax = (whiteOn + blackStartFalling) / 2; // Where grey has its maximum
78
  const int greyStartRising = blackStartFalling; // Where grey starts rising
79
  const int greyOff = whiteOn; // Where grey is completely off again
80

    
81
private:
82
  /**
83
   * Calculate the error from front sensors.
84
   */
85
  int getError();
86

    
87
  /**
88
   * Error calculation while changing from one EDGE_* strategy to another.
89
   * This prevents the AMiRo from random turns while switching strategies.
90
   * 
91
   * @param FL value of left front sensor
92
   * @param FR value of right front sensor
93
   * @param targetL left threshold 
94
   * @param targetR right threshold
95
   */
96
  int transitionError(int FL, int FR, int targetL, int targetR);
97

    
98
  /**
99
   * Use the error according to the strategy to calculate the correction speed.
100
   * Currently only the P part of the PID controller is used to calculate the 
101
   * correction speed.
102
   */
103
  int getPidCorrectionSpeed();
104

    
105
  // Fuzzy line following methods--------------
106
  void lineFollowing(int (&proximity)[4], int (&rpmFuzzyCtrl)[2]);
107
  Color memberToLed(colorMember member);
108
  void defuzzyfication(colorMember (&member)[4], int (&rpmFuzzyCtrl)[2]);
109
  colorMember getMember(float (&fuzzyValue)[3]);
110
  void fuzzyfication(int sensorValue, float (&fuzziedValue)[3]);
111
  void copyRpmSpeed(const int (&source)[2], int (&target)[2]);
112
  int vcnl4020AmbientLight[4] = {0};
113
  int vcnl4020Proximity[4] = {0};
114
  // -----------------------------------------
115
  LineFollowStrategy strategy = LineFollowStrategy::EDGE_RIGHT;
116
  char whiteFlag = 0;
117
  int trans = 0;
118

    
119
  // PID controller components ---------------
120
  int32_t K_p = 1900;
121
  float K_i = 0.15;
122
  float K_d = 0.6;
123
  int32_t accumHist = 0;
124
  int32_t oldError = 0;
125
  // ----------------------------------------
126
};
127

    
128

    
129

    
130
} // end of namespace amiro
131

    
132
#endif // AMIRO_LINEFOLLOWING_H