group14 - k9 / Mbed 2 deprecated potmeter_test

aansturing van twee motoren met behulp van potmeters

Dependencies:   Encoder biquadFilter mbed

Fork of potmeter_test by Silver Eagle

main.cpp@1:718c43124021, 2015-10-19 (annotated)

Committer:
S1lverEagle
Date:
Mon Oct 19 13:00:26 2015 +0000
Revision:
1:718c43124021
Parent:
0:53f83cb0b38d
Child:
2:3835d7ff7600
biquad in code;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
S1lverEagle 0:53f83cb0b38d 1 #include "mbed.h"
S1lverEagle 0:53f83cb0b38d 2 #include "encoder.h"
S1lverEagle 0:53f83cb0b38d 3 #include "biquadFilter.h"
S1lverEagle 0:53f83cb0b38d 4
S1lverEagle 0:53f83cb0b38d 5 /*programma voor het testen van de beweging van de arm, aangestuurd door twee potmeters*/
S1lverEagle 0:53f83cb0b38d 6
S1lverEagle 0:53f83cb0b38d 7 //INPUT/OUTPUT
S1lverEagle 0:53f83cb0b38d 8 PwmOut pwm_motor1(D5); //onderste motor
S1lverEagle 0:53f83cb0b38d 9 DigitalOut dir_motor1(D4);
S1lverEagle 0:53f83cb0b38d 10 PwmOut pwm_motor2(D6); //bovenste motor
S1lverEagle 0:53f83cb0b38d 11 DigitalOut dir_motor2(D7);
S1lverEagle 0:53f83cb0b38d 12 Encoder encoder1(D13,D12,true);
S1lverEagle 0:53f83cb0b38d 13 Encoder encoder2(D10,D9,true);
S1lverEagle 0:53f83cb0b38d 14 //potmeters voor testen:
S1lverEagle 0:53f83cb0b38d 15 AnalogIn potmeter1(A4);
S1lverEagle 0:53f83cb0b38d 16 AnalogIn potmeter2(A5);
S1lverEagle 0:53f83cb0b38d 17
S1lverEagle 0:53f83cb0b38d 18 //VARIABELEN
S1lverEagle 0:53f83cb0b38d 19 Ticker control_ticker;
S1lverEagle 0:53f83cb0b38d 20 double pos_1;
S1lverEagle 0:53f83cb0b38d 21 double pos_2;
S1lverEagle 1:718c43124021 22 double m1_f_v1 = 0, m1_f_v2 = 0;
S1lverEagle 0:53f83cb0b38d 23
S1lverEagle 0:53f83cb0b38d 24 // Sample time (motor1−timestep)
S1lverEagle 0:53f83cb0b38d 25 const double m1_Ts = 0.01;
S1lverEagle 0:53f83cb0b38d 26 // Controller gains (motor1−Kp,−Ki,...)
S1lverEagle 0:53f83cb0b38d 27 const double m1_Kp = 5, m1_Ki = 0.005, m1_Kd = 2;
S1lverEagle 0:53f83cb0b38d 28 double m1_err_int = 0, m1_prev_err = 0;
S1lverEagle 0:53f83cb0b38d 29 // Derivative filter coeicients (motor1−filter−b0,−b1,...)
S1lverEagle 0:53f83cb0b38d 30 const double m1_f_a1 = -0.11175688680, m1_f_a2 = 0.17749674417, m1_f_b0 = 1.0, m1_f_b1 = 2, m1_f_b2 = 1;
S1lverEagle 0:53f83cb0b38d 31
S1lverEagle 0:53f83cb0b38d 32
S1lverEagle 0:53f83cb0b38d 33 // Sample time (motor1−timestep)
S1lverEagle 0:53f83cb0b38d 34 const double m2_Ts = 0.01;
S1lverEagle 0:53f83cb0b38d 35 // Controller gains (motor1−Kp,−Ki,...)
S1lverEagle 0:53f83cb0b38d 36 const double m2_Kp = 5, m2_Ki = 0.005, m2_Kd = 2;
S1lverEagle 0:53f83cb0b38d 37 double m2_err_int = 0, m2_prev_err = 0;
S1lverEagle 0:53f83cb0b38d 38 // Derivative filter coeicients (motor1−filter−b0,−b1,...)
S1lverEagle 0:53f83cb0b38d 39 const double m2_f_a1 = -0.11175688680, m2_f_a2 = 0.17749674417, m2_f_b0 = 1.0, m2_f_b1 = 2, m2_f_b2 = 1;
S1lverEagle 0:53f83cb0b38d 40
S1lverEagle 1:718c43124021 41 double biquad( double u, double &v1, double &v2, const double a1, const double a2,
S1lverEagle 1:718c43124021 42 const double b0, const double b1, const double b2 )
S1lverEagle 1:718c43124021 43 {
S1lverEagle 1:718c43124021 44 double v = u - a1*v1 - a2*v2;
S1lverEagle 1:718c43124021 45 double y = b0*v + b1*v1 + b2*v2;
S1lverEagle 1:718c43124021 46 v2 = v1;
S1lverEagle 1:718c43124021 47 v1 = v;
S1lverEagle 1:718c43124021 48 return y;
S1lverEagle 1:718c43124021 49 }
S1lverEagle 0:53f83cb0b38d 50
S1lverEagle 0:53f83cb0b38d 51 // Reusable PID controller with filter
S1lverEagle 1:718c43124021 52 double PID( double e, const double Kp, const double Ki, const double Kd, double Ts, double &e_int, double &e_prev, double &f_v1, double &f_v2, const double f_a1, const double f_a2, const double f_b0, const double f_b1, const double f_b2)
S1lverEagle 0:53f83cb0b38d 53 {
S1lverEagle 0:53f83cb0b38d 54 // Derivative
S1lverEagle 0:53f83cb0b38d 55 double e_der = (e - e_prev)/Ts;
S1lverEagle 1:718c43124021 56 /*biquadFilter derfilter(f_a1, f_a2, f_b0, f_b1, f_b2);
S1lverEagle 1:718c43124021 57 e_der = derfilter.step(e_der);*/
S1lverEagle 1:718c43124021 58 e_der = biquad( e_der, f_v1, f_v2, f_a1, f_a2, f_b0, f_b1, f_b2 );
S1lverEagle 0:53f83cb0b38d 59 e_prev = e;
S1lverEagle 0:53f83cb0b38d 60 // Integral
S1lverEagle 0:53f83cb0b38d 61 e_int = e_int + Ts * e;
S1lverEagle 0:53f83cb0b38d 62 // PID
S1lverEagle 0:53f83cb0b38d 63 return Kp * e + Ki * e_int + Kd * e_der;
S1lverEagle 0:53f83cb0b38d 64 }
S1lverEagle 0:53f83cb0b38d 65
S1lverEagle 0:53f83cb0b38d 66 void m1_Controller()
S1lverEagle 0:53f83cb0b38d 67 {
S1lverEagle 0:53f83cb0b38d 68 double reference = (potmeter1.read()-0.5)*4200;
S1lverEagle 0:53f83cb0b38d 69 double position = encoder1.getPosition(); // Don’t use magic numbers!
S1lverEagle 0:53f83cb0b38d 70 double error = (reference - position)/1000;
S1lverEagle 1:718c43124021 71 pos_1 = PID( error, m1_Kp, m1_Ki, m1_Kd, m1_Ts, m1_err_int, m1_prev_err, m1_f_v1, m1_f_v2, m1_f_a1, m1_f_a2, m1_f_b0, m1_f_b1, m1_f_b2 );
S1lverEagle 0:53f83cb0b38d 72 if(pos_1 > 0)
S1lverEagle 0:53f83cb0b38d 73 dir_motor1 = 0;
S1lverEagle 0:53f83cb0b38d 74 else
S1lverEagle 0:53f83cb0b38d 75 dir_motor1 = 1;
S1lverEagle 0:53f83cb0b38d 76
S1lverEagle 0:53f83cb0b38d 77 pwm_motor1.write(abs(pos_1));
S1lverEagle 0:53f83cb0b38d 78
S1lverEagle 0:53f83cb0b38d 79 } // Attach this function to a Ticker
S1lverEagle 0:53f83cb0b38d 80
S1lverEagle 0:53f83cb0b38d 81 void m2_Controller()
S1lverEagle 0:53f83cb0b38d 82 {
S1lverEagle 1:718c43124021 83 double reference = (potmeter2.read()-0.5)*4200;
S1lverEagle 1:718c43124021 84 double position = encoder2.getPosition(); // Don’t use magic numbers!
S1lverEagle 0:53f83cb0b38d 85 double error = (reference - position)/1000;
S1lverEagle 1:718c43124021 86 pos_2 = PID( error, m2_Kp, m2_Ki, m2_Kd, m2_Ts, m2_err_int, m2_prev_err, m1_f_v1, m1_f_v2, m2_f_a1, m2_f_a2, m2_f_b0, m2_f_b1, m2_f_b2 );
S1lverEagle 0:53f83cb0b38d 87 if(pos_2 > 0)
S1lverEagle 1:718c43124021 88 dir_motor2 = 1;
S1lverEagle 1:718c43124021 89 else
S1lverEagle 0:53f83cb0b38d 90 dir_motor2 = 0;
S1lverEagle 0:53f83cb0b38d 91
S1lverEagle 0:53f83cb0b38d 92 pwm_motor2.write(abs(pos_2));
S1lverEagle 0:53f83cb0b38d 93
S1lverEagle 0:53f83cb0b38d 94 } // Attach this function to a Ticker
S1lverEagle 0:53f83cb0b38d 95
S1lverEagle 0:53f83cb0b38d 96 int main()
S1lverEagle 0:53f83cb0b38d 97 {
S1lverEagle 0:53f83cb0b38d 98 pwm_motor1.period(0.0001);
S1lverEagle 0:53f83cb0b38d 99 pwm_motor2.period(0.0001);
S1lverEagle 0:53f83cb0b38d 100 control_ticker.attach(&m1_Controller, 0.01);
S1lverEagle 0:53f83cb0b38d 101 control_ticker.attach(&m2_Controller, 0.01);
S1lverEagle 0:53f83cb0b38d 102
S1lverEagle 0:53f83cb0b38d 103 while(1) {}
S1lverEagle 0:53f83cb0b38d 104 }