amiro-os / devices / DiWheelDrive / linefollow2.hpp @ 1b3adcdd
History | View | Annotate | Download (2.59 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 |
MIDDLE, |
13 |
FUZZY |
14 |
}; |
15 |
|
16 |
enum colorMember : uint8_t {
|
17 |
BLACK=0,
|
18 |
GREY=1,
|
19 |
WHITE=2
|
20 |
}; |
21 |
|
22 |
class LineFollow |
23 |
{ |
24 |
public:
|
25 |
|
26 |
|
27 |
int biggestDiff = 0; |
28 |
Global *global; |
29 |
LineFollow(Global *global); |
30 |
LineFollow(Global *global, LineFollowStrategy strategy); |
31 |
// void calibrateZiegler(float KCrit, 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 |
61 |
|
62 |
private:
|
63 |
int delta();
|
64 |
int getError();
|
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]); |
73 |
|
74 |
char whiteFlag = 0; |
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}; |
83 |
}; |
84 |
|
85 |
|
86 |
|
87 |
} // end of namespace amiro
|
88 |
|
89 |
#endif // AMIRO_LINEFOLLOWING_H |