amiro-os / devices / DiWheelDrive / linefollow2.hpp @ bfffb0bd
History | View | Annotate | Download (2.6 KB)
1 |
#ifndef AMIRO_LINEFOLLOWING_H
|
---|---|
2 |
#define AMIRO_LINEFOLLOWING_H
|
3 |
|
4 |
#include <ch.hpp> |
5 |
|
6 |
#include <amiroosconf.h> |
7 |
|
8 |
namespace amiro {
|
9 |
enum class LineFollowStrategy{ |
10 |
EDGE_LEFT, |
11 |
EDGE_RIGHT, |
12 |
DOCKING, |
13 |
MIDDLE, |
14 |
FUZZY |
15 |
}; |
16 |
|
17 |
enum colorMember : uint8_t {
|
18 |
BLACK=0,
|
19 |
GREY=1,
|
20 |
WHITE=2
|
21 |
}; |
22 |
|
23 |
class LineFollow |
24 |
{ |
25 |
public:
|
26 |
|
27 |
|
28 |
int biggestDiff = 0; |
29 |
Global *global; |
30 |
LineFollow(Global *global); |
31 |
LineFollow(Global *global, LineFollowStrategy strategy); |
32 |
// void calibrateZiegler(float KCrit, int rpmSpeed[2]);
|
33 |
int followLine(int (&rpmSpeed)[2]); |
34 |
// int followLeftEdge(int rpmSpeed[2]);
|
35 |
// int followRightEdge(int rpmSpeed[2]);
|
36 |
// int followMiddle(int rpmSpeed[2]);
|
37 |
void setStrategy(LineFollowStrategy strategy);
|
38 |
LineFollowStrategy getStrategy(); |
39 |
void setGains(float Kp, float Ki, float Kd); |
40 |
|
41 |
|
42 |
const int rpmTurnLeft[2] = {-10, 10}; |
43 |
const int rpmTurnRight[2] = {rpmTurnLeft[1],rpmTurnLeft[0]}; |
44 |
const int rpmHalt[2] = {0, 0}; |
45 |
// Definition of the fuzzyfication function
|
46 |
// | Membership
|
47 |
// 1|_B__ G __W__
|
48 |
// | \ /\ /
|
49 |
// | \/ \/
|
50 |
// |_____/\__/\______ Sensor values
|
51 |
// SEE MATLAB SCRIPT "fuzzyRule.m" for adjusting the values
|
52 |
// All values are "raw sensor values"
|
53 |
/* Use these values for white ground surface (e.g. paper) */
|
54 |
|
55 |
const int blackStartFalling = 0x1000; // Where the black curve starts falling |
56 |
const int blackOff = 0x1800; // Where no more black is detected |
57 |
const int whiteStartRising = 0x2800; // Where the white curve starts rising |
58 |
const int whiteOn = 0x6000; // Where the white curve has reached the maximum value |
59 |
const int greyMax = (whiteOn + blackStartFalling) / 2; // Where grey has its maximum |
60 |
const int greyStartRising = blackStartFalling; // Where grey starts rising |
61 |
const int greyOff = whiteOn; // Where grey is completely off again |
62 |
|
63 |
private:
|
64 |
int delta();
|
65 |
int getError();
|
66 |
int getPidCorrectionSpeed();
|
67 |
void lineFollowing(int (&proximity)[4], int (&rpmFuzzyCtrl)[2]); |
68 |
// void defuzz(colorMember (&member)[4], int (&rpmFuzzyCtrl)[2]);
|
69 |
Color memberToLed(colorMember member); |
70 |
void defuzzyfication(colorMember (&member)[4], int (&rpmFuzzyCtrl)[2]); |
71 |
colorMember getMember(float (&fuzzyValue)[3]); |
72 |
void fuzzyfication(int sensorValue, float (&fuzziedValue)[3]); |
73 |
void copyRpmSpeed(const int (&source)[2], int (&target)[2]); |
74 |
|
75 |
char whiteFlag = 0; |
76 |
LineFollowStrategy strategy = LineFollowStrategy::EDGE_RIGHT; |
77 |
float Kp = 0.002; |
78 |
float Ki = 0; |
79 |
float Kd = 0; |
80 |
int accumHist = 0; |
81 |
float oldError = 0; |
82 |
int vcnl4020AmbientLight[4] = {0}; |
83 |
int vcnl4020Proximity[4] = {0}; |
84 |
}; |
85 |
|
86 |
|
87 |
|
88 |
} // end of namespace amiro
|
89 |
|
90 |
#endif // AMIRO_LINEFOLLOWING_H |