pid gecomment
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed
Fork of a_pid_kal_end_def by
Diff: main.cpp
- Revision:
- 41:9ea3d5921f07
- Parent:
- 40:187ef29de53d
- Child:
- 42:7164ccd2aa14
--- a/main.cpp Tue Nov 01 08:48:08 2016 +0000 +++ b/main.cpp Tue Nov 01 09:14:55 2016 +0000 @@ -23,11 +23,12 @@ //define variables //other -int onoffsignal_rightarm=0; // on/off signal: 1; biceps activation, 0: nothing, -1, triceps activation -int switch_signal_leftarm=0; // switching between motors. -double cut_off_value_biceps =0.04; //gespecificeerd door floortje -double cut_off_value_triceps=-0.03; //gespecificeerd door floortje -double signal_right_arm; +int onoffsignal_biceps=0; // on/off signal: 1; biceps activation, 0: nothing, -1, triceps activation +int switch_signal_triceps=0; // switching between motors. +double cut_off_value_biceps_right =0.04; //gespecificeerd door floortje +double cut_off_value_biceps_left=-0.04; +double cut_off_value_triceps=0.05; //gespecificeerd door floortje +double signal_biceps_sum; int n = 0; //start van de teller wordt op nul gesteld //biceps arm 1, right arm @@ -73,23 +74,25 @@ //function teller void SwitchN() { // maakt simpele functie die 1 bij n optelt - if(switch_signal_leftarm==1) + if(switch_signal_triceps==1) { n++; green=!green; red=!red; if (n%2==0) - {pc.printf("If you contract the biceps, the robot will go up \r\n"); - pc.printf("If you contract the triceps, the robot will go down \r\n"); + {pc.printf("If you contract the biceps of right arm, the robot will go up \r\n"); + pc.printf("If you contract the biceps of left arm, the robot will go down \r\n"); + pc.printf("\r\n"); } else - {pc.printf("If you contract the biceps, the robot will go right \r\n"); - pc.printf("If you contract the triceps, the robot will go left \r\n"); - } + {pc.printf("If you contract the right arm, the robot will go right \r\n"); + pc.printf("If you contract biceps of the left rm, the robot will go left \r\n"); + pc.printf("\r\n"); } } + } //functions which are called in ticker to sample the analog signal @@ -117,39 +120,38 @@ emg_abs_biceps_left=fabs(emg_filtered_high_notch_1_biceps_left); //fabs because float 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; + //signal substraction of filter biceps and triceps. right Biceps + left biceps - + signal_biceps_sum=emg_filtered_biceps_right-emg_filtered_biceps_left; //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) - {onoffsignal_rightarm=1;} + if (signal_biceps_sum>cut_off_value_biceps_right) + {onoffsignal_biceps=1;} - else if (signal_right_arm<cut_off_value_triceps) + else if (signal_biceps_sum<cut_off_value_biceps_left) { - onoffsignal_rightarm=-1; + onoffsignal_biceps=-1; } else - {onoffsignal_rightarm=0;} + {onoffsignal_biceps=0;} //creating on/off signal for switch (left arm) - if (emg_filtered_biceps_left>cut_off_value_biceps) + if (emg_filtered_triceps_right>cut_off_value_triceps) { - switch_signal_leftarm=1; + switch_signal_triceps=1; } else { - switch_signal_leftarm=0; + switch_signal_triceps=0; } //send signals to scope scope.set(0, emg_filtered_biceps_right); //set emg signal to scope in channel 0 scope.set(1, emg_filtered_triceps_right); // set emg signal to scope in channel 1 scope.set(2, emg_filtered_biceps_left); // set emg signal to scope in channel 2 - scope.set(3, onoffsignal_rightarm); // set emg signal to scope in channel 3 - scope.set(4, switch_signal_leftarm); + scope.send(); //send all the signals to the scope } @@ -163,14 +165,14 @@ red=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 -switch_function.attach(&SwitchN,1.0); +switch_function.attach(&SwitchN,1.5); //endless loop while (true) { // zorgt er voor dat de code oneindig doorgelopen wordt - if (onoffsignal_rightarm==-1) // als s ingedrukt wordt gebeurd het volgende + if (onoffsignal_biceps==-1) // als s ingedrukt wordt gebeurd het volgende { if (n%2==0) // als s ingedrukt wordt en het getal is even gebeurd het onderstaande { @@ -187,7 +189,7 @@ } } - else if (onoffsignal_rightarm==1) // als d ingedrukt wordt gebeurd het volgende + else if (onoffsignal_biceps==1) // als d ingedrukt wordt gebeurd het volgende { if (n%2==0) // als d is ingedrukt en n is even dan gebeurd het volgende {