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

Committer:
Gerth
Date:
Mon Sep 28 11:36:56 2015 +0000
Revision:
7:dbc066a7bb66
Parent:
6:60227fd3b30f 
now with the serial connection value change working;

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