amiro-os / devices / DiWheelDrive / linefollow.hpp @ 726fdc72
History | View | Annotate | Download (2.679 KB)
1 | 726fdc72 | galberding | #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 | |||
10 | namespace amiro {
|
||
11 | |||
12 | enum LineFollowStrategy{
|
||
13 | EDGE_LEFT, |
||
14 | TRANSITION_L_R, |
||
15 | TRANSITION_R_L, |
||
16 | EDGE_RIGHT, |
||
17 | REVERSE, |
||
18 | MIDDLE, |
||
19 | FUZZY, |
||
20 | NONE |
||
21 | }; |
||
22 | |||
23 | enum colorMember : uint8_t {
|
||
24 | BLACK=0,
|
||
25 | GREY=1,
|
||
26 | WHITE=2
|
||
27 | }; |
||
28 | |||
29 | class LineFollow |
||
30 | { |
||
31 | public:
|
||
32 | |||
33 | // TODO: Documentation
|
||
34 | int biggestDiff = 0; |
||
35 | Global *global; |
||
36 | LineFollow(Global *global); |
||
37 | LineFollow(Global *global, LineFollowStrategy strategy); |
||
38 | int followLine(int (&rpmSpeed)[2]); |
||
39 | void setStrategy(LineFollowStrategy strategy);
|
||
40 | void promptStrategyChange(LineFollowStrategy strategy);
|
||
41 | LineFollowStrategy getStrategy(); |
||
42 | void setGains(float Kp, float Ki, float Kd); |
||
43 | |||
44 | |||
45 | const int rpmTurnLeft[2] = {-10, 10}; |
||
46 | const int rpmTurnRight[2] = {rpmTurnLeft[1],rpmTurnLeft[0]}; |
||
47 | const int rpmHalt[2] = {0, 0}; |
||
48 | // Definition of the fuzzyfication function
|
||
49 | // | Membership
|
||
50 | // 1|_B__ G __W__
|
||
51 | // | \ /\ /
|
||
52 | // | \/ \/
|
||
53 | // |_____/\__/\______ Sensor values
|
||
54 | // SEE MATLAB SCRIPT "fuzzyRule.m" for adjusting the values
|
||
55 | // All values are "raw sensor values"
|
||
56 | /* Use these values for white ground surface (e.g. paper) */
|
||
57 | |||
58 | const int blackStartFalling = 0x1000; // Where the black curve starts falling |
||
59 | const int blackOff = 0x1800; // Where no more black is detected |
||
60 | const int whiteStartRising = 0x2800; // Where the white curve starts rising |
||
61 | const int whiteOn = 0x6000; // Where the white curve has reached the maximum value |
||
62 | const int greyMax = (whiteOn + blackStartFalling) / 2; // Where grey has its maximum |
||
63 | const int greyStartRising = blackStartFalling; // Where grey starts rising |
||
64 | const int greyOff = whiteOn; // Where grey is completely off again |
||
65 | |||
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}; |
||
89 | }; |
||
90 | |||
91 | |||
92 | |||
93 | } // end of namespace amiro
|
||
94 | |||
95 | #endif // AMIRO_LINEFOLLOWING_H |