pid gecomment
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed
Fork of a_pid_kal_end_def by
Diff: main.cpp
- Revision:
- 28:e2553066989e
- Parent:
- 27:b8d61361d709
- Child:
- 29:76b2cc33690c
--- a/main.cpp Fri Oct 28 08:19:14 2016 +0000 +++ b/main.cpp Fri Oct 28 08:35:31 2016 +0000 @@ -13,8 +13,8 @@ Ticker sample_timer; //ticker Ticker switch_function; //ticker -Ticker calibration_ticker_biceps //ticker -Ticker calibratio_ticker_triceps //ticker +Ticker calibration_ticker_biceps; //ticker +Ticker calibration_ticker_triceps; //ticker HIDScope scope(5); //open 3 channels in hidscope MODSERIAL pc(USBTX, USBRX); //pc connection @@ -39,31 +39,11 @@ double max_triceps; //calibration maximum triceps int n = 0; //start van de teller wordt op nul gesteld, om te kunnen switchen -//biceps arm 1, right arm double emg_biceps_right; -double emg_filtered_high_biceps_right; -double emg_abs_biceps_right; -double emg_filtered_biceps_right; -double emg_filtered_high_notch_1_biceps_right; -double emg_filtered_high_notch_1_2_biceps_right; - -//triceps arm 1, right arm double emg_triceps_right; -double emg_filtered_high_triceps_right; -double emg_abs_triceps_right; -double emg_filtered_triceps_right; -double emg_filtered_high_notch_1_triceps_right; -double emg_filtered_high_notch_1_2_triceps_right; +double emg_biceps_left; -//biceps arm 1, left arm -double emg_biceps_left; -double emg_filtered_high_biceps_left; -double emg_abs_biceps_left; -double emg_filtered_biceps_left; -double emg_filtered_high_notch_1_biceps_left; -double emg_filtered_high_notch_1_2_biceps_left; - -//before abs filtering +//Biquads defined //b1 = biceps right arm BiQuad filterhigh_b1(9.5654e-01,-1.9131e+00,9.5654e-01,-1.9112e+00,9.1498e-01); @@ -99,30 +79,30 @@ void filter(){ //biceps right arm read+filtering emg_biceps_right=emg_biceps_right_in.read(); //read the emg value from the elektrodes - emg_filtered_high_biceps_right= filterhigh_b1.step(emg_biceps_right); - emg_filtered_high_notch_1_biceps_right=filternotch1_b1.step(emg_filtered_high_biceps_right); - emg_filtered_high_notch_1_2_biceps_right=filternotch2_b1.step(emg_filtered_high_notch_1_biceps_right); - emg_abs_biceps_right=fabs(emg_filtered_high_notch_1_2_biceps_right); //fabs because float - emg_filtered_biceps_right=filterlow_b1.step(emg_abs_biceps_right); + double emg_filtered_high_biceps_right= filterhigh_b1.step(emg_biceps_right); + double emg_filtered_high_notch_1_biceps_right=filternotch1_b1.step(emg_filtered_high_biceps_right); + double emg_filtered_high_notch_1_2_biceps_right=filternotch2_b1.step(emg_filtered_high_notch_1_biceps_right); + double emg_abs_biceps_right=fabs(emg_filtered_high_notch_1_2_biceps_right); //fabs because float + double emg_filtered_biceps_right=filterlow_b1.step(emg_abs_biceps_right); //triceps right arm read+filtering emg_triceps_right=emg_triceps_right_in.read(); //read the emg value from the elektrodes - emg_filtered_high_triceps_right= filterhigh_t1.step(emg_triceps_right); - emg_filtered_high_notch_1_triceps_right=filternotch1_t1.step(emg_filtered_high_triceps_right); - emg_filtered_high_notch_1_2_triceps_right=filternotch2_t1.step(emg_filtered_high_notch_1_triceps_right); - emg_abs_triceps_right=fabs(emg_filtered_high_notch_1_2_triceps_right); //fabs because float - emg_filtered_triceps_right=filterlow_t1.step(emg_abs_triceps_right); + double emg_filtered_high_triceps_right= filterhigh_t1.step(emg_triceps_right); + double emg_filtered_high_notch_1_triceps_right=filternotch1_t1.step(emg_filtered_high_triceps_right); + double emg_filtered_high_notch_1_2_triceps_right=filternotch2_t1.step(emg_filtered_high_notch_1_triceps_right); + double emg_abs_triceps_right=fabs(emg_filtered_high_notch_1_2_triceps_right); //fabs because float + double emg_filtered_triceps_right=filterlow_t1.step(emg_abs_triceps_right); //biceps left arm read+filtering emg_biceps_left=emg_biceps_left_in.read(); //read the emg value from the elektrodes - emg_filtered_high_biceps_left= filterhigh_b2.step(emg_biceps_left); - emg_filtered_high_notch_1_biceps_left=filternotch1_b2.step(emg_filtered_high_biceps_left); - emg_filtered_high_notch_1_2_biceps_left=filternotch2_b2.step(emg_filtered_high_notch_1_biceps_left); - emg_abs_biceps_left=fabs(emg_filtered_high_notch_1_2_biceps_left); //fabs because float - emg_filtered_biceps_left=filterlow_b2.step(emg_abs_biceps_left); + double emg_filtered_high_biceps_left= filterhigh_b2.step(emg_biceps_left); + double emg_filtered_high_notch_1_biceps_left=filternotch1_b2.step(emg_filtered_high_biceps_left); + double emg_filtered_high_notch_1_2_biceps_left=filternotch2_b2.step(emg_filtered_high_notch_1_biceps_left); + double emg_abs_biceps_left=fabs(emg_filtered_high_notch_1_2_biceps_left); //fabs because float + double emg_filtered_biceps_left=filterlow_b2.step(emg_abs_biceps_left); //signal substraction of filter biceps and triceps. Biceps +,triceps - - signal_right_arm=emg_filtered_biceps_right-emg_filtered_triceps_right; + double signal_right_arm=emg_filtered_biceps_right-emg_filtered_triceps_right; //creating of on/off signal with the created on/off signals, with if statement for right arm! if (signal_right_arm>cut_off_value_biceps) @@ -164,20 +144,22 @@ { for(int n =0; n<2000;n++) //read for 5000 samples as calibration { - emg_biceps_right=emg_biceps_right_in.read(); //read the emg value from the elektrodes - emg_filtered_high_biceps_right= filterhigh.step(emg_biceps_right); //highpass - emg_abs_biceps_right=fabs(emg_filtered_high_biceps_right); //fabs because float - emg_filtered_biceps_right=filterlow.step(emg_abs_biceps_right); //lowpass to envelope - - if (emg_filtered_biceps_right > max_right_biceps) //determine what the highest reachable emg signal is + emg_biceps_right=emg_biceps_right_in.read(); //read the emg value from the elektrodes + double emg_filtered_high_biceps_right= filterhigh_b1.step(emg_biceps_right); + double emg_filtered_high_notch_1_biceps_right=filternotch1_b1.step(emg_filtered_high_biceps_right); + double emg_filtered_high_notch_1_2_biceps_right=filternotch2_b1.step(emg_filtered_high_notch_1_biceps_right); + double emg_abs_biceps_right=fabs(emg_filtered_high_notch_1_2_biceps_right); //fabs because float + double emg_filtered_biceps_right=filterlow_b1.step(emg_abs_biceps_right); + + if (emg_filtered_biceps_right > max_biceps) //determine what the highest reachable emg signal is { - max_right_biceps = emg_filtered_biceps_right; + max_biceps = emg_filtered_biceps_right; } wait(0.001f); //to sample at same freq; 1000Hz } - cut_off_value=0.2*max_right_biceps; - pc.printf(" change of cv %f ",cut_off_value ); + cut_off_value_biceps=0.2*max_biceps; + pc.printf(" change of cv %f ",cut_off_value_biceps ); } } @@ -187,7 +169,8 @@ { pc.baud(115200); sample_timer.attach(&filter, 0.001); //continously execute the EMG reader and filter, it ensures that filter and sampling is executed every 1/frequency seconds -switch_function.attach(&SwitchN,1); +switch_function.attach(&SwitchN,0.1); +calibration_ticker_biceps.attach(&calibration_biceps,0.001); //to call calibration biceps //endless loop while (true) { // zorgt er voor dat de code oneindig doorgelopen wordt @@ -196,8 +179,7 @@ { if (n%2==0) // als s ingedrukt wordt en het getal is even gebeurd het onderstaande { - pc.printf("n is even \n\r"); // print lijn "n is even" - pc.printf("up \n\r"); // print lijn "up" + richting_motor1 = 2.5; pwm_motor1 = 1; @@ -205,8 +187,7 @@ else // als s is ingedrukt maar het getal is niet even (dus oneven) gebeurdt het onderstaande { - pc.printf("n is odd \n\r"); // print lijn "n is odd" - pc.printf("left \n\r"); // print lijn "left" + richting_motor2 = 2.5; pwm_motor2 = 1; @@ -217,23 +198,20 @@ { if (n%2==0) // als d is ingedrukt en n is even dan gebeurd het volgende { - pc.printf("n is even \n\r"); // print lijn "n is even" - pc.printf("down \n\r"); // print lijn "down" + richting_motor1 = 0; pwm_motor1 = 1; } else // als d is ingedrukt maar het getal is niet even (dus oneven) gebeurdt het onderstaande { - pc.printf("n is odd \n\r"); // print lijn "n is odd" - pc.printf("right \n\r"); // print lijn "right" + richting_motor2 = 0; pwm_motor2 = 1; } } else{ - pc.printf("motor staat stil \n\r"); pwm_motor2=0; pwm_motor1=0; }