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

Committer:
Gerth
Date:
Thu Sep 24 16:04:43 2015 +0000
Revision:
0:28db0afc950f
Child:
1:49f60db70b5a
simple P control with lowpass;

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 const double filter_pot1_frequency=200;
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 Ticker filter_pot1_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 0:28db0afc950f 40 //another more powerful low pass filter
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 0:28db0afc950f 69 const float motor1_pid_kp=0.5;
Gerth 0:28db0afc950f 70
Gerth 0:28db0afc950f 71 //re usable P controller
Gerth 0:28db0afc950f 72 double p_control(float error,float kp)
Gerth 0:28db0afc950f 73 {
Gerth 0:28db0afc950f 74 return (error*kp);
Gerth 0:28db0afc950f 75 }
Gerth 0:28db0afc950f 76
Gerth 0:28db0afc950f 77
Gerth 0:28db0afc950f 78 ///////////////////////////////////////////////////////////GO-FLAGS AND ACTIVATION FUNCTIONS///////////////////////////////
Gerth 0:28db0afc950f 79 //go flags
Gerth 0:28db0afc950f 80 volatile bool scopedata_go=false, pid_control_go=false, filter_pot1_go=false;
Gerth 0:28db0afc950f 81
Gerth 0:28db0afc950f 82 //acvitator functions
Gerth 0:28db0afc950f 83
Gerth 0:28db0afc950f 84 void scopedata_activate()
Gerth 0:28db0afc950f 85 {
Gerth 0:28db0afc950f 86 scopedata_go=true;
Gerth 0:28db0afc950f 87 }
Gerth 0:28db0afc950f 88 void pid_control_activate()
Gerth 0:28db0afc950f 89 {
Gerth 0:28db0afc950f 90 pid_control_go=true;
Gerth 0:28db0afc950f 91 }
Gerth 0:28db0afc950f 92 void filter_pot1_activate()
Gerth 0:28db0afc950f 93 {
Gerth 0:28db0afc950f 94 filter_pot1_go=true;
Gerth 0:28db0afc950f 95 }
Gerth 0:28db0afc950f 96 ///////////////////////////////////////////////////////FUNCTIONS//////////////////////////////////////////////////////////////////////////
Gerth 0:28db0afc950f 97
Gerth 0:28db0afc950f 98 //scopedata
Gerth 0:28db0afc950f 99 void scopedata()
Gerth 0:28db0afc950f 100 {
Gerth 0:28db0afc950f 101 scope.set(0,2*PI*pot1.read());
Gerth 0:28db0afc950f 102 scope.set(1,counttorad*encoder1.getPulses());
Gerth 0:28db0afc950f 103 scope.set(2,motor1_speed_control.read());
Gerth 0:28db0afc950f 104 scope.send();
Gerth 0:28db0afc950f 105 }
Gerth 0:28db0afc950f 106
Gerth 0:28db0afc950f 107 //////////////////////////////////////////////////MAIN////////////////////////////////////////////////////////////////////
Gerth 0:28db0afc950f 108 int main()
Gerth 0:28db0afc950f 109 {
Gerth 0:28db0afc950f 110 //set initial shizzle
Gerth 0:28db0afc950f 111 //motor1_speed_control.period(1.0/pwm_frequency);
Gerth 0:28db0afc950f 112 motor1_speed_control.write(0);
Gerth 0:28db0afc950f 113
Gerth 0:28db0afc950f 114 //tickers
Gerth 0:28db0afc950f 115 hidscope_ticker.attach(&scopedata_activate,1.0/hidscope_frequency);
Gerth 0:28db0afc950f 116 pid_control_ticker.attach(&pid_control_activate,1.0/p_control_frequency);
Gerth 0:28db0afc950f 117 filter_pot1_ticker.attach(&filter_pot1_activate,1.0/filter_pot1_frequency);
Gerth 0:28db0afc950f 118
Gerth 0:28db0afc950f 119 //make variables
Gerth 0:28db0afc950f 120 volatile float signal_motor1=0;
Gerth 0:28db0afc950f 121 volatile double pot1_filtered=0;
Gerth 0:28db0afc950f 122 while(1) {
Gerth 0:28db0afc950f 123
Gerth 0:28db0afc950f 124 //filter signal with low pass biquad filter
Gerth 0:28db0afc950f 125 if (filter_pot1_go==true) {
Gerth 0:28db0afc950f 126 double pass1, pass2;
Gerth 0:28db0afc950f 127 pass1=biquad(pot1.read(),v1,v2,a1_f1, a2_f1, b0_f1, b1_f1, b2_f1);
Gerth 0:28db0afc950f 128 pass2=biquad(pass1,v1,v2,a1_f2, a2_f2, b0_f2, b1_f2, b2_f2);
Gerth 0:28db0afc950f 129 pot1_filtered=pass2;
Gerth 0:28db0afc950f 130 filter_pot1_go=false;
Gerth 0:28db0afc950f 131 }
Gerth 0:28db0afc950f 132
Gerth 0:28db0afc950f 133 //control motor 1 with a p controller
Gerth 0:28db0afc950f 134 if (p_control_go==true) {
Gerth 0:28db0afc950f 135 double error=2*PI*pot1_filtered-counttorad*encoder1.getPulses();
Gerth 0:28db0afc950f 136 double signal_motor1=pid_control(error,motor1_pid_kp);//send error to pid controller
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 0:28db0afc950f 149 p_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 0:28db0afc950f 156 return 0;
Gerth 0:28db0afc950f 157 }
Gerth 0:28db0afc950f 158 }