V D / Mbed 2 deprecated 20171016_PID_controller_working

20171016_PID_controller_working

Dependencies:   MODSERIAL QEI biquadFilter mbed

Fork of 20171013_PI_controller_working_tryingtoaddPID by V D

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?

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 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 }