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@3:e1b081265e97, 2015-10-20 (annotated)

Committer:
S1lverEagle
Date:
Tue Oct 20 12:01:44 2015 +0000
Revision:
3:e1b081265e97
Parent:
2:3835d7ff7600 
twee tickers

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 0:53f83cb0b38d 22
S1lverEagle 0:53f83cb0b38d 23 // Sample time (motor1−timestep)
S1lverEagle 0:53f83cb0b38d 24 const double m1_Ts = 0.01;
S1lverEagle 0:53f83cb0b38d 25 // Controller gains (motor1−Kp,−Ki,...)
S1lverEagle 2:3835d7ff7600 26 const double m1_Kp = 0.005, m1_Ki = 0.005, m1_Kd = 0.002;
S1lverEagle 0:53f83cb0b38d 27 double m1_err_int = 0, m1_prev_err = 0;
S1lverEagle 0:53f83cb0b38d 28 // Derivative filter coeicients (motor1−filter−b0,−b1,...)
S1lverEagle 0:53f83cb0b38d 29 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 30
S1lverEagle 0:53f83cb0b38d 31
S1lverEagle 0:53f83cb0b38d 32 // Sample time (motor1−timestep)
S1lverEagle 0:53f83cb0b38d 33 const double m2_Ts = 0.01;
S1lverEagle 0:53f83cb0b38d 34 // Controller gains (motor1−Kp,−Ki,...)
S1lverEagle 2:3835d7ff7600 35 const double m2_Kp = 0.005, m2_Ki = 0.005, m2_Kd = 0.002;
S1lverEagle 0:53f83cb0b38d 36 double m2_err_int = 0, m2_prev_err = 0;
S1lverEagle 0:53f83cb0b38d 37 // Derivative filter coeicients (motor1−filter−b0,−b1,...)
S1lverEagle 0:53f83cb0b38d 38 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 39
S1lverEagle 0:53f83cb0b38d 40
S1lverEagle 0:53f83cb0b38d 41 // Reusable PID controller with filter
S1lverEagle 2:3835d7ff7600 42 double PID( double e, const double Kp, const double Ki, const double Kd, double Ts, double &e_int, double &e_prev, const double f_a1, const double f_a2, const double f_b0, const double f_b1, const double f_b2)
S1lverEagle 0:53f83cb0b38d 43 {
S1lverEagle 0:53f83cb0b38d 44 // Derivative
S1lverEagle 0:53f83cb0b38d 45 double e_der = (e - e_prev)/Ts;
S1lverEagle 2:3835d7ff7600 46 biquadFilter derfilter(f_a1, f_a2, f_b0, f_b1, f_b2);
S1lverEagle 2:3835d7ff7600 47 e_der = derfilter.step(e_der);
S1lverEagle 0:53f83cb0b38d 48 e_prev = e;
S1lverEagle 0:53f83cb0b38d 49 // Integral
S1lverEagle 0:53f83cb0b38d 50 e_int = e_int + Ts * e;
S1lverEagle 0:53f83cb0b38d 51 // PID
S1lverEagle 0:53f83cb0b38d 52 return Kp * e + Ki * e_int + Kd * e_der;
S1lverEagle 0:53f83cb0b38d 53 }
S1lverEagle 0:53f83cb0b38d 54
S1lverEagle 0:53f83cb0b38d 55 void m1_Controller()
S1lverEagle 0:53f83cb0b38d 56 {
S1lverEagle 0:53f83cb0b38d 57 double reference = (potmeter1.read()-0.5)*4200;
S1lverEagle 0:53f83cb0b38d 58 double position = encoder1.getPosition(); // Don’t use magic numbers!
S1lverEagle 2:3835d7ff7600 59 double error = (reference - position);
S1lverEagle 2:3835d7ff7600 60 pos_1 = PID( error, m1_Kp, m1_Ki, m1_Kd, m1_Ts, m1_err_int, m1_prev_err, m1_f_a1, m1_f_a2, m1_f_b0, m1_f_b1, m1_f_b2 );
S1lverEagle 0:53f83cb0b38d 61 if(pos_1 > 0)
S1lverEagle 0:53f83cb0b38d 62 dir_motor1 = 0;
S1lverEagle 0:53f83cb0b38d 63 else
S1lverEagle 0:53f83cb0b38d 64 dir_motor1 = 1;
S1lverEagle 0:53f83cb0b38d 65
S1lverEagle 0:53f83cb0b38d 66 pwm_motor1.write(abs(pos_1));
S1lverEagle 0:53f83cb0b38d 67
S1lverEagle 0:53f83cb0b38d 68 } // Attach this function to a Ticker
S1lverEagle 0:53f83cb0b38d 69
S1lverEagle 0:53f83cb0b38d 70 void m2_Controller()
S1lverEagle 0:53f83cb0b38d 71 {
S1lverEagle 1:718c43124021 72 double reference = (potmeter2.read()-0.5)*4200;
S1lverEagle 1:718c43124021 73 double position = encoder2.getPosition(); // Don’t use magic numbers!
S1lverEagle 2:3835d7ff7600 74 double error = (reference - position);
S1lverEagle 2:3835d7ff7600 75 pos_2 = PID( error, m2_Kp, m2_Ki, m2_Kd, m2_Ts, m2_err_int, m2_prev_err, m2_f_a1, m2_f_a2, m2_f_b0, m2_f_b1, m2_f_b2 );
S1lverEagle 0:53f83cb0b38d 76 if(pos_2 > 0)
S1lverEagle 1:718c43124021 77 dir_motor2 = 1;
S1lverEagle 1:718c43124021 78 else
S1lverEagle 0:53f83cb0b38d 79 dir_motor2 = 0;
S1lverEagle 0:53f83cb0b38d 80
S1lverEagle 0:53f83cb0b38d 81 pwm_motor2.write(abs(pos_2));
S1lverEagle 0:53f83cb0b38d 82
S1lverEagle 0:53f83cb0b38d 83 } // Attach this function to a Ticker
S1lverEagle 0:53f83cb0b38d 84
S1lverEagle 0:53f83cb0b38d 85 int main()
S1lverEagle 0:53f83cb0b38d 86 {
S1lverEagle 0:53f83cb0b38d 87 pwm_motor1.period(0.0001);
S1lverEagle 0:53f83cb0b38d 88 pwm_motor2.period(0.0001);
S1lverEagle 3:e1b081265e97 89 control_ticker.attach(&m1_Controller, 0.01);
S1lverEagle 3:e1b081265e97 90 wait(0.005);
S1lverEagle 3:e1b081265e97 91 control_ticker.attach(&m2_Controller, 0.01);
S1lverEagle 0:53f83cb0b38d 92
S1lverEagle 0:53f83cb0b38d 93 while(1) {}
S1lverEagle 0:53f83cb0b38d 94 }