pid gecomment
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed
Fork of a_pid_kal_end_def by
Diff: main.cpp
- Revision:
- 48:0a16643c9de4
- Parent:
- 47:ddaa59d48aca
--- a/main.cpp Thu Nov 03 08:43:08 2016 +0000 +++ b/main.cpp Thu Nov 03 08:49:36 2016 +0000 @@ -12,13 +12,21 @@ AnalogIn emg_biceps_right_in( A0 ); //analog in to get EMG biceps (r) in to c++ AnalogIn emg_triceps_right_in(A1); //analog in to get EMG triceps (r) in to c++ AnalogIn emg_biceps_left_in (A2); //analog in to get EMG biceps (l) in to c++ -Ticker sample_timer; //ticker -Ticker ticker_switch; //ticker + HIDScope scope(5); //open 5 channels in hidscope MODSERIAL pc(USBTX, USBRX); //pc connection DigitalOut red(LED_RED); DigitalOut green(LED_GREEN); +DigitalOut blue(LED_BLUE); + DigitalIn button_calibration_biceps (SW3); //button to start calibration biceps + DigitalIn button_calibration_triceps (SW2); // button to start calibration tricps + +//tickers +Ticker sample_timer; //ticker +Ticker ticker_switch; //ticker +Ticker ticker_calibration_biceps; +Ticker ticker_calibration_triceps; //motors DigitalOut richting_motor1(D7); ///motor 1 aansluiting op motor 1 PwmOut pwm_motor1(D6); @@ -40,6 +48,12 @@ float speedmotor1=0.18; float speedmotor2=1.0; +//calibration +const float percentage_max_triceps=0.25; +const float percentage_max_biceps =0.3; +double max_biceps; //calibration maximum biceps +double max_triceps; //calibration maximum triceps + int cw=0; int ccw=1; @@ -152,6 +166,89 @@ scope.send(); //send all the signals to the scope } + //============================================================================================================ +void calibration_biceps(){ + if (button_calibration_biceps==0){ + ticker_switch.detach(); + pc.printf("start of calibration biceps, contract maximal \n"); + red=1; + green=1; + blue=0; + + for(int n =0; n<1500;n++) //read for 2000 samples as calibration + { + double 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_abs_biceps_right=fabs(emg_filtered_high_notch_1_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_biceps = emg_filtered_biceps_right; + + } + wait(0.001f); //to sample at same freq; 1000Hz + } + cut_off_value_biceps_right=percentage_max_biceps*max_biceps; + cut_off_value_biceps_left=-cut_off_value_biceps_right; + //toggle lights + blue=!blue; + + pc.printf(" end of calibration\r\n",cut_off_value_biceps_right ); + pc.printf(" change of cv biceps: %f ",cut_off_value_biceps_right ); + + wait(0.2f); + + if (switch_signal%2==0) + {green=0; + red=1;} + + else {green=1; + red=0;} + } + ticker_switch.attach(&switch_function,1.0); + } + //======================================================================= +void calibration_triceps(){ + if(button_calibration_triceps==0){ + ticker_switch.detach(); + red=1; + green=1; + blue=0; + + pc.printf("start of calibration triceps\r\n"); + + for(int n =0; n<1500;n++) //read for 2000 samples as calibration + { + double emg_triceps_right=emg_triceps_right_in.read(); //read the emg value from the elektrodes + 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_abs_triceps_right=fabs(emg_filtered_high_notch_1_triceps_right); //fabs because float + double emg_filtered_triceps_right=filterlow_t1.step(emg_abs_triceps_right); + + if (emg_filtered_triceps_right > max_triceps) //determine what the highest reachable emg signal is + { + max_triceps = emg_filtered_triceps_right; + + } + wait(0.001f); //to sample at same freq; 1000Hz + } + cut_off_value_triceps=-percentage_max_triceps*max_triceps; + pc.printf(" end of calibration\r\n"); + pc.printf(" change of cv triceps: %f ",cut_off_value_triceps ); + blue=!blue; + wait(0.2f); + if (switch_signal%2==0) + {green=0; + red=1;} + + else {green=1; + red=0;} + } + ticker_switch.attach(&switch_function,1.0); + } + //======================================================================= //program @@ -160,9 +257,12 @@ pc.baud(115200); green=1; red=0; +blue=1; 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 ticker_switch.attach(&switch_function,1.0); +ticker_calibration_biceps.attach (&calibration_biceps,2.0); //to call calibration biceps, stop everything else +ticker_calibration_triceps.attach(&calibration_triceps,2.0); //to call calibration triceps, stop everything else pc.printf("We will start the demonstration\r\n");