Statistics
| Branch: | Tag: | Revision:

amiro-os / devices / DiWheelDrive / linefollow2.hpp @ 9c46b728

History | View | Annotate | Download (2.64 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
namespace amiro {
9
  
10
  enum class LineFollowStrategy{
11
  EDGE_LEFT,
12
  EDGE_RIGHT,
13
  DOCKING,
14
  MIDDLE,
15
  FUZZY,
16
  NONE
17
  };
18

    
19
enum colorMember : uint8_t {
20
        BLACK=0,
21
        GREY=1,
22
        WHITE=2
23
};
24

    
25
class LineFollow
26
{
27
public:
28

    
29
  
30
  int biggestDiff = 0;
31
  Global *global;
32
  LineFollow(Global *global);
33
  LineFollow(Global *global, LineFollowStrategy strategy);
34
  // void calibrateZiegler(float KCrit, int rpmSpeed[2]);
35
  int followLine(int (&rpmSpeed)[2]);
36
  // int followLeftEdge(int rpmSpeed[2]);
37
  // int followRightEdge(int rpmSpeed[2]);
38
  // int followMiddle(int rpmSpeed[2]);
39
  void setStrategy(LineFollowStrategy strategy);
40
  LineFollowStrategy getStrategy();
41
  void setGains(float Kp, float Ki, float Kd);
42

    
43

    
44
  const int rpmTurnLeft[2] = {-10, 10};
45
  const int rpmTurnRight[2] = {rpmTurnLeft[1],rpmTurnLeft[0]};
46
  const int rpmHalt[2] = {0, 0};
47
  // Definition of the fuzzyfication function
48
  //  | Membership
49
  // 1|_B__   G    __W__
50
  //  |    \  /\  /
51
  //  |     \/  \/
52
  //  |_____/\__/\______ Sensor values
53
  // SEE MATLAB SCRIPT "fuzzyRule.m" for adjusting the values
54
  // All values are "raw sensor values"
55
  /* Use these values for white ground surface (e.g. paper) */
56

    
57
  const int blackStartFalling = 0x1000; // Where the black curve starts falling
58
  const int blackOff = 0x1800; // Where no more black is detected
59
  const int whiteStartRising = 0x2800; // Where the white curve starts rising
60
  const int whiteOn = 0x6000; // Where the white curve has reached the maximum value
61
  const int greyMax = (whiteOn + blackStartFalling) / 2; // Where grey has its maximum
62
  const int greyStartRising = blackStartFalling; // Where grey starts rising
63
  const int greyOff = whiteOn; // Where grey is completely off again
64

    
65
  private:
66
    int delta();
67
    int getError();
68
    int getPidCorrectionSpeed();
69
    void lineFollowing(int (&proximity)[4], int (&rpmFuzzyCtrl)[2]);
70
    // void defuzz(colorMember (&member)[4], int (&rpmFuzzyCtrl)[2]);
71
    Color memberToLed(colorMember member);
72
    void defuzzyfication(colorMember (&member)[4], int (&rpmFuzzyCtrl)[2]);
73
    colorMember getMember(float (&fuzzyValue)[3]);
74
    void fuzzyfication(int sensorValue, float (&fuzziedValue)[3]);
75
    void copyRpmSpeed(const int (&source)[2], int (&target)[2]);
76

    
77
    char whiteFlag = 0;
78
    LineFollowStrategy strategy = LineFollowStrategy::EDGE_RIGHT;
79
    float Kp = 0.002;
80
    float Ki = 0;
81
    float Kd = 0;
82
    int accumHist = 0;
83
    float oldError = 0;
84
    int vcnl4020AmbientLight[4] = {0};
85
    int vcnl4020Proximity[4] = {0};
86
};
87

    
88

    
89

    
90
} // end of namespace amiro
91

    
92
#endif // AMIRO_LINEFOLLOWING_H