Daniqe Kottelenberg / Mbed 2 deprecated another_try_from_scratch_on_emg

emg with text

Dependencies:   HIDScope MODSERIAL biquadFilter mbed

Fork of emg_import by Daniqe Kottelenberg

main.cpp@48:0a16643c9de4, 2016-11-03 (annotated)

Committer:
daniQQue
Date:
Thu Nov 03 08:49:36 2016 +0000
Revision:
48:0a16643c9de4
Parent:
47:ddaa59d48aca 
werkend, met kallibratie die doet.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
daniQQue 46:4a8889f9dc9f 1
daniQQue 46:4a8889f9dc9f 2 //=====================================================================================
daniQQue 0:34c739fcc3e0 3 //libraries
daniQQue 0:34c739fcc3e0 4 #include "mbed.h"
daniQQue 0:34c739fcc3e0 5 #include "HIDScope.h"
daniQQue 14:5b17697cf775 6 #include "BiQuad.h"
daniQQue 40:187ef29de53d 7 #include "MODSERIAL.h"
daniQQue 0:34c739fcc3e0 8
daniQQue 46:4a8889f9dc9f 9
daniQQue 46:4a8889f9dc9f 10 //=====================================================================================
daniQQue 0:34c739fcc3e0 11 //Define objects
daniQQue 40:187ef29de53d 12 AnalogIn emg_biceps_right_in( A0 ); //analog in to get EMG biceps (r) in to c++
daniQQue 40:187ef29de53d 13 AnalogIn emg_triceps_right_in(A1); //analog in to get EMG triceps (r) in to c++
daniQQue 40:187ef29de53d 14 AnalogIn emg_biceps_left_in (A2); //analog in to get EMG biceps (l) in to c++
daniQQue 48:0a16643c9de4 15
daniQQue 46:4a8889f9dc9f 16 HIDScope scope(5); //open 5 channels in hidscope
daniQQue 46:4a8889f9dc9f 17 MODSERIAL pc(USBTX, USBRX); //pc connection
daniQQue 40:187ef29de53d 18 DigitalOut red(LED_RED);
daniQQue 40:187ef29de53d 19 DigitalOut green(LED_GREEN);
daniQQue 48:0a16643c9de4 20 DigitalOut blue(LED_BLUE);
daniQQue 40:187ef29de53d 21
daniQQue 48:0a16643c9de4 22 DigitalIn button_calibration_biceps (SW3); //button to start calibration biceps
daniQQue 48:0a16643c9de4 23 DigitalIn button_calibration_triceps (SW2); // button to start calibration tricps
daniQQue 48:0a16643c9de4 24
daniQQue 48:0a16643c9de4 25 //tickers
daniQQue 48:0a16643c9de4 26 Ticker sample_timer; //ticker
daniQQue 48:0a16643c9de4 27 Ticker ticker_switch; //ticker
daniQQue 48:0a16643c9de4 28 Ticker ticker_calibration_biceps;
daniQQue 48:0a16643c9de4 29 Ticker ticker_calibration_triceps;
daniQQue 40:187ef29de53d 30 //motors
daniQQue 47:ddaa59d48aca 31 DigitalOut richting_motor1(D7); ///motor 1 aansluiting op motor 1
daniQQue 47:ddaa59d48aca 32 PwmOut pwm_motor1(D6);
daniQQue 47:ddaa59d48aca 33 DigitalOut richting_motor2(D4);
daniQQue 47:ddaa59d48aca 34 PwmOut pwm_motor2(D5);
daniQQue 0:34c739fcc3e0 35
daniQQue 46:4a8889f9dc9f 36 //=====================================================================================
daniQQue 0:34c739fcc3e0 37 //define variables
daniQQue 40:187ef29de53d 38 //other
daniQQue 47:ddaa59d48aca 39
daniQQue 45:08bddea67bd8 40 double cut_off_value_biceps_right =0.04; //gespecificeerd door floortje
daniQQue 45:08bddea67bd8 41 double cut_off_value_biceps_left=-0.04;
daniQQue 45:08bddea67bd8 42 double cut_off_value_triceps=-0.04;
daniQQue 45:08bddea67bd8 43 int switch_signal = 0; //start van de teller wordt op nul gesteld
daniQQue 45:08bddea67bd8 44
daniQQue 45:08bddea67bd8 45 int onoffsignal_biceps;
daniQQue 45:08bddea67bd8 46 int switch_signal_triceps;
daniQQue 46:4a8889f9dc9f 47
daniQQue 46:4a8889f9dc9f 48 float speedmotor1=0.18;
daniQQue 46:4a8889f9dc9f 49 float speedmotor2=1.0;
daniQQue 46:4a8889f9dc9f 50
daniQQue 48:0a16643c9de4 51 //calibration
daniQQue 48:0a16643c9de4 52 const float percentage_max_triceps=0.25;
daniQQue 48:0a16643c9de4 53 const float percentage_max_biceps =0.3;
daniQQue 48:0a16643c9de4 54 double max_biceps; //calibration maximum biceps
daniQQue 48:0a16643c9de4 55 double max_triceps; //calibration maximum triceps
daniQQue 48:0a16643c9de4 56
daniQQue 46:4a8889f9dc9f 57 int cw=0;
daniQQue 46:4a8889f9dc9f 58 int ccw=1;
daniQQue 40:187ef29de53d 59
daniQQue 44:c79e5944ac91 60 //=======================================
daniQQue 44:c79e5944ac91 61 //filter coefficients
daniQQue 40:187ef29de53d 62
daniQQue 40:187ef29de53d 63 //b1 = biceps right arm
daniQQue 40:187ef29de53d 64 BiQuad filterhigh_b1(9.5654e-01,-1.9131e+00,9.5654e-01,-1.9112e+00,9.1498e-01);
daniQQue 40:187ef29de53d 65 BiQuad filternotch1_b1 (9.9376e-01 , -1.8902e-00, 9.9376e-01 , -1.8902e-00 , 9.875e-01);
daniQQue 40:187ef29de53d 66
daniQQue 40:187ef29de53d 67 //t1= triceps right arm
daniQQue 40:187ef29de53d 68 BiQuad filterhigh_t1(9.5654e-01,-1.9131e+00,9.5654e-01,-1.9112e+00,9.1498e-01);
daniQQue 40:187ef29de53d 69 BiQuad filternotch1_t1 (9.9376e-01 , -1.8902e-00, 9.9376e-01 , -1.8902e-00 , 9.875e-01);
daniQQue 5:688b1b5530d8 70
daniQQue 40:187ef29de53d 71 //b2= biceps left arm
daniQQue 40:187ef29de53d 72 BiQuad filterhigh_b2(9.5654e-01,-1.9131e+00,9.5654e-01,-1.9112e+00,9.1498e-01);
daniQQue 40:187ef29de53d 73 BiQuad filternotch1_b2 (9.9376e-01 , -1.8902e-00, 9.9376e-01 , -1.8902e-00 , 9.875e-01);
daniQQue 40:187ef29de53d 74
daniQQue 40:187ef29de53d 75 //after abs filtering
daniQQue 40:187ef29de53d 76 BiQuad filterlow_b1 (6.2942e-06, 1.2588e-05,6.2942e-06,-1.9929e+00,9.9292e-01);
daniQQue 40:187ef29de53d 77 BiQuad filterlow_t1 (6.2942e-06, 1.2588e-05,6.2942e-06,-1.9929e+00,9.9292e-01);
daniQQue 40:187ef29de53d 78 BiQuad filterlow_b2 (6.2942e-06, 1.2588e-05,6.2942e-06,-1.9929e+00,9.9292e-01);
daniQQue 10:7255b59224cc 79
daniQQue 45:08bddea67bd8 80 //======================================================================
daniQQue 45:08bddea67bd8 81 //voids
daniQQue 45:08bddea67bd8 82 //======================================================================
daniQQue 45:08bddea67bd8 83
daniQQue 40:187ef29de53d 84 //function teller
daniQQue 45:08bddea67bd8 85 void switch_function() { // maakt simpele functie die 1 bij n optelt
daniQQue 45:08bddea67bd8 86 if(switch_signal_triceps==1)
daniQQue 40:187ef29de53d 87 {
daniQQue 45:08bddea67bd8 88 switch_signal++;
daniQQue 40:187ef29de53d 89 green=!green;
daniQQue 40:187ef29de53d 90 red=!red;
daniQQue 47:ddaa59d48aca 91
daniQQue 45:08bddea67bd8 92 if (switch_signal%2==0)
daniQQue 47:ddaa59d48aca 93 {pc.printf("If you contract the biceps, the robot will go right \r\n");
daniQQue 47:ddaa59d48aca 94 pc.printf("If you contract the triceps, the robot will go left \r\n");
daniQQue 47:ddaa59d48aca 95 pc.printf("\r\n");
daniQQue 47:ddaa59d48aca 96 }
daniQQue 47:ddaa59d48aca 97
daniQQue 47:ddaa59d48aca 98
daniQQue 47:ddaa59d48aca 99 else
daniQQue 40:187ef29de53d 100 {pc.printf("If you contract the biceps, the robot will go up \r\n");
daniQQue 40:187ef29de53d 101 pc.printf("If you contract the triceps, the robot will go down \r\n");
daniQQue 47:ddaa59d48aca 102 pc.printf("\r\n");
daniQQue 40:187ef29de53d 103 }
daniQQue 47:ddaa59d48aca 104
daniQQue 40:187ef29de53d 105 }
daniQQue 40:187ef29de53d 106 }
daniQQue 40:187ef29de53d 107
daniQQue 40:187ef29de53d 108 //functions which are called in ticker to sample the analog signal
daniQQue 40:187ef29de53d 109
daniQQue 0:34c739fcc3e0 110 void filter(){
daniQQue 40:187ef29de53d 111 //biceps right arm read+filtering
daniQQue 44:c79e5944ac91 112 double emg_biceps_right=emg_biceps_right_in.read(); //read the emg value from the elektrodes
daniQQue 44:c79e5944ac91 113 double emg_filtered_high_biceps_right= filterhigh_b1.step(emg_biceps_right);
daniQQue 44:c79e5944ac91 114 double emg_filtered_high_notch_1_biceps_right=filternotch1_b1.step(emg_filtered_high_biceps_right);
daniQQue 44:c79e5944ac91 115 double emg_abs_biceps_right=fabs(emg_filtered_high_notch_1_biceps_right); //fabs because float
daniQQue 44:c79e5944ac91 116 double emg_filtered_biceps_right=filterlow_b1.step(emg_abs_biceps_right);
daniQQue 40:187ef29de53d 117
daniQQue 40:187ef29de53d 118 //triceps right arm read+filtering
daniQQue 44:c79e5944ac91 119 double emg_triceps_right=emg_triceps_right_in.read(); //read the emg value from the elektrodes
daniQQue 44:c79e5944ac91 120 double emg_filtered_high_triceps_right= filterhigh_t1.step(emg_triceps_right);
daniQQue 44:c79e5944ac91 121 double emg_filtered_high_notch_1_triceps_right=filternotch1_t1.step(emg_filtered_high_triceps_right);
daniQQue 44:c79e5944ac91 122 double emg_abs_triceps_right=fabs(emg_filtered_high_notch_1_triceps_right); //fabs because float
daniQQue 44:c79e5944ac91 123 double emg_filtered_triceps_right=filterlow_t1.step(emg_abs_triceps_right);
daniQQue 7:42d0e38196f1 124
daniQQue 40:187ef29de53d 125 //biceps left arm read+filtering
daniQQue 44:c79e5944ac91 126 double emg_biceps_left=emg_biceps_left_in.read(); //read the emg value from the elektrodes
daniQQue 44:c79e5944ac91 127 double emg_filtered_high_biceps_left= filterhigh_b2.step(emg_biceps_left);
daniQQue 44:c79e5944ac91 128 double emg_filtered_high_notch_1_biceps_left=filternotch1_b2.step(emg_filtered_high_biceps_left);
daniQQue 44:c79e5944ac91 129 double emg_abs_biceps_left=fabs(emg_filtered_high_notch_1_biceps_left); //fabs because float
daniQQue 44:c79e5944ac91 130 double emg_filtered_biceps_left=filterlow_b2.step(emg_abs_biceps_left);
daniQQue 40:187ef29de53d 131
daniQQue 40:187ef29de53d 132 //creating of on/off signal with the created on/off signals, with if statement for right arm!
daniQQue 44:c79e5944ac91 133 //signal substraction of filter biceps and triceps. right Biceps + left biceps -
daniQQue 44:c79e5944ac91 134 double signal_biceps_sum=emg_filtered_biceps_right-emg_filtered_biceps_left;
daniQQue 44:c79e5944ac91 135 double bicepstriceps_rightarm=emg_filtered_biceps_right-emg_filtered_triceps_right;
daniQQue 45:08bddea67bd8 136
daniQQue 44:c79e5944ac91 137 //creating of on/off signal with the created on/off signals, with if statement for right arm!
daniQQue 44:c79e5944ac91 138 if (signal_biceps_sum>cut_off_value_biceps_right)
daniQQue 44:c79e5944ac91 139 {onoffsignal_biceps=1;}
daniQQue 7:42d0e38196f1 140
daniQQue 44:c79e5944ac91 141 else if (signal_biceps_sum<cut_off_value_biceps_left)
daniQQue 40:187ef29de53d 142 {
daniQQue 44:c79e5944ac91 143 onoffsignal_biceps=-1;
daniQQue 40:187ef29de53d 144 }
daniQQue 40:187ef29de53d 145
daniQQue 40:187ef29de53d 146 else
daniQQue 44:c79e5944ac91 147 {onoffsignal_biceps=0;}
daniQQue 7:42d0e38196f1 148
daniQQue 40:187ef29de53d 149 //creating on/off signal for switch (left arm)
daniQQue 40:187ef29de53d 150
daniQQue 44:c79e5944ac91 151 if (bicepstriceps_rightarm<cut_off_value_triceps)
daniQQue 40:187ef29de53d 152 {
daniQQue 44:c79e5944ac91 153 switch_signal_triceps=1;
daniQQue 40:187ef29de53d 154 }
daniQQue 40:187ef29de53d 155
daniQQue 40:187ef29de53d 156 else
daniQQue 40:187ef29de53d 157 {
daniQQue 44:c79e5944ac91 158 switch_signal_triceps=0;
daniQQue 40:187ef29de53d 159 }
daniQQue 0:34c739fcc3e0 160 //send signals to scope
daniQQue 40:187ef29de53d 161 scope.set(0, emg_filtered_biceps_right); //set emg signal to scope in channel 0
daniQQue 40:187ef29de53d 162 scope.set(1, emg_filtered_triceps_right); // set emg signal to scope in channel 1
daniQQue 40:187ef29de53d 163 scope.set(2, emg_filtered_biceps_left); // set emg signal to scope in channel 2
daniQQue 45:08bddea67bd8 164 scope.set(3, onoffsignal_biceps); // set emg signal to scope in channel 3
daniQQue 45:08bddea67bd8 165 scope.set(4, switch_signal_triceps);
daniQQue 40:187ef29de53d 166
daniQQue 40:187ef29de53d 167 scope.send(); //send all the signals to the scope
daniQQue 0:34c739fcc3e0 168 }
daniQQue 48:0a16643c9de4 169 //============================================================================================================
daniQQue 48:0a16643c9de4 170 void calibration_biceps(){
daniQQue 48:0a16643c9de4 171 if (button_calibration_biceps==0){
daniQQue 48:0a16643c9de4 172 ticker_switch.detach();
daniQQue 48:0a16643c9de4 173 pc.printf("start of calibration biceps, contract maximal \n");
daniQQue 48:0a16643c9de4 174 red=1;
daniQQue 48:0a16643c9de4 175 green=1;
daniQQue 48:0a16643c9de4 176 blue=0;
daniQQue 48:0a16643c9de4 177
daniQQue 48:0a16643c9de4 178 for(int n =0; n<1500;n++) //read for 2000 samples as calibration
daniQQue 48:0a16643c9de4 179 {
daniQQue 48:0a16643c9de4 180 double emg_biceps_right=emg_biceps_right_in.read(); //read the emg value from the elektrodes
daniQQue 48:0a16643c9de4 181 double emg_filtered_high_biceps_right= filterhigh_b1.step(emg_biceps_right);
daniQQue 48:0a16643c9de4 182 double emg_filtered_high_notch_1_biceps_right=filternotch1_b1.step(emg_filtered_high_biceps_right);
daniQQue 48:0a16643c9de4 183 double emg_abs_biceps_right=fabs(emg_filtered_high_notch_1_biceps_right); //fabs because float
daniQQue 48:0a16643c9de4 184 double emg_filtered_biceps_right=filterlow_b1.step(emg_abs_biceps_right);
daniQQue 48:0a16643c9de4 185
daniQQue 48:0a16643c9de4 186 if (emg_filtered_biceps_right > max_biceps) //determine what the highest reachable emg signal is
daniQQue 48:0a16643c9de4 187 {
daniQQue 48:0a16643c9de4 188 max_biceps = emg_filtered_biceps_right;
daniQQue 48:0a16643c9de4 189
daniQQue 48:0a16643c9de4 190 }
daniQQue 48:0a16643c9de4 191 wait(0.001f); //to sample at same freq; 1000Hz
daniQQue 48:0a16643c9de4 192 }
daniQQue 48:0a16643c9de4 193 cut_off_value_biceps_right=percentage_max_biceps*max_biceps;
daniQQue 48:0a16643c9de4 194 cut_off_value_biceps_left=-cut_off_value_biceps_right;
daniQQue 48:0a16643c9de4 195 //toggle lights
daniQQue 48:0a16643c9de4 196 blue=!blue;
daniQQue 48:0a16643c9de4 197
daniQQue 48:0a16643c9de4 198 pc.printf(" end of calibration\r\n",cut_off_value_biceps_right );
daniQQue 48:0a16643c9de4 199 pc.printf(" change of cv biceps: %f ",cut_off_value_biceps_right );
daniQQue 48:0a16643c9de4 200
daniQQue 48:0a16643c9de4 201 wait(0.2f);
daniQQue 48:0a16643c9de4 202
daniQQue 48:0a16643c9de4 203 if (switch_signal%2==0)
daniQQue 48:0a16643c9de4 204 {green=0;
daniQQue 48:0a16643c9de4 205 red=1;}
daniQQue 48:0a16643c9de4 206
daniQQue 48:0a16643c9de4 207 else {green=1;
daniQQue 48:0a16643c9de4 208 red=0;}
daniQQue 48:0a16643c9de4 209 }
daniQQue 48:0a16643c9de4 210 ticker_switch.attach(&switch_function,1.0);
daniQQue 48:0a16643c9de4 211 }
daniQQue 48:0a16643c9de4 212 //=======================================================================
daniQQue 48:0a16643c9de4 213 void calibration_triceps(){
daniQQue 48:0a16643c9de4 214 if(button_calibration_triceps==0){
daniQQue 48:0a16643c9de4 215 ticker_switch.detach();
daniQQue 48:0a16643c9de4 216 red=1;
daniQQue 48:0a16643c9de4 217 green=1;
daniQQue 48:0a16643c9de4 218 blue=0;
daniQQue 48:0a16643c9de4 219
daniQQue 48:0a16643c9de4 220 pc.printf("start of calibration triceps\r\n");
daniQQue 48:0a16643c9de4 221
daniQQue 48:0a16643c9de4 222 for(int n =0; n<1500;n++) //read for 2000 samples as calibration
daniQQue 48:0a16643c9de4 223 {
daniQQue 48:0a16643c9de4 224 double emg_triceps_right=emg_triceps_right_in.read(); //read the emg value from the elektrodes
daniQQue 48:0a16643c9de4 225 double emg_filtered_high_triceps_right= filterhigh_t1.step(emg_triceps_right);
daniQQue 48:0a16643c9de4 226 double emg_filtered_high_notch_1_triceps_right=filternotch1_t1.step(emg_filtered_high_triceps_right);
daniQQue 48:0a16643c9de4 227 double emg_abs_triceps_right=fabs(emg_filtered_high_notch_1_triceps_right); //fabs because float
daniQQue 48:0a16643c9de4 228 double emg_filtered_triceps_right=filterlow_t1.step(emg_abs_triceps_right);
daniQQue 48:0a16643c9de4 229
daniQQue 48:0a16643c9de4 230 if (emg_filtered_triceps_right > max_triceps) //determine what the highest reachable emg signal is
daniQQue 48:0a16643c9de4 231 {
daniQQue 48:0a16643c9de4 232 max_triceps = emg_filtered_triceps_right;
daniQQue 48:0a16643c9de4 233
daniQQue 48:0a16643c9de4 234 }
daniQQue 48:0a16643c9de4 235 wait(0.001f); //to sample at same freq; 1000Hz
daniQQue 48:0a16643c9de4 236 }
daniQQue 48:0a16643c9de4 237 cut_off_value_triceps=-percentage_max_triceps*max_triceps;
daniQQue 48:0a16643c9de4 238 pc.printf(" end of calibration\r\n");
daniQQue 48:0a16643c9de4 239 pc.printf(" change of cv triceps: %f ",cut_off_value_triceps );
daniQQue 48:0a16643c9de4 240 blue=!blue;
daniQQue 48:0a16643c9de4 241 wait(0.2f);
daniQQue 48:0a16643c9de4 242 if (switch_signal%2==0)
daniQQue 48:0a16643c9de4 243 {green=0;
daniQQue 48:0a16643c9de4 244 red=1;}
daniQQue 48:0a16643c9de4 245
daniQQue 48:0a16643c9de4 246 else {green=1;
daniQQue 48:0a16643c9de4 247 red=0;}
daniQQue 48:0a16643c9de4 248 }
daniQQue 48:0a16643c9de4 249 ticker_switch.attach(&switch_function,1.0);
daniQQue 48:0a16643c9de4 250 }
daniQQue 48:0a16643c9de4 251 //=======================================================================
daniQQue 0:34c739fcc3e0 252
daniQQue 0:34c739fcc3e0 253 //program
daniQQue 0:34c739fcc3e0 254
daniQQue 0:34c739fcc3e0 255 int main()
daniQQue 0:34c739fcc3e0 256 {
daniQQue 40:187ef29de53d 257 pc.baud(115200);
daniQQue 47:ddaa59d48aca 258 green=1;
daniQQue 47:ddaa59d48aca 259 red=0;
daniQQue 48:0a16643c9de4 260 blue=1;
daniQQue 40:187ef29de53d 261
daniQQue 0:34c739fcc3e0 262 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
daniQQue 45:08bddea67bd8 263 ticker_switch.attach(&switch_function,1.0);
daniQQue 48:0a16643c9de4 264 ticker_calibration_biceps.attach (&calibration_biceps,2.0); //to call calibration biceps, stop everything else
daniQQue 48:0a16643c9de4 265 ticker_calibration_triceps.attach(&calibration_triceps,2.0); //to call calibration triceps, stop everything else
daniQQue 44:c79e5944ac91 266
daniQQue 47:ddaa59d48aca 267 pc.printf("We will start the demonstration\r\n");
daniQQue 47:ddaa59d48aca 268
daniQQue 47:ddaa59d48aca 269 if (switch_signal%2==0)
daniQQue 47:ddaa59d48aca 270 {pc.printf("If you contract the biceps, the robot will go right \r\n");
daniQQue 47:ddaa59d48aca 271 pc.printf("If you contract the triceps, the robot will go left \r\n");
daniQQue 47:ddaa59d48aca 272 pc.printf("\r\n");
daniQQue 47:ddaa59d48aca 273 }
daniQQue 47:ddaa59d48aca 274
daniQQue 47:ddaa59d48aca 275
daniQQue 47:ddaa59d48aca 276 else
daniQQue 45:08bddea67bd8 277 {pc.printf("If you contract the biceps, the robot will go up \r\n");
daniQQue 45:08bddea67bd8 278 pc.printf("If you contract the triceps, the robot will go down \r\n");
daniQQue 47:ddaa59d48aca 279 pc.printf("\r\n");
daniQQue 45:08bddea67bd8 280 }
daniQQue 45:08bddea67bd8 281
daniQQue 44:c79e5944ac91 282 //==============================================================================================
daniQQue 0:34c739fcc3e0 283 //endless loop
daniQQue 0:34c739fcc3e0 284
daniQQue 40:187ef29de53d 285
daniQQue 40:187ef29de53d 286 while (true) { // zorgt er voor dat de code oneindig doorgelopen wordt
daniQQue 40:187ef29de53d 287
daniQQue 8:cd0cb71b69f2 288
daniQQue 45:08bddea67bd8 289 if (onoffsignal_biceps==-1) //left biceps contracted
daniQQue 40:187ef29de53d 290 {
daniQQue 45:08bddea67bd8 291 if (switch_signal%2==0)
daniQQue 40:187ef29de53d 292 {
daniQQue 46:4a8889f9dc9f 293 richting_motor1 = ccw; //motor 1, left
daniQQue 46:4a8889f9dc9f 294 pwm_motor1 = speedmotor1;
daniQQue 40:187ef29de53d 295
daniQQue 40:187ef29de53d 296 }
daniQQue 40:187ef29de53d 297
daniQQue 45:08bddea67bd8 298 else
daniQQue 40:187ef29de53d 299 {
daniQQue 46:4a8889f9dc9f 300 richting_motor2 = ccw; //motor 2, up
daniQQue 46:4a8889f9dc9f 301 pwm_motor2 = speedmotor2;
daniQQue 40:187ef29de53d 302
daniQQue 40:187ef29de53d 303 }
daniQQue 40:187ef29de53d 304
daniQQue 8:cd0cb71b69f2 305 }
daniQQue 45:08bddea67bd8 306 else if (onoffsignal_biceps==1) // als d ingedrukt wordt gebeurd het volgende
daniQQue 40:187ef29de53d 307 {
daniQQue 45:08bddea67bd8 308 if (switch_signal%2==0) // als d is ingedrukt en n is even dan gebeurd het volgende
daniQQue 40:187ef29de53d 309 {
daniQQue 46:4a8889f9dc9f 310 richting_motor1 = cw; //motor 1, right
daniQQue 46:4a8889f9dc9f 311 pwm_motor1 = speedmotor1;
daniQQue 40:187ef29de53d 312
daniQQue 40:187ef29de53d 313 }
daniQQue 40:187ef29de53d 314 else // als d is ingedrukt maar het getal is niet even (dus oneven) gebeurt het onderstaande
daniQQue 40:187ef29de53d 315 {
daniQQue 46:4a8889f9dc9f 316 richting_motor2 = cw; //motor 2. down
daniQQue 46:4a8889f9dc9f 317 pwm_motor2 = speedmotor2;
daniQQue 40:187ef29de53d 318
daniQQue 40:187ef29de53d 319 }
daniQQue 40:187ef29de53d 320 }
daniQQue 40:187ef29de53d 321 else{
daniQQue 40:187ef29de53d 322
daniQQue 40:187ef29de53d 323 pwm_motor2=0;
daniQQue 40:187ef29de53d 324 pwm_motor1=0;
daniQQue 40:187ef29de53d 325 }
daniQQue 40:187ef29de53d 326
daniQQue 40:187ef29de53d 327 }
daniQQue 0:34c739fcc3e0 328
daniQQue 0:34c739fcc3e0 329 }