Arnoud Meutstege / Mbed 2 deprecated 20170910_PID_V1_0

Working P loop

Dependencies:   MODSERIAL QEI mbed

main.cpp@2:c2c76a7a7250, 2017-10-13 (annotated)

Committer:
Arnoud113
Date:
Fri Oct 13 08:00:35 2017 +0000
Revision:
2:c2c76a7a7250
Parent:
1:b66e14435f70 
NIET WERKENDE PID

Who changed what in which revision?

UserRevisionLine numberNew 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 2:c2c76a7a7250 28 const float M1_KP = 20 , M1_KI = 1.0, M1_KD = 0.5;
Arnoud113 2:c2c76a7a7250 29 const double M1_TS = 0.001;
Arnoud113 0:85ca550760e9 30 const float RAD_PER_PULSE = (2*pi)/4200;
Arnoud113 2:c2c76a7a7250 31 const float CONTROLLER_TS = 0.001; //TIME INTERVAL/ hZ
Arnoud113 2:c2c76a7a7250 32 double m1_err_int = 0, m1_prev_err = 0;
Arnoud113 2:c2c76a7a7250 33 const double M1_F_A1 = 1.0, M1_F_A2 = 2.0, M1_F_B0 = 1.0, M1_F_B1 = 3.0, M1_F_B2 = 4.0;
Arnoud113 2:c2c76a7a7250 34 double m1_f_v1=0, m1_f_v2 = 0;
Arnoud113 0:85ca550760e9 35
Arnoud113 2:c2c76a7a7250 36 //Biquad filter
Arnoud113 2:c2c76a7a7250 37 double biquad(double u, double &v1, double &v2, const double a1, const double a2, const double b0, const double b1, const double b2){
Arnoud113 2:c2c76a7a7250 38 double v = u-a1*v1-a2*v2;
Arnoud113 2:c2c76a7a7250 39 double y = b0*v+b1*v1+b2*v2;
Arnoud113 2:c2c76a7a7250 40 v2 = v1;v1 = v;
Arnoud113 2:c2c76a7a7250 41 return y;
Arnoud113 2:c2c76a7a7250 42 }
Arnoud113 2:c2c76a7a7250 43
Arnoud113 2:c2c76a7a7250 44 /* the working P controller beneath here
Arnoud113 1:b66e14435f70 45 //float P(double error, const float Kp){
Arnoud113 1:b66e14435f70 46 // return Kp * error;
Arnoud113 1:b66e14435f70 47 // }
Arnoud113 1:b66e14435f70 48
Arnoud113 1:b66e14435f70 49 // New PI controller
Arnoud113 1:b66e14435f70 50
Arnoud113 1:b66e14435f70 51 double PI(double e, const double Kp, const double Ki, double Ts, double &e_int){
Arnoud113 1:b66e14435f70 52 e_int += Ts*e;
Arnoud113 1:b66e14435f70 53 return Kp*e+Ki*e_int;
Arnoud113 2:c2c76a7a7250 54 }*/
Arnoud113 2:c2c76a7a7250 55
Arnoud113 2:c2c76a7a7250 56 //new PID part
Arnoud113 2:c2c76a7a7250 57
Arnoud113 2:c2c76a7a7250 58 double PID( double e, const double Kp, const double Ki, const double Kd, double Ts, double &e_int, double &e_prev, double &f_v1, double &f_v2, const double f_a1, const double f_a2, const double f_b0, const double f_b1, const double f_b2){
Arnoud113 2:c2c76a7a7250 59 // Derivative
Arnoud113 2:c2c76a7a7250 60 double e_der = (e-e_prev)/Ts;
Arnoud113 2:c2c76a7a7250 61 e_der = biquad( e_der, f_v1, f_v2, f_a1, f_a2, f_b0, f_b1, f_b2 );
Arnoud113 2:c2c76a7a7250 62 e_prev = e;
Arnoud113 2:c2c76a7a7250 63 // Integral
Arnoud113 2:c2c76a7a7250 64 e_int += Ts*e;
Arnoud113 2:c2c76a7a7250 65 // PID
Arnoud113 2:c2c76a7a7250 66 return Kp * e + Ki * e_int + Kd * e_der;
Arnoud113 1:b66e14435f70 67 }
Arnoud113 1:b66e14435f70 68
Arnoud113 1:b66e14435f70 69 /* Working P controller part
Arnoud113 0:85ca550760e9 70
Arnoud113 0:85ca550760e9 71 void motor1_Controller(){
Arnoud113 0:85ca550760e9 72 double reference = 10*potMeter1.read();
Arnoud113 0:85ca550760e9 73 double position = RAD_PER_PULSE*Encoder1.getPulses();
Arnoud113 1:b66e14435f70 74 double motor1 = P(reference-position, M1_KP);
Arnoud113 0:85ca550760e9 75 motor1PWM = motor1;
Arnoud113 0:85ca550760e9 76
Arnoud113 0:85ca550760e9 77 if(motor1 > 0.1){
Arnoud113 0:85ca550760e9 78 motor1DC = 1;
Arnoud113 0:85ca550760e9 79
Arnoud113 0:85ca550760e9 80 ledr = 1;
Arnoud113 0:85ca550760e9 81 ledg = 1; //Blau
Arnoud113 0:85ca550760e9 82 ledb = 0;
Arnoud113 0:85ca550760e9 83 }
Arnoud113 0:85ca550760e9 84 else if (motor1<-0.1) {
Arnoud113 0:85ca550760e9 85 motor1DC = 0;
Arnoud113 0:85ca550760e9 86
Arnoud113 0:85ca550760e9 87 ledb = 1;
Arnoud113 0:85ca550760e9 88 ledr = 1;
Arnoud113 0:85ca550760e9 89 ledg = 0; //Groen
Arnoud113 0:85ca550760e9 90
Arnoud113 0:85ca550760e9 91 }
Arnoud113 0:85ca550760e9 92 else{
Arnoud113 0:85ca550760e9 93 motor1PWM = 0;
Arnoud113 0:85ca550760e9 94
Arnoud113 0:85ca550760e9 95 ledb = 1; //Rood
Arnoud113 0:85ca550760e9 96 ledr = 0;
Arnoud113 0:85ca550760e9 97 ledg = 1;
Arnoud113 0:85ca550760e9 98 }
Arnoud113 0:85ca550760e9 99
Arnoud113 0:85ca550760e9 100
Arnoud113 0:85ca550760e9 101 }
Arnoud113 1:b66e14435f70 102 */
Arnoud113 1:b66e14435f70 103
Arnoud113 2:c2c76a7a7250 104 /*/ New PI part
Arnoud113 1:b66e14435f70 105
Arnoud113 1:b66e14435f70 106 void m1_Controller(){
Arnoud113 1:b66e14435f70 107 double reference = 10*potMeter1.read();
Arnoud113 1:b66e14435f70 108 double position = RAD_PER_PULSE*Encoder1.getPulses();
Arnoud113 1:b66e14435f70 109 float motor1 = PI(reference - position, M1_KP, M1_KI, M1_TS, m1_err_int);
Arnoud113 1:b66e14435f70 110
Arnoud113 1:b66e14435f70 111 motor1PWM = motor1;
Arnoud113 1:b66e14435f70 112
Arnoud113 1:b66e14435f70 113 if(motor1 > 0.5){
Arnoud113 1:b66e14435f70 114 motor1DC = 1;
Arnoud113 1:b66e14435f70 115
Arnoud113 1:b66e14435f70 116 ledr = 1;
Arnoud113 1:b66e14435f70 117 ledg = 1; //Blau
Arnoud113 1:b66e14435f70 118 ledb = 0;
Arnoud113 1:b66e14435f70 119 }
Arnoud113 1:b66e14435f70 120 else if (motor1<-0.5) {
Arnoud113 1:b66e14435f70 121 motor1DC = 0;
Arnoud113 1:b66e14435f70 122
Arnoud113 1:b66e14435f70 123 ledb = 1;
Arnoud113 1:b66e14435f70 124 ledr = 1;
Arnoud113 1:b66e14435f70 125 ledg = 0; //Groen
Arnoud113 1:b66e14435f70 126
Arnoud113 1:b66e14435f70 127 }
Arnoud113 1:b66e14435f70 128 else{
Arnoud113 1:b66e14435f70 129 motor1PWM = 0;
Arnoud113 1:b66e14435f70 130
Arnoud113 1:b66e14435f70 131 ledb = 1; //Rood
Arnoud113 1:b66e14435f70 132 ledr = 0;
Arnoud113 1:b66e14435f70 133 ledg = 1;
Arnoud113 1:b66e14435f70 134 }
Arnoud113 1:b66e14435f70 135
Arnoud113 1:b66e14435f70 136
Arnoud113 1:b66e14435f70 137 }
Arnoud113 2:c2c76a7a7250 138 */
Arnoud113 2:c2c76a7a7250 139
Arnoud113 2:c2c76a7a7250 140 void m1_Controller() {
Arnoud113 2:c2c76a7a7250 141 double reference = potMeter1.read();
Arnoud113 2:c2c76a7a7250 142 double position = RAD_PER_PULSE*Encoder1.getPulses();
Arnoud113 2:c2c76a7a7250 143 //float motor1 = PID( reference - position, M1_KP, M1_KI, M1_KD, M1_TS, m1_err_int, m1_prev_err, m1_f_v1, m1_f_v2, M1_F_A1, M1_F_A2, M1_F_B0, M1_F_B1, M1_F_B2 );
Arnoud113 2:c2c76a7a7250 144 //pc.printf("\r value motor1: %f. reference Pot: %f. Position: %f \n", motor1, reference, position);
Arnoud113 2:c2c76a7a7250 145 pc.printf("\r value motor1: . reference Pot: %f. Position: %f \n", reference, position);
Arnoud113 2:c2c76a7a7250 146
Arnoud113 2:c2c76a7a7250 147 motor1PWM = motor1;
Arnoud113 2:c2c76a7a7250 148
Arnoud113 2:c2c76a7a7250 149 if(motor1 > 0.5){
Arnoud113 2:c2c76a7a7250 150 motor1DC = 1;
Arnoud113 2:c2c76a7a7250 151
Arnoud113 2:c2c76a7a7250 152 ledr = 1;
Arnoud113 2:c2c76a7a7250 153 ledg = 1; //Blau
Arnoud113 2:c2c76a7a7250 154 ledb = 0;
Arnoud113 2:c2c76a7a7250 155 }
Arnoud113 2:c2c76a7a7250 156 else if (motor1<-0.5) {
Arnoud113 2:c2c76a7a7250 157 motor1DC = 0;
Arnoud113 2:c2c76a7a7250 158
Arnoud113 2:c2c76a7a7250 159 ledb = 1;
Arnoud113 2:c2c76a7a7250 160 ledr = 1;
Arnoud113 2:c2c76a7a7250 161 ledg = 0; //Groen
Arnoud113 2:c2c76a7a7250 162
Arnoud113 2:c2c76a7a7250 163 }
Arnoud113 2:c2c76a7a7250 164 else{
Arnoud113 2:c2c76a7a7250 165 motor1PWM = 0;
Arnoud113 2:c2c76a7a7250 166
Arnoud113 2:c2c76a7a7250 167 ledb = 1; //Rood
Arnoud113 2:c2c76a7a7250 168 ledr = 0;
Arnoud113 2:c2c76a7a7250 169 ledg = 1;
Arnoud113 2:c2c76a7a7250 170 }
Arnoud113 2:c2c76a7a7250 171
Arnoud113 2:c2c76a7a7250 172 }
Arnoud113 0:85ca550760e9 173
Arnoud113 0:85ca550760e9 174 int main(){
Arnoud113 0:85ca550760e9 175 pc.baud(115200);
Arnoud113 1:b66e14435f70 176 //controller.attach(&m1_Controller, CONTROLLER_TS); --> P one
Arnoud113 1:b66e14435f70 177
Arnoud113 1:b66e14435f70 178 controller.attach(&m1_Controller, M1_TS);
Arnoud113 0:85ca550760e9 179
Arnoud113 0:85ca550760e9 180 while(1){
Arnoud113 0:85ca550760e9 181 double reference = 10*potMeter1.read();
Arnoud113 0:85ca550760e9 182 double position = RAD_PER_PULSE*Encoder1.getPulses();
Arnoud113 1:b66e14435f70 183 // double motor1 = P(reference-position, M1_KP); --> old one
Arnoud113 2:c2c76a7a7250 184 //float motor1 = PI(reference - position, M1_KP, M1_KI, M1_TS, m1_err_int);
Arnoud113 2:c2c76a7a7250 185 float motor1 = PID( reference - position, M1_KP, M1_KI, M1_KD, M1_TS, m1_err_int, m1_prev_err, m1_f_v1, m1_f_v2, M1_F_A1, M1_F_A2, M1_F_B0, M1_F_B1, M1_F_B2 );
Arnoud113 2:c2c76a7a7250 186
Arnoud113 0:85ca550760e9 187 }
Arnoud113 0:85ca550760e9 188 }