testcode pid
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed
Fork of testPID by
Diff: main.cpp
- Revision:
- 24:56db31267f10
- Parent:
- 23:e9b1b426cde6
- Child:
- 25:49ccdc98639a
diff -r e9b1b426cde6 -r 56db31267f10 main.cpp --- a/main.cpp Tue Oct 13 12:57:33 2015 +0000 +++ b/main.cpp Wed Oct 14 13:04:54 2015 +0000 @@ -47,30 +47,95 @@ DigitalOut dir_motor1(D7); //Direction motor 1 DigitalOut dir_motor2(D4); //Direction motor 2 -//Filters -//Syntax: biquadFilter filter(a1, a2, b0, b1, b2); coefficients. Call with: filter.step(u), with u signal to be filtered. -biquadFilter highpass( 0.0009446914586925257 , 0.0018893829173850514 , 0.0009446914586925257 , -1.911196288237583 , 0.914975054072353 ); // removes DC and movement artefacts -biquadFilter notch1( 0.0009446914586925257 , 0.0018893829173850514 , 0.0009446914586925257 , -1.911196288237583 , 0.914975054072353 ); // removes 49-51 Hz power interference -biquadFilter notch2( 0.0009446914586925257 , 0.0018893829173850514 , 0.0009446914586925257 , -1.911196288237583 , 0.914975054072353 ); // -biquadFilter lowpass( 0.0009446914586925257 , 0.0018893829173850514 , 0.0009446914586925257 , -1.911196288237583 , 0.914975054072353 ); // EMG envelope -biquadFilter derfilter( 0.0009446914586925257 , 0.0018893829173850514 , 0.0009446914586925257 , -1.911196288237583 , 0.914975054072353 ); // derivative filter +//Limit Switches +DigitalIn shoulder_limit(PTA4); //using FRDM buttons for now +DigitalIn elbow_limit(PTC6); //using FRDM buttons for now + /*-------------------------------------------------------------------------------------------------------------------- ---- DECLARE VARIABLES ----------------------------------------------------------------------------------------------- --------------------------------------------------------------------------------------------------------------------*/ //EMG variables -double biceps_power; double bicepsMVC = 0; -double triceps_power; double tricepsMVC = 0; -double flexor_power; double flexorMVC = 0; -double extens_power; double extensorMVC = 0; +double emg_biceps; double biceps_power; double bicepsMVC = 0; +double emg_triceps; double triceps_power; double tricepsMVC = 0; +double emg_flexor; double flexor_power; double flexorMVC = 0; +double emg_extens; double extens_power; double extensMVC = 0; + +//PID variables +double u1; double u2; //Output of PID controller (PWM value for motor 1 and 2) +double m1_error=0; double m1_e_int=0; double m1_e_prev=0; //Error, integrated error, previous error +const double m1_kp=0; const double m1_ki=0; const double m1_kd=0; //Proportional, integral and derivative gains. + +double m2_error=0; double m2_e_int=0; double m2_e_prev=0; //Error, integrated error, previous error +const double m2_kp=0; const double m2_ki=0; const double m2_kd=0; //Proportional, integral and derivative gains. + +//highpass filter 20 Hz +const double high_b0 = 0.956543225556877; +const double high_b1 = -1.91308645113754; +const double high_b2 = 0.956543225556877; +const double high_a1 = -1.91197067426073; +const double high_a2 = 0.9149758348014341; + +//notchfilter 50Hz +/* ** Primary Filter (H1)** +Filter Arithmetic = Floating Point (Double Precision) +Architecture = IIR +Response = Bandstop +Method = Butterworth +Biquad = Yes +Stable = Yes +Sampling Frequency = 500Hz +Filter Order = 2 + +Band Frequencies (Hz) Att/Ripple (dB) Biquad #1 Biquad #2 + +1 0, 48 0.001 Gain = 1.000000 Gain = 0.973674 +2 49, 51 -60.000 B = [ 1.00000000000, -1.61816176147, 1.00000000000] B = [ 1.00000000000, -1.61816176147, 1.00000000000] +3 52, 250 0.001 A = [ 1.00000000000, -1.58071559235, 0.97319685401] A = [ 1.00000000000, -1.61244708381, 0.97415116257] +*/ + +//biquad 1 +const double notch1gain = 1.000000; +const double notch1_b0 = 1; +const double notch1_b1 = -1.61816176147 * notch1gain; +const double notch1_b2 = 1.00000000000 * notch1gain; +const double notch1_a1 = -1.58071559235 * notch1gain; +const double notch1_a2 = 0.97319685401 * notch1gain; + +//biquad 2 +const double notch2gain = 0.973674; +const double notch2_b0 = 1 * notch2gain; +const double notch2_b1 = -1.61816176147 * notch2gain; +const double notch2_b2 = 1.00000000000 * notch2gain; +const double notch2_a1 = -1.61244708381 * notch2gain; +const double notch2_a2 = 0.97415116257 * notch2gain; + +//lowpass filter 7 Hz - envelop +const double low_b0 = 0.000119046743110057; +const double low_b1 = 0.000238093486220118; +const double low_b2 = 0.000119046743110057; +const double low_a1 = -1.968902268531908; +const double low_a2 = 0.9693784555043481; /*-------------------------------------------------------------------------------------------------------------------- ----- DECLARE FUNCTION NAMES------------------------------------------------------------------------------------------- +---- Filters --------------------------------------------------------------------------------------------------------- --------------------------------------------------------------------------------------------------------------------*/ -void keep_in_range(float * in, float min, float max); + +//Using biquadFilter library +//Syntax: biquadFilter filter(a1, a2, b0, b1, b2); coefficients. Call with: filter.step(u), with u signal to be filtered. +biquadFilter highpass( high_a1 , high_a2 , high_b0 , high_b1 , high_b2 ); // removes DC and movement artefacts +biquadFilter notch1( notch1_a1 , notch1_a2 , notch1_b0 , notch1_b1 , notch1_b2 ); // removes 49-51 Hz power interference +biquadFilter notch2( notch2_a1 , notch2_a2 , notch2_b0 , notch2_b1 , notch2_b2 ); // +biquadFilter lowpass( low_a1 , low_a2 , low_b0 , low_b1 , low_b2 ); // EMG envelope +biquadFilter derfilter( 0.0009446914586925257 , 0.0018893829173850514 , 0.0009446914586925257 , -1.911196288237583 , 0.914975054072353 ); // derivative filter + +/*-------------------------------------------------------------------------------------------------------------------- +---- DECLARE FUNCTION NAMES ------------------------------------------------------------------------------------------ +--------------------------------------------------------------------------------------------------------------------*/ +void keep_in_range(double * in, double min, double max); void sample_filter(void); void control(); void calibrate_emg(int muscle); @@ -90,12 +155,13 @@ int main() { - // make a menu, user has to initiated next step + pc.baud(115200); + // make a menu, user has to initiate next step mainMenu(); caliMenu(); // Menu function calibrate_arm(); //Start Calibration start_sampling(); //500 Hz EMG - calibrate_emg(1); + calibrate_emg(1); //calibrate muscle 1 start_control(); //100 Hz control //maybe some stop commands when a button is pressed: detach from timers. @@ -117,18 +183,6 @@ ---- FUNCTIONS ------------------------------------------------------------------------------------------------------- --------------------------------------------------------------------------------------------------------------------*/ -//Start sampling -void start_sampling(void) -{ - sample_timer.attach(&sample_filter,SAMPLE_RATE); //500 Hz EMG -} - -//stop sampling -void stop_sampling(void) -{ - sample_timer.detach(); -} - //Sample and Filter void sample_filter(void) { @@ -155,51 +209,72 @@ i=0; } + for(int x = 0; x < window; x++){ + avg1 += bicepsbuffer[x]; + } + avg1 = avg1/window; */ - - } //Send arm to mechanical limits, and set encoder to 0. Then send arm to starting position. void calibrate_arm(void) { + bool hit = true; dir_motor1=1; //ccw dir_motor2=1; //ccw - while(limitswitch != hit){ + while(shoulder_limit != hit){ pwm_motor1.write(0.4); + } + Encoder1.reset(); + + while(elbow_limit != hit){ pwm_motor2.write(0.4); } - encoder1.reset(); - encoder2.reset(); + Encoder2.reset(); } //EMG Maximum Voluntary Contraction measurement void calibrate_emg(int muscle) { - if(muscle==1){ start_sampling(); - wait(1); + + //double sampletime=0; + //sampletime=+SAMPLE_RATE; + // + // if(sampletime<5) for(int index=0; index<2500;index++){ //measure 5 seconds@500hz = 2500 samples - if(biceps_power>bicepsMVC){ + if(muscle==1){ + + if(biceps_power>bicepsMVC){ bicepsMVC=biceps_power; - } + } + } + + if(muscle==2){ + + if(triceps_power>tricepsMVC){ + tricepsMVC=triceps_power; + } + } + + if(muscle==3){ + + if(flexor_power>flexorMVC){ + flexorMVC=flexor_power; + } + } + + if(muscle==4){ + + if(extens_power>extensMVC){ + extensMVC=extens_power; + } + } } stop_sampling(); - } - - if(muscle==2){ - start_sampling(); - wait(1); - - for(int index=0; index<2500;index++){ //measure 5 seconds@500hz = 2500 samples - if(triceps_power>tricepsMVC){ - tricepsMVC=triceps_power; - } - } - stop_sampling(); - } + } @@ -207,11 +282,11 @@ double pid(double error, double kp, double ki, double kd,double &e_int, double &e_prev) { // Derivative - double e_der = (error-e_prev)/CONTROL_RATE; + double e_der = (error-e_prev)/ CONTROL_RATE; e_der = derfilter.step(e_der); e_prev = error; // Integral - e_int = e_int+CONTROL_RATE*error; + e_int = e_int + CONTROL_RATE * error; // PID return kp*error + ki*e_int + kd * e_der; @@ -229,9 +304,9 @@ //inverse kinematics (pos error to angle error) //PID controller - double error=0;double kp=0;double ki=0;double kd=0;double e_int=0;double e_prev=0; - u1=pid(error,kp,ki,kd,e_int,e_prev); //motor 1 - u2=pid(error,kp,ki,kd,e_int,e_prev); //motor 2 + + u1=pid(m1_error,m1_kp,m1_ki,m1_kd,m1_e_int,m1_e_prev); //motor 1 + u2=pid(m2_error,m2_kp,m2_ki,m2_kd,m2_e_int,m2_e_prev); //motor 2 keep_in_range(&u1,-1,1); //keep u between -1 and 1, sign = direction, PWM = 0-1 keep_in_range(&u2,-1,1); @@ -265,10 +340,25 @@ } +void mainMenu(){}; +void caliMenu(){}; + +//Start sampling +void start_sampling(void) +{ + sample_timer.attach(&sample_filter,SAMPLE_RATE); //500 Hz EMG +} + +//stop sampling +void stop_sampling(void) +{ + sample_timer.detach(); +} + //Start control void start_control(void) { - control_timer.attach(&control,SAMPLE_RATE); //100 Hz control + control_timer.attach(&control,CONTROL_RATE); //100 Hz control } //stop control @@ -285,7 +375,7 @@ //keeps input limited between min max -void keep_in_range(float * in, float min, float max) +void keep_in_range(double * in, double min, double max) { *in > min ? *in < max? : *in = max: *in = min; } \ No newline at end of file