Micromouse_Test/PID.cpp at master · CharlesJu/Micromouse_Test · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
typedef struct{
    float kP;   float kI;   float kD;
    float val;  float t;
    float e_prev;   float e_diff;
    float e_cur;    float e_tot;
    float o_min;    float o_max;
    float e_diff_prev;  short dT_cont;

} PID_Values;

class PID {
    public:
        PID(float kP, float kI, float kD){
            vals.kP = kP;   vals.kI = kI;   vals.kD = kD;
            vals.val = 0;   vals.t = 0;
            vals.e_prev = 0;    vals.e_diff = 0;
            vals.e_cur = 0;     vals.e_tot = 0;
            vals.o_min = -1E30; vals.o_max = 1E30;
            float e_diff_prev = 0;  short dT_cont = 0;
        }

        PID(float kP, float kI, float kD, float min, float max){
            vals.kP = kP;   vals.kI = kI;   vals.kD = kD;
            vals.val = 0;   vals.t = 0;
            vals.e_prev = 0;    vals.e_diff = 0;
            vals.e_cur = 0;     vals.e_tot = 0;
            vals.o_min = min;   vals.o_max = max;
            float e_diff_prev = 0;  short dT_cont = 0;
        }


        void update(float val, float t){
            //This needs to be update every loop
            vals.val = val;
            vals.t = t;
            if (t != vals.t){
                reset();
            } else {
                vals.e_cur = t - val;
                vals.e_diff = vals.e_prev - vals.e_cur;
                vals.e_tot += vals.e_cur;

                //Prevents I value Windup
                if((vals.e_diff < vals.e_diff_prev + 0.005
                    || vals.e_diff > vals.e_diff_prev - 0.005)
                    && vals.dT_cont < 501) {
                    vals.dT_cont += 1;
                } else {
                    vals.dT_cont = 0;
                    vals.e_tot = 0;
                }
            }
        }

        void reset(){
            vals.val = 0;
            vals.e_prev = 0;    vals.e_diff = 0;
            vals.e_cur = 0;     vals.e_tot = 0;
        }

        float out(){
            float v = vals.e_cur * vals.kP + vals.e_tot * vals.kI + vals.e_diff * vals.kD;

            if(v < vals.o_min){
                v = vals.o_min;
            } else if (v > vals.o_max){
                v = vals.o_max;
            }

            vals.e_prev = vals.e_cur;

            return v;
        }

        float get(float val, float t){
            update(val, t);
            return out();
        }

        float getVal(){
            return vals.val;
        }

        float getErr(){
            return vals.e_cur;
        }

        float getPrevErr(){
            return vals.e_diff + vals.e_cur;
        }

        float getTotErr(){
            return vals.e_tot;
        }


    private:
        PID_Values vals;
};