BioRobotics / Mbed 2 deprecated PID_control_with_lowpass

PID control with a lowpass filter to make the motor follow pot1

Dependencies:   HIDScope QEI mbed

Fork of PID_control_with_lowpass by Jasper Gerth

main.cpp@1:49f60db70b5a, 2015-09-24 (annotated)

Committer:
Gerth
Date:
Thu Sep 24 16:24:59 2015 +0000
Revision:
1:49f60db70b5a
Parent:
0:28db0afc950f
Child:
2:707567853e34
a working PID controller with low pass biquad filter, adjust controller and filter values

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Gerth 0:28db0afc950f 1 #include "mbed.h"
Gerth 0:28db0afc950f 2 #include "QEI.h"
Gerth 0:28db0afc950f 3 #include "HIDScope.h"
Gerth 0:28db0afc950f 4
Gerth 0:28db0afc950f 5 //////////////////////////////////////////////CONSTANTS AND SHIZZLE////////////////////////////////////////////////////////////////////////////
Gerth 0:28db0afc950f 6 //info uit
Gerth 0:28db0afc950f 7 HIDScope scope(3);
Gerth 0:28db0afc950f 8 //encoders
Gerth 0:28db0afc950f 9 QEI encoder1 (D12,D13,NC,32); // first b then a for clockwise +
Gerth 0:28db0afc950f 10
Gerth 0:28db0afc950f 11 //ingangen
Gerth 0:28db0afc950f 12 AnalogIn pot1(A0);
Gerth 0:28db0afc950f 13
Gerth 0:28db0afc950f 14 //uitgangen
Gerth 0:28db0afc950f 15 DigitalOut motor1_direction(D7);// draairichting motor 1 (1 is CW encoder als je daar op kijkt en CW shaft als je daar op kijkt)
Gerth 0:28db0afc950f 16 PwmOut motor1_speed_control(D6);//aanstuursnelheid motor 1
Gerth 0:28db0afc950f 17 //PwmOut motor2_speed_control(D5);
Gerth 0:28db0afc950f 18 //DigitalOut motor2_direction(D4);
Gerth 0:28db0afc950f 19 const int CW=1; //clockwise
Gerth 0:28db0afc950f 20 const int CCW=0; //counterclockwise
Gerth 0:28db0afc950f 21
Gerth 0:28db0afc950f 22 //frequencies
Gerth 0:28db0afc950f 23 //const float pwm_frequency=1000;
Gerth 0:28db0afc950f 24 const double hidscope_frequency=100;
Gerth 0:28db0afc950f 25 const double pid_control_frequency=5;
Gerth 0:28db0afc950f 26
Gerth 0:28db0afc950f 27 //tickers
Gerth 0:28db0afc950f 28 Ticker hidscope_ticker;
Gerth 0:28db0afc950f 29 Ticker pid_control_ticker;
Gerth 0:28db0afc950f 30
Gerth 0:28db0afc950f 31 //constants
Gerth 0:28db0afc950f 32 const int cpr=32*131;
Gerth 0:28db0afc950f 33 const float PI=3.1415;
Gerth 0:28db0afc950f 34 const float counttorad=((2*PI)/cpr);
Gerth 0:28db0afc950f 35
Gerth 0:28db0afc950f 36 //////////////////////////////////////////////////////////FILTERS///////////////////////////////////////////////////////
Gerth 0:28db0afc950f 37
Gerth 1:49f60db70b5a 38 //IK WEET NIET HOE GOED DEZE WAARDES KLOPPEN, VOORZICHTIG DUS!! WAARDES ZIJN TE VERKRIJGEN UIT ASN
Gerth 0:28db0afc950f 39 //Biquad #1
Gerth 0:28db0afc950f 40 const double gain_f1 = 0.004756;
Gerth 0:28db0afc950f 41 const double B_f1[] = { 1.00000000000, -0.34966580719, 1.00000000000};
Gerth 0:28db0afc950f 42 const double A_f1[] = { 1.00000000000, -1.86848759822, 0.87558390333};
Gerth 0:28db0afc950f 43
Gerth 0:28db0afc950f 44 //Biquad #2
Gerth 0:28db0afc950f 45 const double gain_f2 = 0.036454;
Gerth 0:28db0afc950f 46 const double B_f2[] = { 1.00000000000, -1.56455717372, 1.00000000000};
Gerth 0:28db0afc950f 47 const double A_f2[] = { 1.00000000000, -1.93013247482, 0.94815823632};
Gerth 0:28db0afc950f 48
Gerth 0:28db0afc950f 49
Gerth 0:28db0afc950f 50 //filterwaardes uit asn naar aparte waardes omschrijven
Gerth 0:28db0afc950f 51 double v1 = 0, v2 = 0, u = 0, y = 0;
Gerth 0:28db0afc950f 52 const double a1_f1 = gain_f1*A_f1[1], a2_f1 = gain_f1*A_f1[2], b0_f1 = gain_f1*B_f1[0], b1_f1 = gain_f1*B_f1[1], b2_f1 = gain_f1*B_f1[2]; //filter coefficients filter 1
Gerth 0:28db0afc950f 53 const double a1_f2 = gain_f2*A_f1[1], a2_f2 = gain_f2*A_f2[2], b0_f2 = gain_f2*B_f2[0], b1_f2 = gain_f2*B_f2[1], b2_f2 = gain_f2*B_f2[2]; //filter coefficients filter 1
Gerth 0:28db0afc950f 54
Gerth 0:28db0afc950f 55
Gerth 0:28db0afc950f 56 //biquad filter
Gerth 0:28db0afc950f 57 double biquad( double u, double &v1, double &v2, const double a1, const double a2,
Gerth 0:28db0afc950f 58 const double b0, const double b1, const double b2 )
Gerth 0:28db0afc950f 59 {
Gerth 0:28db0afc950f 60 double v = u - a1*v1 - a2*v2;
Gerth 0:28db0afc950f 61 double y = b0*v + b1*v1 + b2*v2;
Gerth 0:28db0afc950f 62 v2 = v1;
Gerth 0:28db0afc950f 63 v1 = v;
Gerth 0:28db0afc950f 64 return y;
Gerth 0:28db0afc950f 65 }
Gerth 0:28db0afc950f 66 ///////////////////////////////////////////////////////////CONTROLLERS//////////////////////////////////////////////////////////
Gerth 1:49f60db70b5a 67 //DIT ZIJN ZOMAAR WAARDEN, AANPASSEN DUS WAARDES BEREKENEN MET MATLAB
Gerth 0:28db0afc950f 68 const float motor1_pid_kp=0.5;
Gerth 1:49f60db70b5a 69 const float motor1_pid_ki=0.01;
Gerth 1:49f60db70b5a 70 const float motor1_pid_kd=0.01;
Gerth 1:49f60db70b5a 71
Gerth 1:49f60db70b5a 72 double motor1_error_int=0;
Gerth 1:49f60db70b5a 73 double motor1_error_prev=0;
Gerth 0:28db0afc950f 74
Gerth 1:49f60db70b5a 75 // Reusable PID controller with filter
Gerth 1:49f60db70b5a 76 double pid_control( double e, const double Kp, const double Ki, const double Kd, double Ts,//Ts is dus sampletijd
Gerth 1:49f60db70b5a 77 double &motor1_error_int, double &motor1_error_prev)
Gerth 0:28db0afc950f 78 {
Gerth 1:49f60db70b5a 79 // Derivative
Gerth 1:49f60db70b5a 80 double e_der = (e - motor1_error_prev)/Ts;
Gerth 1:49f60db70b5a 81 e_der = biquad( e_der, v1, v2, a1_f1, a2_f1, b0_f1, b1_f1, b2_f1);//twee keer hetzelfde met andere filterwaardes, dit klopt
Gerth 1:49f60db70b5a 82 e_der = biquad( e_der, v1, v2, a1_f2, a2_f2, b0_f2, b1_f2, b2_f2);//is omdat een biquad filter maar 6 polen en nulpunten heeft
Gerth 1:49f60db70b5a 83 motor1_error_prev = e;
Gerth 1:49f60db70b5a 84 // Integral
Gerth 1:49f60db70b5a 85 motor1_error_int = motor1_error_int + Ts * e;
Gerth 1:49f60db70b5a 86 // PID
Gerth 1:49f60db70b5a 87 return Kp * e + Ki * motor1_error_int + Kd * e_der;
Gerth 0:28db0afc950f 88 }
Gerth 0:28db0afc950f 89
Gerth 0:28db0afc950f 90
Gerth 0:28db0afc950f 91 ///////////////////////////////////////////////////////////GO-FLAGS AND ACTIVATION FUNCTIONS///////////////////////////////
Gerth 0:28db0afc950f 92 //go flags
Gerth 1:49f60db70b5a 93 volatile bool scopedata_go=false, pid_control_go=false;
Gerth 0:28db0afc950f 94
Gerth 0:28db0afc950f 95 //acvitator functions
Gerth 0:28db0afc950f 96
Gerth 0:28db0afc950f 97 void scopedata_activate()
Gerth 0:28db0afc950f 98 {
Gerth 0:28db0afc950f 99 scopedata_go=true;
Gerth 0:28db0afc950f 100 }
Gerth 0:28db0afc950f 101 void pid_control_activate()
Gerth 0:28db0afc950f 102 {
Gerth 0:28db0afc950f 103 pid_control_go=true;
Gerth 0:28db0afc950f 104 }
Gerth 0:28db0afc950f 105 ///////////////////////////////////////////////////////FUNCTIONS//////////////////////////////////////////////////////////////////////////
Gerth 0:28db0afc950f 106
Gerth 0:28db0afc950f 107 //scopedata
Gerth 0:28db0afc950f 108 void scopedata()
Gerth 0:28db0afc950f 109 {
Gerth 0:28db0afc950f 110 scope.set(0,2*PI*pot1.read());
Gerth 0:28db0afc950f 111 scope.set(1,counttorad*encoder1.getPulses());
Gerth 0:28db0afc950f 112 scope.set(2,motor1_speed_control.read());
Gerth 0:28db0afc950f 113 scope.send();
Gerth 0:28db0afc950f 114 }
Gerth 0:28db0afc950f 115
Gerth 0:28db0afc950f 116 //////////////////////////////////////////////////MAIN////////////////////////////////////////////////////////////////////
Gerth 0:28db0afc950f 117 int main()
Gerth 0:28db0afc950f 118 {
Gerth 0:28db0afc950f 119 //set initial shizzle
Gerth 0:28db0afc950f 120 motor1_speed_control.write(0);
Gerth 0:28db0afc950f 121
Gerth 0:28db0afc950f 122 //tickers
Gerth 0:28db0afc950f 123 hidscope_ticker.attach(&scopedata_activate,1.0/hidscope_frequency);
Gerth 1:49f60db70b5a 124 pid_control_ticker.attach(&pid_control_activate,1.0/pid_control_frequency);
Gerth 0:28db0afc950f 125
Gerth 0:28db0afc950f 126 //make variables
Gerth 0:28db0afc950f 127 volatile float signal_motor1=0;
Gerth 0:28db0afc950f 128 while(1) {
Gerth 0:28db0afc950f 129
Gerth 1:49f60db70b5a 130 //control motor 1 with a pID controller
Gerth 1:49f60db70b5a 131 if (pid_control_go==true) {
Gerth 1:49f60db70b5a 132 double error=2*PI*pot1.read()-counttorad*encoder1.getPulses();
Gerth 1:49f60db70b5a 133 double signal_motor1=pid_control(error,motor1_pid_kp,motor1_pid_ki,motor1_pid_kd,
Gerth 1:49f60db70b5a 134 1.0/pid_control_frequency,motor1_error_int,motor1_error_prev);//send error to pid controller 1.0/pid_control_frequency=sampletijd
Gerth 0:28db0afc950f 135 if (signal_motor1>=0) {//determine CW or CCW rotation
Gerth 0:28db0afc950f 136 motor1_direction.write(CW);
Gerth 0:28db0afc950f 137 } else {
Gerth 0:28db0afc950f 138 motor1_direction.write(CCW);
Gerth 0:28db0afc950f 139 }
Gerth 0:28db0afc950f 140
Gerth 0:28db0afc950f 141 if (fabs(signal_motor1)>=1) { //check if signal is <1
Gerth 0:28db0afc950f 142 signal_motor1=1;//if signal >1 make it 1 to not damage motor
Gerth 0:28db0afc950f 143 } else {
Gerth 0:28db0afc950f 144 signal_motor1=fabs(signal_motor1);// if signal<1 use signal
Gerth 0:28db0afc950f 145 }
Gerth 0:28db0afc950f 146 motor1_speed_control.write(fabs(signal_motor1));//write signal to motor
Gerth 1:49f60db70b5a 147 pid_control_go=false;
Gerth 0:28db0afc950f 148 }
Gerth 0:28db0afc950f 149 //call scopedata
Gerth 0:28db0afc950f 150 if (scopedata_go==true) {
Gerth 0:28db0afc950f 151 scopedata();
Gerth 0:28db0afc950f 152 scopedata_go=false;
Gerth 0:28db0afc950f 153 }
Gerth 0:28db0afc950f 154 return 0;
Gerth 0:28db0afc950f 155 }
Gerth 0:28db0afc950f 156 }