![](/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:
- 27:fa493551be99
- Parent:
- 26:ac5656aa35c7
- Child:
- 28:5e54cd4525de
--- a/main.cpp Wed Oct 31 10:36:01 2018 +0000 +++ b/main.cpp Wed Oct 31 12:38:00 2018 +0000 @@ -41,7 +41,7 @@ Ticker ticker; //Global variables -const float T = 0.002f; //Ticker period +const float T = 0.002f; //Ticker period Deze wordt ook gebruikt in de PID, moet die niet anders??? //EMG filter double emg0_filt, emg1_filt, emg2_filt; //Variables for filtered EMG data channel 0, 1 and 2 @@ -116,6 +116,8 @@ double encoder2 = 0; double encoder_radians2=0; +double start_control = 0; + //--------------Functions----------------------------------------------------------------------------------------------------------------------------// @@ -286,7 +288,6 @@ 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 @@ -294,6 +295,7 @@ EMGFilter0(); EMGFilter1(); EMGFilter2(); + MovAg(); } void switch_to_calibrate() { @@ -379,6 +381,7 @@ case 3: //EMG is calibrated, robot can be set to Home position. { emg_cal = 1; //This is the setting for which the motors can begin turning in this code (!!) + wait(0.001f); break; } @@ -420,11 +423,13 @@ q1ref = q1_ii; q2ref = q2_ii; + + start_control = 1; } void v_des_calculate_qref() { - while(emg_cal==1) //After calibration is finished, emg_cal will be 1. Otherwise 0. + if(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 { @@ -457,11 +462,10 @@ ledb = 0; ledg = 0; } - - break; + + inverse_kinematics(); //Call inverse kinematics function + } - - inverse_kinematics(); //Call inverse kinematics function } //---------PID controller motor 1 + start motor 1 -----------------------------------------------------------// @@ -475,7 +479,7 @@ // Proportional part: double u_k1 = Kp1 * err1; - // Integral part + // Integral part err_integral1 = err_integral1 + err1 * T; double u_i1 = Ki1 * err_integral1; @@ -502,12 +506,17 @@ } } - void engine_control1() //Engine 1 is rotational engine, connected with left side pins +void engine_control1() //Engine 1 is rotational engine, connected with left side pins { - 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 + 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; + } } @@ -539,23 +548,29 @@ 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); - } + 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 { - 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 + 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 --------------------------// @@ -573,8 +588,7 @@ while(true) { - ticker.attach(&emg_filtered,T); //EMG signals filtered every T sec. - ticker.attach(&MovAg,T); //Moving average calculation every T sec. + 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.