PIDcontroller_without_biquadfilter
Dependencies: MODSERIAL QEI biquadFilter mbed
Fork of 20170910_PID_V1_0 by
main.cpp@1:b66e14435f70, 2017-10-11 (annotated)
- Committer:
- Arnoud113
- Date:
- Wed Oct 11 09:11:20 2017 +0000
- Revision:
- 1:b66e14435f70
- Parent:
- 0:85ca550760e9
- Child:
- 2:02b385b96543
with working I part
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Arnoud113 | 0:85ca550760e9 | 1 | #include "mbed.h" |
Arnoud113 | 0:85ca550760e9 | 2 | #include "QEI.h" |
Arnoud113 | 0:85ca550760e9 | 3 | #include "MODSERIAL.h" |
Arnoud113 | 0:85ca550760e9 | 4 | #include "math.h" |
Arnoud113 | 0:85ca550760e9 | 5 | |
Arnoud113 | 0:85ca550760e9 | 6 | |
Arnoud113 | 0:85ca550760e9 | 7 | |
Arnoud113 | 0:85ca550760e9 | 8 | DigitalOut gpo(D0); |
Arnoud113 | 0:85ca550760e9 | 9 | DigitalOut ledb(LED_BLUE); |
Arnoud113 | 0:85ca550760e9 | 10 | DigitalOut ledr(LED_RED); |
Arnoud113 | 0:85ca550760e9 | 11 | DigitalOut ledg(LED_GREEN); |
Arnoud113 | 0:85ca550760e9 | 12 | DigitalOut motor1DC(D7); |
Arnoud113 | 0:85ca550760e9 | 13 | DigitalOut motor1PWM(D6); |
Arnoud113 | 0:85ca550760e9 | 14 | DigitalOut motor2DC(D4); |
Arnoud113 | 0:85ca550760e9 | 15 | DigitalOut motor2PWM(D5); |
Arnoud113 | 0:85ca550760e9 | 16 | |
Arnoud113 | 0:85ca550760e9 | 17 | AnalogIn potMeter1(A0); |
Arnoud113 | 0:85ca550760e9 | 18 | AnalogIn potMeter2(A1); |
Arnoud113 | 0:85ca550760e9 | 19 | DigitalIn button1(D11); |
Arnoud113 | 0:85ca550760e9 | 20 | DigitalIn button2(D12); |
Arnoud113 | 0:85ca550760e9 | 21 | QEI Encoder1(D12,D13,NC,4200); |
Arnoud113 | 0:85ca550760e9 | 22 | |
Arnoud113 | 0:85ca550760e9 | 23 | MODSERIAL pc(USBTX,USBRX); |
Arnoud113 | 0:85ca550760e9 | 24 | |
Arnoud113 | 0:85ca550760e9 | 25 | Ticker controller; |
Arnoud113 | 0:85ca550760e9 | 26 | |
Arnoud113 | 0:85ca550760e9 | 27 | const double pi = 3.1415926535897; |
Arnoud113 | 1:b66e14435f70 | 28 | const float M1_KP = 10, M1_KI = 0.0; |
Arnoud113 | 1:b66e14435f70 | 29 | const double M1_TS = 0.0001; |
Arnoud113 | 0:85ca550760e9 | 30 | const float RAD_PER_PULSE = (2*pi)/4200; |
Arnoud113 | 1:b66e14435f70 | 31 | const float CONTROLLER_TS = 0.0001; //TIME INTERVAL/ hZ |
Arnoud113 | 1:b66e14435f70 | 32 | double m1_err_int = 0; |
Arnoud113 | 0:85ca550760e9 | 33 | |
Arnoud113 | 1:b66e14435f70 | 34 | // the working P controller beneath here |
Arnoud113 | 1:b66e14435f70 | 35 | //float P(double error, const float Kp){ |
Arnoud113 | 1:b66e14435f70 | 36 | // return Kp * error; |
Arnoud113 | 1:b66e14435f70 | 37 | // } |
Arnoud113 | 1:b66e14435f70 | 38 | |
Arnoud113 | 1:b66e14435f70 | 39 | // New PI controller |
Arnoud113 | 1:b66e14435f70 | 40 | |
Arnoud113 | 1:b66e14435f70 | 41 | double PI(double e, const double Kp, const double Ki, double Ts, double &e_int){ |
Arnoud113 | 1:b66e14435f70 | 42 | e_int += Ts*e; |
Arnoud113 | 1:b66e14435f70 | 43 | return Kp*e+Ki*e_int; |
Arnoud113 | 1:b66e14435f70 | 44 | } |
Arnoud113 | 1:b66e14435f70 | 45 | |
Arnoud113 | 1:b66e14435f70 | 46 | /* Working P controller part |
Arnoud113 | 0:85ca550760e9 | 47 | |
Arnoud113 | 0:85ca550760e9 | 48 | void motor1_Controller(){ |
Arnoud113 | 0:85ca550760e9 | 49 | double reference = 10*potMeter1.read(); |
Arnoud113 | 0:85ca550760e9 | 50 | double position = RAD_PER_PULSE*Encoder1.getPulses(); |
Arnoud113 | 1:b66e14435f70 | 51 | double motor1 = P(reference-position, M1_KP); |
Arnoud113 | 0:85ca550760e9 | 52 | motor1PWM = motor1; |
Arnoud113 | 0:85ca550760e9 | 53 | |
Arnoud113 | 0:85ca550760e9 | 54 | if(motor1 > 0.1){ |
Arnoud113 | 0:85ca550760e9 | 55 | motor1DC = 1; |
Arnoud113 | 0:85ca550760e9 | 56 | |
Arnoud113 | 0:85ca550760e9 | 57 | ledr = 1; |
Arnoud113 | 0:85ca550760e9 | 58 | ledg = 1; //Blau |
Arnoud113 | 0:85ca550760e9 | 59 | ledb = 0; |
Arnoud113 | 0:85ca550760e9 | 60 | } |
Arnoud113 | 0:85ca550760e9 | 61 | else if (motor1<-0.1) { |
Arnoud113 | 0:85ca550760e9 | 62 | motor1DC = 0; |
Arnoud113 | 0:85ca550760e9 | 63 | |
Arnoud113 | 0:85ca550760e9 | 64 | ledb = 1; |
Arnoud113 | 0:85ca550760e9 | 65 | ledr = 1; |
Arnoud113 | 0:85ca550760e9 | 66 | ledg = 0; //Groen |
Arnoud113 | 0:85ca550760e9 | 67 | |
Arnoud113 | 0:85ca550760e9 | 68 | } |
Arnoud113 | 0:85ca550760e9 | 69 | else{ |
Arnoud113 | 0:85ca550760e9 | 70 | motor1PWM = 0; |
Arnoud113 | 0:85ca550760e9 | 71 | |
Arnoud113 | 0:85ca550760e9 | 72 | ledb = 1; //Rood |
Arnoud113 | 0:85ca550760e9 | 73 | ledr = 0; |
Arnoud113 | 0:85ca550760e9 | 74 | ledg = 1; |
Arnoud113 | 0:85ca550760e9 | 75 | } |
Arnoud113 | 0:85ca550760e9 | 76 | |
Arnoud113 | 0:85ca550760e9 | 77 | |
Arnoud113 | 0:85ca550760e9 | 78 | } |
Arnoud113 | 1:b66e14435f70 | 79 | */ |
Arnoud113 | 1:b66e14435f70 | 80 | |
Arnoud113 | 1:b66e14435f70 | 81 | // New PI part |
Arnoud113 | 1:b66e14435f70 | 82 | |
Arnoud113 | 1:b66e14435f70 | 83 | void m1_Controller(){ |
Arnoud113 | 1:b66e14435f70 | 84 | double reference = 10*potMeter1.read(); |
Arnoud113 | 1:b66e14435f70 | 85 | double position = RAD_PER_PULSE*Encoder1.getPulses(); |
Arnoud113 | 1:b66e14435f70 | 86 | float motor1 = PI(reference - position, M1_KP, M1_KI, M1_TS, m1_err_int); |
Arnoud113 | 1:b66e14435f70 | 87 | |
Arnoud113 | 1:b66e14435f70 | 88 | motor1PWM = motor1; |
Arnoud113 | 1:b66e14435f70 | 89 | |
Arnoud113 | 1:b66e14435f70 | 90 | if(motor1 > 0.5){ |
Arnoud113 | 1:b66e14435f70 | 91 | motor1DC = 1; |
Arnoud113 | 1:b66e14435f70 | 92 | |
Arnoud113 | 1:b66e14435f70 | 93 | ledr = 1; |
Arnoud113 | 1:b66e14435f70 | 94 | ledg = 1; //Blau |
Arnoud113 | 1:b66e14435f70 | 95 | ledb = 0; |
Arnoud113 | 1:b66e14435f70 | 96 | } |
Arnoud113 | 1:b66e14435f70 | 97 | else if (motor1<-0.5) { |
Arnoud113 | 1:b66e14435f70 | 98 | motor1DC = 0; |
Arnoud113 | 1:b66e14435f70 | 99 | |
Arnoud113 | 1:b66e14435f70 | 100 | ledb = 1; |
Arnoud113 | 1:b66e14435f70 | 101 | ledr = 1; |
Arnoud113 | 1:b66e14435f70 | 102 | ledg = 0; //Groen |
Arnoud113 | 1:b66e14435f70 | 103 | |
Arnoud113 | 1:b66e14435f70 | 104 | } |
Arnoud113 | 1:b66e14435f70 | 105 | else{ |
Arnoud113 | 1:b66e14435f70 | 106 | motor1PWM = 0; |
Arnoud113 | 1:b66e14435f70 | 107 | |
Arnoud113 | 1:b66e14435f70 | 108 | ledb = 1; //Rood |
Arnoud113 | 1:b66e14435f70 | 109 | ledr = 0; |
Arnoud113 | 1:b66e14435f70 | 110 | ledg = 1; |
Arnoud113 | 1:b66e14435f70 | 111 | } |
Arnoud113 | 1:b66e14435f70 | 112 | |
Arnoud113 | 1:b66e14435f70 | 113 | |
Arnoud113 | 1:b66e14435f70 | 114 | } |
Arnoud113 | 0:85ca550760e9 | 115 | |
Arnoud113 | 0:85ca550760e9 | 116 | int main(){ |
Arnoud113 | 0:85ca550760e9 | 117 | pc.baud(115200); |
Arnoud113 | 1:b66e14435f70 | 118 | //controller.attach(&m1_Controller, CONTROLLER_TS); --> P one |
Arnoud113 | 1:b66e14435f70 | 119 | |
Arnoud113 | 1:b66e14435f70 | 120 | controller.attach(&m1_Controller, M1_TS); |
Arnoud113 | 0:85ca550760e9 | 121 | |
Arnoud113 | 0:85ca550760e9 | 122 | while(1){ |
Arnoud113 | 0:85ca550760e9 | 123 | double reference = 10*potMeter1.read(); |
Arnoud113 | 0:85ca550760e9 | 124 | double position = RAD_PER_PULSE*Encoder1.getPulses(); |
Arnoud113 | 1:b66e14435f70 | 125 | // double motor1 = P(reference-position, M1_KP); --> old one |
Arnoud113 | 1:b66e14435f70 | 126 | float motor1 = PI(reference - position, M1_KP, M1_KI, M1_TS, m1_err_int); |
Arnoud113 | 1:b66e14435f70 | 127 | |
Arnoud113 | 0:85ca550760e9 | 128 | pc.printf("\r value motor1: %f. reference Pot: %f. Position: %f \n", motor1, reference, position); |
Arnoud113 | 0:85ca550760e9 | 129 | } |
Arnoud113 | 0:85ca550760e9 | 130 | } |