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

Committer:
Gerth
Date:
Mon Sep 28 09:23:16 2015 +0000
Revision:
4:4d4d7343406d
Parent:
2:707567853e34
Child:
5:43e34b6d66af
added first line

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