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@6:60227fd3b30f, 2015-09-28 (annotated)

Committer:
Gerth
Date:
Mon Sep 28 11:23:06 2015 +0000
Revision:
6:60227fd3b30f
Parent:
5:43e34b6d66af
Child:
7:dbc066a7bb66
now you can change the controller values via the serial, when sw3 is pressed;

Who changed what in which revision?

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