Revision 1b3adcdd devices/DiWheelDrive/linefollow2.hpp
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