![](/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:
- 25:bbef09ff226b
- Parent:
- 24:6d63ad38fef7
- Child:
- 26:ac5656aa35c7
--- a/main.cpp Tue Oct 30 13:09:58 2018 +0000 +++ b/main.cpp Wed Oct 31 09:28:45 2018 +0000 @@ -32,10 +32,7 @@ //HIDScope scope( 6 ); //HIDScope set to 3x2 channels for 3 muscles, raw data + filtered //Tickers -Ticker HIDScope_tick; //Ticker for HIDScope -Ticker filter_tick; //Ticker for EMG filter -Ticker MovAg_tick; //Ticker to calculate Moving Average -Ticker Motor_tick; //Ticker motor aansturen +Ticker ticker; //Global variables const float T = 0.002f; //Ticker period @@ -76,6 +73,31 @@ 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 +// Inverse Kinematica variabelen +const double L1 = 0.208; // Hoogte van tafel tot joint 1 +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 r_trans = 0.035; // Kan gebruikt worden om om te rekenen van translation naar shaft rotation +//const double T = 0.002f; // Ticker value + +// 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 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_ii; // Reference signal for motorangle q1ref +double q2_ii; // Reference signal for motorangle q2ref + + //Functions void switch_to_calibrate() @@ -246,53 +268,60 @@ EMGFilter2(); } -void motor_control() +void inverse_kinematics() +{ + 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_ii = q1ref + q1_dot*T; + q2_ii = q2ref + q2_dot*T; + + q1ref = q1_ii; + q2ref = q2_ii; +} + +void v_des_calculate_qref() { while(emg_cal==1) //After calibration is finished, emg_cal will be 1. Otherwise 0. { - - if(movAg0>Threshold0) //If the filtered EMG signal of muscle 0 is higher than the threshold, motor1 will turn in 1 direction + if(movAg1>Threshold1) //If the filtered EMG signal of muscle 1 is higher than the threshold, motor 1 will turn { - pwmpin1 = 1; - directionpin1.write(1); //translatie vooruit - - ledr = 1; //Blue - ledb = 0; - ledg = 1; - - } - else if(movAg1>Threshold1) //If the filtered EMG signal of muscle 1 is higher than the threshold, motor2 will turn in 1 direction - { - pwmpin2 = 1; - directionpin2.write(1); //rotatie omhoog - ledr = 0; //red - ledb = 1; - ledg = 1; + v_x = 1.0; //beweging in +x direction + 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 { - - pwmpin1 = 1; - pwmpin2 = 1; - directionpin1.write(0); //translatie achteruit - directionpin2.write(0); //rotatie omlaag - - ledr = 1; //green - ledb = 1; - ledg = 0; + v_y = 1.0; //beweging in +y direction + ledr = 1; //green + ledb = 1; + ledg = 0; } - else //If not higher than the threshold, motors will not turn at all + + else if(movAg0>Threshold0) //If the filtered EMG signal of muscle 0 is higher than the threshold, motor1 will turn in 1 direction { - pwmpin1 = 0; //Motoren doen niets - pwmpin2 = 0; - + 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; } - + break; } + inverse_kinematics(); } @@ -308,10 +337,10 @@ while(true) { - filter_tick.attach(&emg_filtered,T); //EMG signals filtered every T sec. - MovAg_tick.attach(&MovAg,T); //Moving average calculation every T sec. - Motor_tick.attach(&motor_control,T); //Test motor control - + ticker.attach(&emg_filtered,T); //EMG signals filtered every T sec. + ticker.attach(&MovAg,T); //Moving average calculation 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)