![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
final version
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed
Fork of Project_script_union_final by
Diff: main.cpp
- Revision:
- 28:5e54cd4525de
- Parent:
- 27:fa493551be99
- Child:
- 29:6cd4f5ac57c4
--- a/main.cpp Wed Oct 31 12:38:00 2018 +0000 +++ b/main.cpp Thu Nov 01 17:29:06 2018 +0000 @@ -3,6 +3,7 @@ #include "BiQuad.h" #include "HIDScope.h" #include <math.h> +#include "QEI.h" //ATTENTION: set mBed to version 151 // set QEI to version 0, (gebruiken wij (nog) niet, is voor encoder) @@ -14,19 +15,14 @@ AnalogIn emg1_in (A1); //Second raw EMG signal input AnalogIn emg2_in (A2); //Third raw EMG signal input -InterruptIn encoderA1 (D9); -InterruptIn encoderB1 (D8); -InterruptIn encoderA2 (D12); -InterruptIn encoderB2 (D13); - InterruptIn button1 (D10); InterruptIn button2 (D11); -DigitalOut directionpin1 (D7); -DigitalOut directionpin2 (D4); +DigitalOut directionpin1 (D4); +DigitalOut directionpin2 (D7); -PwmOut pwmpin1 (D6); -PwmOut pwmpin2 (D5); +PwmOut pwmpin1 (D5); +PwmOut pwmpin2 (D6); DigitalOut ledr (LED_RED); DigitalOut ledb (LED_BLUE); @@ -34,11 +30,15 @@ MODSERIAL pc(USBTX, USBRX); //Serial communication to see if the code works step by step, turn on if hidscope is off +QEI encoder2 (D9, D8, NC, 8400,QEI::X4_ENCODING); +QEI encoder1 (D12, D13, NC, 8400,QEI::X4_ENCODING); //HIDScope scope( 6 ); //HIDScope set to 3x2 channels for 3 muscles, raw data + filtered //Tickers -Ticker ticker; +Ticker func_tick; +Ticker movag_tick; +Ticker emg_tick; //Global variables const float T = 0.002f; //Ticker period Deze wordt ook gebruikt in de PID, moet die niet anders??? @@ -79,224 +79,66 @@ BiQuad emg2band3( 1.00000e+00, -1.99999e+00, 9.99994e-01, -1.93552e+00, 9.39358e-01 ); BiQuad notch3( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 ); //Notch filter +//Global variables +const float T = 0.002f; //Ticker period Deze wordt ook gebruikt in de PID, moet die niet anders??? +const float T2 = 0.002f; + // Inverse Kinematica variables const double L1 = 0.208; // Hoogte van tafel tot joint 1 -const double L2 = 0.288; // Hoogte van tafel tot joint 2 +//const double L2 = 0.288; // Hoogte van tafel tot joint 2 const double L3 = 0.212; // Lengte van de arm const double L4 = 0.089; // Afstand van achterkant base tot joint 1 -const double L5 = 0.030; // Afstand van joint 1 naar joint 2 +//const double L5 = 0.030; // Afstand van joint 1 naar joint 2 const double r_trans = 0.035; // Kan gebruikt worden om om te rekenen van translation naar shaft rotation // Variërende variabelen inverse kinematics: -double q1ref = 0; // Huidige motorhoek van joint 1 zoals bepaald uit referentiesignaal --> checken of het goede type is -double q2ref = 0; // Huidige motorhoek van joint 2 zoals bepaald uit referentiesignaal --> checken of het goede type is +double q1ref = 0.0; // Huidige motorhoek van joint 1 zoals bepaald uit referentiesignaal --> checken of het goede type is +double q2ref = 0.0; // Huidige motorhoek van joint 2 zoals bepaald uit referentiesignaal --> checken of het goede type is double v_x; // Desired velocity end effector in x direction --> Determined by EMG signals double v_y; // Desired velocity end effector in y direction --> Determined by EMG signals double Lq1; // Translatieafstand als gevolg van motor rotation joint 1 double Cq2; // Joint angle of the system (corrected for gear ratio 1:5) -double q1_dot; // Benodigde hoeksnelheid van motor 1 om v_des te bereiken -double q2_dot; // Benodigde hoeksnelheid van motor 2 om v_des te bereiken +double q1_dot=0.0; // Benodigde hoeksnelheid van motor 1 om v_des te bereiken +double q2_dot=0.0; // Benodigde hoeksnelheid van motor 2 om v_des te bereiken -double q1_ii; // Reference signal for motorangle q1ref -double q2_ii; // Reference signal for motorangle q2ref +double q1_ii=0.0; // Reference signal for motorangle q1ref +double q2_ii=0.0; // Reference signal for motorangle q2ref //Variables PID controller double PI = 3.14159; -double Kp1 = 17.5; //Motor 1 +double Kp1 = 20.0; //Motor 1 eerst 17.5 , nu 1 double Ki1 = 1.02; -double Kd1 = 23.2; -double encoder1 = 0; +double Kd1 = 1.0; double encoder_radians1=0; +double err_integral1 = 0; +double err_prev1, err_prev2; +double err1, err2; -double Kp2 = 17.5; //Motor 2 +//BiQuad LowPassFilterDer1(0.0640, 0.1279, 0.0640, -1.1683, 0.4241); +//BiQuad LowPassFilterDer2(0.0640, 0.1279, 0.0640, -1.1683, 0.4241); +BiQuad LowPassFilterDer1( 1.12160e-01, 1.12160e-01, 0.00000e+00, -7.75680e-01, 0.00000e+00 ); //sample frequency 500 Hz, cutoff 20Hz low pass +BiQuad LowPassFilterDer2( 1.12160e-01, 1.12160e-01, 0.00000e+00, -7.75680e-01, 0.00000e+00 ); + + +double Kp2 = 20.0; //Motor 2 eerst 17.5, nu 1 double Ki2 = 1.02; -double Kd2 = 23.2; -double encoder2 = 0; +double Kd2 = 1.0; double encoder_radians2=0; +double err_integral2 = 0; +double u1, u2; -double start_control = 0; + +int start_control = 0; +double emg_cal = 1; //--------------Functions----------------------------------------------------------------------------------------------------------------------------// -//------------------ Encoder motor 1 --------------------------------// - -void encoderA1_rise() -{ - if(encoderB1==false) - { - encoder1++; - } - else - { - encoder1--; - } -} - -void encoderA1_fall() -{ - if(encoderB1==true) - { - encoder1++; - } - else - { - encoder1--; - } -} - -void encoderB1_rise() -{ - if(encoderA1==true) - { - encoder1++; - } - else - { - encoder1--; - } -} - -void encoderB1_fall() -{ - if(encoderA1==false) - { - encoder1++; - } - else - { - encoder1--; - } -} - -void encoder_count1() -{ - encoderA1.rise(&encoderA1_rise); - encoderA1.fall(&encoderA1_fall); - encoderB1.rise(&encoderB1_rise); - encoderB1.fall(&encoderB1_fall); -} - -//------------------ Encoder motor 2 --------------------------------// - -void encoderA2_rise() -{ - if(encoderB2==false) - { - encoder2++; - } - else - { - encoder2--; - } -} - -void encoderA2_fall() -{ - if(encoderB2==true) - { - encoder2++; - } - else - { - encoder2--; - } -} - -void encoderB2_rise() -{ - if(encoderA2==true) - { - encoder2++; - } - else - { - encoder2--; - } -} - -void encoderB2_fall() -{ - if(encoderA2==false) - { - encoder2++; - } - else - { - encoder2--; - } -} - -void encoder_count2() -{ - encoderA2.rise(&encoderA2_rise); - encoderA2.fall(&encoderA2_fall); - encoderB2.rise(&encoderB2_rise); - encoderB2.fall(&encoderB2_fall); -} - //------------------ Filter EMG + Calibration EMG --------------------------------// -void EMGFilter0() -{ - emg0_raw = emg0_in.read(); //give name to raw EMG0 data calve - emg0_filt_x = emg0filter.step(emg0_raw); //Use biquad chain to filter raw EMG data - emg0_filt = abs(emg0_filt_x); //rectifier. LET OP: volgorde filter: band-notch-rectifier. Eerst band-rect-notch, stel er komt iets raars uit, dan Notch uit de biquad chain halen en aparte chain voor aanmaken. -} - -void EMGFilter1() -{ - emg1_raw = emg1_in.read(); //give name to raw EMG1 data bicep 1 - emg1_filt_x = emg1filter.step(emg1_raw); //Use biquad chain to filter raw EMG data - emg1_filt = abs(emg1_filt_x); //rectifier. LET OP: volgorde filter: band-notch-rectifier. Eerst band-rect-notch. -} - -void EMGFilter2() -{ - emg2_raw = emg2_in.read(); //Give name to raw EMG1 data bicep 2 - emg2_filt_x = emg2filter.step(emg2_raw); //Use biquad chain to filter raw EMG data - emg2_filt = abs(emg2_filt_x); //Rectifier. LET OP: volgorde filter: band-notch-rectifier. -} - -void MovAg() //Calculate moving average (MovAg) -{ - for (int i = windowsize-1; i>=0; i--) //Make arrays for the last datapoints of the filtered signals - { - StoreArray0[i] = StoreArray0[i-1]; //Shifts the i'th element one place to the right, this makes it "rolling or moving" average. - StoreArray1[i] = StoreArray1[i-1]; - StoreArray2[i] = StoreArray2[i-1]; - } - - StoreArray0[0] = emg0_filt; //Stores the latest datapoint of the filtered signal in the first element of the array - StoreArray1[0] = emg1_filt; - StoreArray2[0] = emg2_filt; - - sum1 = 0.0; - sum2 = 0.0; - sum3 = 0.0; - - for(int a = 0; a<= windowsize-1; a++) //Sums the elements in the arrays - { - sum1 += StoreArray0[a]; - sum2 += StoreArray1[a]; - sum3 += StoreArray2[a]; - } - - movAg0 = sum1/windowsize; //calculates an average in the array - movAg1 = sum2/windowsize; - movAg2 = sum3/windowsize; -} - -void emg_filtered() //Call all filter functions -{ - EMGFilter0(); - EMGFilter1(); - EMGFilter2(); - MovAg(); -} void switch_to_calibrate() { x++; //Every time function gets called, x increases. Every button press --> new calibration state. @@ -391,6 +233,65 @@ } } } + +void EMGFilter0() +{ + emg0_raw = emg0_in.read(); //give name to raw EMG0 data + emg0_filt_x = emg0filter.step(emg0_raw); //Use biquad chain to filter raw EMG data + emg0_filt = abs(emg0_filt_x); //rectifier. LET OP: volgorde filter: band-notch-rectifier. Eerst band-rect-notch, stel er komt iets raars uit, dan Notch uit de biquad chain halen en aparte chain voor aanmaken. +} + +void EMGFilter1() +{ + emg1_raw = emg1_in.read(); //give name to raw EMG1 data + emg1_filt_x = emg1filter.step(emg1_raw); //Use biquad chain to filter raw EMG data + emg1_filt = abs(emg1_filt_x); //rectifier. LET OP: volgorde filter: band-notch-rectifier. Eerst band-rect-notch. +} + +void EMGFilter2() +{ + emg2_raw = emg2_in.read(); //Give name to raw EMG1 data + emg2_filt_x = emg2filter.step(emg2_raw); //Use biquad chain to filter raw EMG data + emg2_filt = abs(emg2_filt_x); //Rectifier. LET OP: volgorde filter: band-notch-rectifier. +} + +void MovAg() //Calculate moving average (MovAg), klopt nog niet!! +{ + for (int i = windowsize-1; i>=0; i--) //Make arrays for the last datapoints of the filtered signals + { + StoreArray0[i] = StoreArray0[i-1]; //Shifts the i'th element one place to the right, this makes it "rolling or moving" average. + StoreArray1[i] = StoreArray1[i-1]; + StoreArray2[i] = StoreArray2[i-1]; + } + + StoreArray0[0] = emg0_filt; //Stores the latest datapoint of the filtered signal in the first element of the array + StoreArray1[0] = emg1_filt; + StoreArray2[0] = emg2_filt; + + sum1 = 0.0; + sum2 = 0.0; + sum3 = 0.0; + + for(int a = 0; a<= windowsize-1; a++) //Sums the elements in the arrays + { + sum1 += StoreArray0[a]; + sum2 += StoreArray1[a]; + sum3 += StoreArray2[a]; + } + + movAg0 = sum1/windowsize; //calculates an average in the array + movAg1 = sum2/windowsize; + movAg2 = sum3/windowsize; + //serial getallen sturen, als het 1 getal is gaat hier wat fout, als het een reeks is dan gaat er bij de input naar HIDscope wat fout. +} + +void emg_filtered() //Call all filter functions +{ + EMGFilter0(); + EMGFilter1(); + EMGFilter2(); +} + /* void HIDScope_sample() { @@ -408,39 +309,142 @@ } */ + +//---------PID controller 1 + 2 + motor control 1 & 2-----------------------------------------------------------// +void PID_control1() +{ + //pc.printf("ik doe het, PDI \n\r"); + + // Proportional part: + double u_k1 = Kp1 * err1; + + //Integral part + err_integral1 = err_integral1 + err1 * T; + double u_i1 = Ki1 * err_integral1; + + // Derivative part + double err_derivative1 = (err1 - err_prev1)/T; + double filtered_err_derivative1 = LowPassFilterDer1.step(err_derivative1); + double u_d1 = Kd1 * filtered_err_derivative1; + err_prev1 = err1; + + + // Sum all parts and return it + u1 = u_k1 + u_i1 + u_d1; +} + +void PID_control2() +{ + //pc.printf("ik doe het, PDI \n\r"); + + // Proportional part: + double u_k2 = Kp2 * err2; + + //Integral part + err_integral2 = err_integral2 + err2 * T; + double u_i2 = Ki2 * err_integral2; + + // Derivative part + double err_derivative2 = (err2 - err_prev2)/T; + double filtered_err_derivative2 = LowPassFilterDer2.step(err_derivative2); + double u_d2 = Kd2 * filtered_err_derivative2; + err_prev2 = err2; + + + // Sum all parts and return it + u2 = u_k2 + u_i2 + u_d2; +} + +void engine_control1() //Engine 1 is translational engine, connected with left side pins +{ + //pc.printf("ik doe het, engine control 1\n\r"); + encoder_radians1 = (double)encoder1.getPulses()*(2.0*PI)/8400.0; + //pc.printf("encoder1 %f \n\r", (float)encoder1.getPulses()); + //pc.printf("encoder_radians1 %f \n\r",(float) encoder_radians1); + err1 = q1ref - encoder_radians1; + //pc.printf("err1 = %f\n\r",err1); + PID_control1(); //PID controller function call + + //pc.printf("u1 = %f\n\r",u1); + + //if(encoder1.getPulses()<12000 && encoder1.getPulses()>-1) //limits translation in counts, eerst 12600 + //{ + pwmpin1 = fabs(u1); //u_total moet nog geschaald worden om in de motor gevoerd te worden!!! + directionpin1.write(u1<0); + //} + //else + // { + // pwmpin1 = 0; + // } +} + +void engine_control2() //Engine 2 is rotational engine, connected with right side wires +{ + encoder_radians2 = (float)encoder2.getPulses()*(2.0*PI)/8400.0; + //pc.printf("encoder2 %f \n\r",(float)encoder2.getPulses()); + //pc.printf("encoder_radians2 %f \n\r",(float)encoder_radians2); + err2 = q2ref - encoder_radians2; + //pc.printf("err2 = %f\n\r",err2); + PID_control2(); //PID controller function call + //pc.printf("u2 = %f\n\r",u2); + + //if(encoder2.getPulses()<-5250 && encoder2.getPulses()>5250) //limits rotation, in counts + // { + pwmpin2 = fabs(u2); //u_total moet nog geschaald worden om in de motor gevoerd te worden!!! + directionpin2.write(u2>0); + // } + //else + // { + // pwmpin2 = 0; + // } +} + //------------------ Inversed Kinematics --------------------------------// void inverse_kinematics() { + + //pc.printf("v_x is %f en v_y is %f\n\r",v_x, v_y); + Lq1 = q1ref*r_trans; Cq2 = q2ref/5.0; - q1_dot = v_x + (v_y*(L1 + L3*sin(Cq2)))/(L4 + Lq1 + L3*cos(Cq2)); - q2_dot = v_y/(L4 + Lq1 + L3*cos(Cq2)); + q1_dot = v_x + (v_y*(L1 + L3*sin(Cq2)))/(L4 + Lq1 + L3*cos(Cq2)); //RKI systeem + q2_dot = v_y/(L4 + Lq1 + L3*cos(Cq2)); // - q1_ii = q1ref + q1_dot*T; - q2_ii = q2ref + q2_dot*T; + q1_ii = q1ref + (q1_dot/r_trans)*T; //Omgezet naar motorhoeken + q2_ii = q2ref + (q2_dot*5.0)*T; q1ref = q1_ii; q2ref = q2_ii; - start_control = 1; + + //pc.printf("q1ref is %f en q2ref is %f\n\r",q1ref, q2ref); + + + //start_control = 1; + engine_control1(); + engine_control2(); } void v_des_calculate_qref() { - if(emg_cal==1) //After calibration is finished, emg_cal will be 1. Otherwise 0. + while(emg_cal==1) //After calibration is finished, emg_cal will be 1. Otherwise 0. { if(movAg1>Threshold1) //If the filtered EMG signal of muscle 1 is higher than the threshold, motor 1 will turn { - v_x = 1.0; //beweging in +x direction + v_x = 0.5; //beweging in +x direction + v_y = 0.0; + ledr = 0; //red ledb = 1; ledg = 1; } else if(movAg2>Threshold2) //If the filtered EMG signal of muscle 2 is higher than the threshold, motor 1 and 2 will turn { - v_y = 1.0; //beweging in +y direction + v_y = 0.5; //beweging in +y direction + v_x = 0.0; + ledr = 1; //green ledb = 1; ledg = 0; @@ -450,14 +454,17 @@ { v_x = -v_x; v_y = -v_y; + ledr = 1; //Blue ledb = 0; ledg = 1; } + else //If not higher than the threshold, motors will not turn at all { v_x = 0; v_y = 0; + ledr = 0; //white ledb = 0; ledg = 0; @@ -465,113 +472,11 @@ inverse_kinematics(); //Call inverse kinematics function + break; } } -//---------PID controller motor 1 + start motor 1 -----------------------------------------------------------// -double PID_controller1(double err1) -{ - static double err_integral1 = 0; - static double err_prev1 = err1; // initialization with this value only done once! - - static BiQuad LowPassFilter1(0.0640, 0.1279, 0.0640, -1.1683, 0.4241); - // Proportional part: - double u_k1 = Kp1 * err1; - - // Integral part - err_integral1 = err_integral1 + err1 * T; - double u_i1 = Ki1 * err_integral1; - - // Derivative part - double err_derivative1 = (err1 - err_prev1)/T; - double filtered_err_derivative1 = LowPassFilter1.step(err_derivative1); - double u_d1 = Kd1 * filtered_err_derivative1; - err_prev1 = err1; - - // Sum all parts and return it - return u_k1 + u_i1 + u_d1; -} - -void start_your_engines1(double u1) -{ - if(encoder1<5250 && encoder1>-5250) //limits rotation, in counts - { - pwmpin1 = fabs(u1); //u_total moet nog geschaald worden om in de motor gevoerd te worden!!! - directionpin1.write(u1 < 0.0f); - } - else - { - pwmpin1 = 0; - } -} - -void engine_control1() //Engine 1 is rotational engine, connected with left side pins -{ - while(start_control == 1) - { - encoder_radians1 = encoder1*(2*PI)/8400; - double err1 = q1ref - encoder_radians1; - double u1 = PID_controller1(err1); //PID controller function call - start_your_engines1(u1); //Call start_your_engines function - - break; - } -} - - - -//---------PID controller motor 1 + start motor 1 -----------------------------------------------------------// -double PID_controller2(double err2) -{ - static double err_integral2 = 0; - static double err_prev2 = err2; // initialization with this value only done once! - - static BiQuad LowPassFilter2(0.0640, 0.1279, 0.0640, -1.1683, 0.4241); - - // Proportional part: - double u_k2 = Kp2 * err2; - - // Integral part - err_integral2 = err_integral2 + err2 * T; - double u_i2 = Ki2 * err_integral2; - - // Derivative part - double err_derivative2 = (err2 - err_prev2)/T; - double filtered_err_derivative2 = LowPassFilter2.step(err_derivative2); - double u_d2 = Kd2 * filtered_err_derivative2; - err_prev2 = err2; - - // Sum all parts and return it - return u_k2 + u_i2 + u_d2; -} - -void start_your_engines2(double u2) -{ - if(encoder2<12600 && encoder2>-1) //limits translation in counts - { - pwmpin2 = fabs(u2); //u_total moet nog geschaald worden om in de motor gevoerd te worden!!! - directionpin2.write(u2 < 0.0f); - } - else - { - pwmpin2 = 0; - } - -} - -void engine_control2() //Engine 2 is translational engine, connected with right side wires -{ - while(start_control == 1) - { - encoder_radians2 = encoder2*(2*PI)/8400; - double err2 = q2ref - encoder_radians2; - double u2 = PID_controller2(err2); //PID controller function call - start_your_engines2(u2); //Call start_your_engines function - - break; - } -} //------------------ Start main function --------------------------// @@ -586,18 +491,21 @@ emg1filter.add( &emg1band1 ).add( &emg1band2 ).add( &emg1band3 ).add( ¬ch2 ); emg2filter.add( &emg2band1 ).add( &emg2band2 ).add( &emg2band3 ).add( ¬ch3 ); + emg_tick.attach(&emg_filtered,T); //EMG signals filtered + moving average every T sec. + movag_tick.attach(&MovAg,T); + func_tick.attach(&v_des_calculate_qref,T); //v_des determined every T + engine_control1_tick.attach(&engine_control1,T); + engine_control2_tick.attach(&engine_control2,T); + + // HIDScope_tick.attach(&HIDScope_sample,T); //EMG signals raw + filtered to HIDScope every T sec. + + button1.rise(switch_to_calibrate); //Switch state of calibration (which muscle) + //wait(0.2f); //Wait to avoid bouncing of button + button2.rise(calibrate); //Calibrate threshold for 3 muscles + //wait(0.2f); //Wait to avoid bouncing of button + while(true) { - ticker.attach(&emg_filtered,T); //EMG signals filtered + moving average every T sec. - ticker.attach(&v_des_calculate_qref,T); //v_des determined every T - - // HIDScope_tick.attach(&HIDScope_sample,T); //EMG signals raw + filtered to HIDScope every T sec. - - button1.rise(switch_to_calibrate); //Switch state of calibration (which muscle) - wait(0.2f); //Wait to avoid bouncing of button - button2.rise(calibrate); //Calibrate threshold for 3 muscles - wait(0.2f); //Wait to avoid bouncing of button - pc.printf("x is %i\n\r",x); pc.printf("Movag0 = %f , Movag1 = %f, Movag2 = %f \n\r",movAg0, movAg1, movAg2); pc.printf("Thresh0 = %f , Thresh1 = %f, Thresh2 = %f \n\r",Threshold0, Threshold1, Threshold2);