sibren vuurberg / Mbed 2 deprecated another_try_from_scratch_on_emg

now with PID controler XXXD

Dependencies:   HIDScope MODSERIAL QEI biquadFilter mbed

Fork of another_try_from_scratch_on_emg by Daniqe Kottelenberg

main.cpp@54:fb72c58a7150, 2016-11-03 (annotated)

Committer:
sivuu
Date:
Thu Nov 03 12:18:53 2016 +0000
Revision:
54:fb72c58a7150
Parent:
53:6b91f69fa2dc 
voordat er gestest wordt zonder PID controler

Who changed what in which revision?

UserRevisionLine numberNew contents of line
daniQQue 46:4a8889f9dc9f 1 //=====================================================================================
daniQQue 0:34c739fcc3e0 2 //libraries
daniQQue 49:818a0e90ed9c 3 #include "mbed.h" //mbed revision 113
daniQQue 49:818a0e90ed9c 4 #include "HIDScope.h" //Hidscope by Tom Lankhorst
daniQQue 49:818a0e90ed9c 5 #include "BiQuad.h" //BiQuad by Tom Lankhorst
daniQQue 49:818a0e90ed9c 6 #include "MODSERIAL.h" //Modserial
sivuu 50:2c03357de7cc 7 #include "QEI.h" //QEI library for the encoders
daniQQue 0:34c739fcc3e0 8
daniQQue 46:4a8889f9dc9f 9
daniQQue 46:4a8889f9dc9f 10 //=====================================================================================
daniQQue 0:34c739fcc3e0 11 //Define objects
daniQQue 49:818a0e90ed9c 12
daniQQue 49:818a0e90ed9c 13 //EMG
sivuu 50:2c03357de7cc 14 AnalogIn emg_biceps_right_in (A0); //analog in to get EMG biceps (r) in to c++
daniQQue 40:187ef29de53d 15 AnalogIn emg_triceps_right_in(A1); //analog in to get EMG triceps (r) in to c++
daniQQue 40:187ef29de53d 16 AnalogIn emg_biceps_left_in (A2); //analog in to get EMG biceps (l) in to c++
daniQQue 49:818a0e90ed9c 17
daniQQue 49:818a0e90ed9c 18 //Tickers
daniQQue 49:818a0e90ed9c 19 Ticker sample_timer; //ticker for EMG signal sampling, analog becomes digital
daniQQue 49:818a0e90ed9c 20 Ticker ticker_switch; //ticker for switch, every second it is possible to switch
sivuu 50:2c03357de7cc 21 Ticker ticker_referenceangle; //ticker for the reference angle
sivuu 50:2c03357de7cc 22 Ticker ticker_controllerm1; //ticker for the controller (PID) of motor 1
daniQQue 49:818a0e90ed9c 23
daniQQue 49:818a0e90ed9c 24 //Monitoring
daniQQue 46:4a8889f9dc9f 25 HIDScope scope(5); //open 5 channels in hidscope
daniQQue 46:4a8889f9dc9f 26 MODSERIAL pc(USBTX, USBRX); //pc connection
daniQQue 49:818a0e90ed9c 27 DigitalOut red(LED_RED); //LED on K64F board, 1 is out; 0 is on
daniQQue 49:818a0e90ed9c 28 DigitalOut green(LED_GREEN); //LED on K64f board, 1 is out; o is on
daniQQue 40:187ef29de53d 29
daniQQue 40:187ef29de53d 30 //motors
sivuu 50:2c03357de7cc 31 DigitalOut richting_motor1(D7); //motor 1 connected to motor 1 at k64f board; for turningtable
daniQQue 47:ddaa59d48aca 32 PwmOut pwm_motor1(D6);
sivuu 50:2c03357de7cc 33 DigitalOut richting_motor2(D4); //motor 2 connected to motor 2 at k64f board; for linear actuator
daniQQue 47:ddaa59d48aca 34 PwmOut pwm_motor2(D5);
daniQQue 0:34c739fcc3e0 35
sivuu 50:2c03357de7cc 36 //encoders
sivuu 50:2c03357de7cc 37 DigitalIn encoder1A(D13);
sivuu 50:2c03357de7cc 38 DigitalIn encoder1B(D12);
sivuu 50:2c03357de7cc 39 DigitalIn encoder2A(D11);
sivuu 50:2c03357de7cc 40 DigitalIn encoder2B(D10);
sivuu 50:2c03357de7cc 41
sivuu 50:2c03357de7cc 42 //controller
sivuu 50:2c03357de7cc 43 BiQuad PID_controller;
sivuu 50:2c03357de7cc 44
sivuu 50:2c03357de7cc 45
daniQQue 46:4a8889f9dc9f 46 //=====================================================================================
daniQQue 0:34c739fcc3e0 47 //define variables
daniQQue 47:ddaa59d48aca 48
sivuu 50:2c03357de7cc 49 //thresholds
sivuu 50:2c03357de7cc 50 double treshold_biceps_right = 0.04; //common values that work.
sivuu 52:0135deb3b07f 51 double treshold_biceps_left = -0.04; // tested on multiple persons
sivuu 52:0135deb3b07f 52 double treshold_triceps = -0.04; //triceps and left biceps is specified negative, thus negative treshold
daniQQue 49:818a0e90ed9c 53
daniQQue 49:818a0e90ed9c 54 //on/off and switch signals
daniQQue 49:818a0e90ed9c 55 int switch_signal = 0; //start of counter, switch made by even and odd numbers
daniQQue 49:818a0e90ed9c 56 int onoffsignal_biceps;
daniQQue 45:08bddea67bd8 57 int switch_signal_triceps;
daniQQue 46:4a8889f9dc9f 58
daniQQue 49:818a0e90ed9c 59 //motorvariables
daniQQue 49:818a0e90ed9c 60 float speedmotor1=0.18; //speed of motor 1 is 0.18pwm at start
daniQQue 49:818a0e90ed9c 61 float speedmotor2=1.0; //speed of motor 2 is 1.0 pwm at start
daniQQue 46:4a8889f9dc9f 62
daniQQue 49:818a0e90ed9c 63 int cw=0; //clockwise direction
daniQQue 49:818a0e90ed9c 64 int ccw=1; //counterclockwise direction
sivuu 50:2c03357de7cc 65
sivuu 50:2c03357de7cc 66 //encoder
sivuu 50:2c03357de7cc 67 int counts_encoder1;
sivuu 50:2c03357de7cc 68 //int counts_encoder2;
sivuu 50:2c03357de7cc 69 float rev_counts_motor1;
sivuu 50:2c03357de7cc 70 float rev_counts_motor1_rad;
sivuu 51:b344a92b6a5f 71 const float gearboxratio=131.25; // gearboxratio van encoder naar motor
sivuu 51:b344a92b6a5f 72 const float rev_rond=64.0; // aantal revoluties per omgang van de encoder
sivuu 50:2c03357de7cc 73
sivuu 50:2c03357de7cc 74 //reference
sivuu 50:2c03357de7cc 75 volatile float d_ref = 0;
sivuu 50:2c03357de7cc 76 const float w_ref = 3;
sivuu 50:2c03357de7cc 77 const double Ts = 0.001;
sivuu 50:2c03357de7cc 78
sivuu 50:2c03357de7cc 79 //controller
sivuu 50:2c03357de7cc 80 const double Kp = 0.3823;
sivuu 50:2c03357de7cc 81 const double Ki = 0.1279;
sivuu 50:2c03357de7cc 82 const double Kd = 0.2519;
sivuu 50:2c03357de7cc 83 const double N = 100;
sivuu 50:2c03357de7cc 84 volatile double error1;
sivuu 50:2c03357de7cc 85 volatile double controlOutput;
daniQQue 44:c79e5944ac91 86 //=======================================
daniQQue 44:c79e5944ac91 87 //filter coefficients
daniQQue 40:187ef29de53d 88
daniQQue 40:187ef29de53d 89 //b1 = biceps right arm
daniQQue 49:818a0e90ed9c 90 BiQuad filterhigh_b1(9.5654e-01,-1.9131e+00,9.5654e-01,-1.9112e+00,9.1498e-01); // second order highpass filter, with frequency of?
daniQQue 49:818a0e90ed9c 91 BiQuad filternotch1_b1 (9.9376e-01 , -1.8902e-00, 9.9376e-01 , -1.8902e-00 , 9.875e-01); // second order notch filter, with frequency of?
daniQQue 40:187ef29de53d 92
daniQQue 40:187ef29de53d 93 //t1= triceps right arm
daniQQue 49:818a0e90ed9c 94 BiQuad filterhigh_t1(9.5654e-01,-1.9131e+00,9.5654e-01,-1.9112e+00,9.1498e-01); // second order highpass filter, with frequency of?
daniQQue 49:818a0e90ed9c 95 BiQuad filternotch1_t1 (9.9376e-01 , -1.8902e-00, 9.9376e-01 , -1.8902e-00 , 9.875e-01); // second order notch filter, with frequency of?
daniQQue 5:688b1b5530d8 96
daniQQue 40:187ef29de53d 97 //b2= biceps left arm
daniQQue 49:818a0e90ed9c 98 BiQuad filterhigh_b2(9.5654e-01,-1.9131e+00,9.5654e-01,-1.9112e+00,9.1498e-01); // second order highpass filter, with frequency of?
daniQQue 49:818a0e90ed9c 99 BiQuad filternotch1_b2 (9.9376e-01 , -1.8902e-00, 9.9376e-01 , -1.8902e-00 , 9.875e-01); // second order notch filter, with frequency of?
daniQQue 40:187ef29de53d 100
daniQQue 40:187ef29de53d 101 //after abs filtering
daniQQue 49:818a0e90ed9c 102 BiQuad filterlow_b1 (6.2942e-06, 1.2588e-05,6.2942e-06,-1.9929e+00,9.9292e-01); // second order lowpass filter, with frequency of?
daniQQue 49:818a0e90ed9c 103 BiQuad filterlow_t1 (6.2942e-06, 1.2588e-05,6.2942e-06,-1.9929e+00,9.9292e-01); // second order lowpass filter, with frequency of?
daniQQue 49:818a0e90ed9c 104 BiQuad filterlow_b2 (6.2942e-06, 1.2588e-05,6.2942e-06,-1.9929e+00,9.9292e-01); // second order lowpass filter, with frequency of?
daniQQue 10:7255b59224cc 105
daniQQue 45:08bddea67bd8 106 //======================================================================
daniQQue 45:08bddea67bd8 107 //voids
daniQQue 45:08bddea67bd8 108 //======================================================================
daniQQue 45:08bddea67bd8 109
daniQQue 40:187ef29de53d 110 //function teller
daniQQue 49:818a0e90ed9c 111 void switch_function() { // The switch function. Makes it possible to switch between the motors. It simply adds one at switch_signal.
daniQQue 45:08bddea67bd8 112 if(switch_signal_triceps==1)
daniQQue 40:187ef29de53d 113 {
daniQQue 45:08bddea67bd8 114 switch_signal++;
daniQQue 49:818a0e90ed9c 115
daniQQue 49:818a0e90ed9c 116 // To monitor what is happening: we will show the text in putty and change led color from red to green or vice versa.
daniQQue 49:818a0e90ed9c 117
daniQQue 40:187ef29de53d 118 green=!green;
daniQQue 40:187ef29de53d 119 red=!red;
daniQQue 47:ddaa59d48aca 120
daniQQue 45:08bddea67bd8 121 if (switch_signal%2==0)
daniQQue 49:818a0e90ed9c 122 {pc.printf("If you contract the biceps, the robot will go right \r\n");
daniQQue 47:ddaa59d48aca 123 pc.printf("If you contract the triceps, the robot will go left \r\n");
daniQQue 47:ddaa59d48aca 124 pc.printf("\r\n");
daniQQue 47:ddaa59d48aca 125 }
daniQQue 47:ddaa59d48aca 126
daniQQue 47:ddaa59d48aca 127
daniQQue 47:ddaa59d48aca 128 else
daniQQue 40:187ef29de53d 129 {pc.printf("If you contract the biceps, the robot will go up \r\n");
daniQQue 40:187ef29de53d 130 pc.printf("If you contract the triceps, the robot will go down \r\n");
daniQQue 47:ddaa59d48aca 131 pc.printf("\r\n");
daniQQue 40:187ef29de53d 132 }
daniQQue 47:ddaa59d48aca 133
daniQQue 40:187ef29de53d 134 }
daniQQue 40:187ef29de53d 135 }
daniQQue 49:818a0e90ed9c 136
daniQQue 49:818a0e90ed9c 137 //======================================================================
daniQQue 49:818a0e90ed9c 138 //functions which are called in ticker to sample the analog signal and make the on/off and switch signal.
daniQQue 40:187ef29de53d 139
daniQQue 0:34c739fcc3e0 140 void filter(){
daniQQue 40:187ef29de53d 141 //biceps right arm read+filtering
daniQQue 49:818a0e90ed9c 142 double emg_biceps_right=emg_biceps_right_in.read(); //read the emg value from the elektrodes
daniQQue 49:818a0e90ed9c 143 double emg_filtered_high_biceps_right= filterhigh_b1.step(emg_biceps_right); //high pass filter, to remove offset
daniQQue 49:818a0e90ed9c 144 double emg_filtered_high_notch_1_biceps_right=filternotch1_b1.step(emg_filtered_high_biceps_right); //notch filter, to remove noise
daniQQue 49:818a0e90ed9c 145 double emg_abs_biceps_right=fabs(emg_filtered_high_notch_1_biceps_right); //rectify the signal, fabs because float
daniQQue 49:818a0e90ed9c 146 double emg_filtered_biceps_right=filterlow_b1.step(emg_abs_biceps_right); //low pass filter to envelope the signal
daniQQue 40:187ef29de53d 147
daniQQue 40:187ef29de53d 148 //triceps right arm read+filtering
daniQQue 49:818a0e90ed9c 149 double emg_triceps_right=emg_triceps_right_in.read(); //read the emg value from the elektrodes
daniQQue 49:818a0e90ed9c 150 double emg_filtered_high_triceps_right= filterhigh_t1.step(emg_triceps_right); //high pass filter, to remove offset
daniQQue 49:818a0e90ed9c 151 double emg_filtered_high_notch_1_triceps_right=filternotch1_t1.step(emg_filtered_high_triceps_right); //notch filter, to remove noise
daniQQue 49:818a0e90ed9c 152 double emg_abs_triceps_right=fabs(emg_filtered_high_notch_1_triceps_right); //rectify the signal, fabs because float
daniQQue 49:818a0e90ed9c 153 double emg_filtered_triceps_right=filterlow_t1.step(emg_abs_triceps_right); //low pass filter to envelope the signal
daniQQue 7:42d0e38196f1 154
daniQQue 40:187ef29de53d 155 //biceps left arm read+filtering
daniQQue 49:818a0e90ed9c 156 double emg_biceps_left=emg_biceps_left_in.read(); //read the emg value from the elektrodes
daniQQue 49:818a0e90ed9c 157 double emg_filtered_high_biceps_left= filterhigh_b2.step(emg_biceps_left); //high pass filter, to remove offset
daniQQue 49:818a0e90ed9c 158 double emg_filtered_high_notch_1_biceps_left=filternotch1_b2.step(emg_filtered_high_biceps_left); //notch filter, to remove noise
daniQQue 49:818a0e90ed9c 159 double emg_abs_biceps_left=fabs(emg_filtered_high_notch_1_biceps_left); //rectify the signal, fabs because float
daniQQue 49:818a0e90ed9c 160 double emg_filtered_biceps_left=filterlow_b2.step(emg_abs_biceps_left); //low pass filter to envelope the signal
daniQQue 40:187ef29de53d 161
daniQQue 40:187ef29de53d 162 //creating of on/off signal with the created on/off signals, with if statement for right arm!
daniQQue 44:c79e5944ac91 163 //signal substraction of filter biceps and triceps. right Biceps + left biceps -
daniQQue 44:c79e5944ac91 164 double signal_biceps_sum=emg_filtered_biceps_right-emg_filtered_biceps_left;
daniQQue 44:c79e5944ac91 165 double bicepstriceps_rightarm=emg_filtered_biceps_right-emg_filtered_triceps_right;
daniQQue 45:08bddea67bd8 166
daniQQue 44:c79e5944ac91 167 //creating of on/off signal with the created on/off signals, with if statement for right arm!
daniQQue 49:818a0e90ed9c 168 if (signal_biceps_sum>treshold_biceps_right)
daniQQue 49:818a0e90ed9c 169 {onoffsignal_biceps=1;}
daniQQue 7:42d0e38196f1 170
daniQQue 49:818a0e90ed9c 171 else if (signal_biceps_sum<treshold_biceps_left)
daniQQue 49:818a0e90ed9c 172 { onoffsignal_biceps=-1; }
daniQQue 40:187ef29de53d 173
daniQQue 49:818a0e90ed9c 174 else
daniQQue 49:818a0e90ed9c 175 {onoffsignal_biceps=0;}
daniQQue 7:42d0e38196f1 176
daniQQue 40:187ef29de53d 177 //creating on/off signal for switch (left arm)
daniQQue 40:187ef29de53d 178
daniQQue 49:818a0e90ed9c 179 if (bicepstriceps_rightarm<treshold_triceps)
daniQQue 49:818a0e90ed9c 180 { switch_signal_triceps=1; }
daniQQue 40:187ef29de53d 181
daniQQue 40:187ef29de53d 182 else
daniQQue 49:818a0e90ed9c 183 { switch_signal_triceps=0; }
daniQQue 49:818a0e90ed9c 184
daniQQue 49:818a0e90ed9c 185 //send signals to scope to monitor the EMG signals
daniQQue 49:818a0e90ed9c 186 scope.set(0, emg_filtered_biceps_right); //set emg signal of right biceps to scope in channel 0
daniQQue 49:818a0e90ed9c 187 scope.set(1, emg_filtered_triceps_right); // set emg signal of right triceps to scope in channel 1
daniQQue 49:818a0e90ed9c 188 scope.set(2, emg_filtered_biceps_left); // set emg signal of left biceps to scope in channel 2
daniQQue 49:818a0e90ed9c 189 scope.set(3, onoffsignal_biceps); // set on/off signal for the motors to scope in channel 3
daniQQue 49:818a0e90ed9c 190 scope.set(4, switch_signal_triceps); // set the switch signal to scope in channel 4
daniQQue 40:187ef29de53d 191
daniQQue 40:187ef29de53d 192 scope.send(); //send all the signals to the scope
daniQQue 0:34c739fcc3e0 193 }
sivuu 50:2c03357de7cc 194
sivuu 50:2c03357de7cc 195 void reference(){
sivuu 52:0135deb3b07f 196 if (onoffsignal_biceps==-1 && switch_signal%2==0){ //switch even //right biceps contracted{
sivuu 50:2c03357de7cc 197 d_ref = d_ref + w_ref * Ts;
sivuu 50:2c03357de7cc 198 }
sivuu 54:fb72c58a7150 199 if (d_ref > 8){
sivuu 54:fb72c58a7150 200 d_ref = 8;
sivuu 50:2c03357de7cc 201 //d_ref_const_cw = 1;
sivuu 50:2c03357de7cc 202 }
sivuu 50:2c03357de7cc 203 else{
sivuu 50:2c03357de7cc 204 d_ref = d_ref;
sivuu 50:2c03357de7cc 205 }
sivuu 50:2c03357de7cc 206
sivuu 52:0135deb3b07f 207 if (onoffsignal_biceps==1 && switch_signal%2==0){ //switch even //left biceps contracted{
sivuu 50:2c03357de7cc 208 d_ref = d_ref - w_ref * Ts;
sivuu 50:2c03357de7cc 209 }
sivuu 54:fb72c58a7150 210 if (d_ref < -8){
sivuu 54:fb72c58a7150 211 d_ref = -8;
sivuu 50:2c03357de7cc 212
sivuu 50:2c03357de7cc 213 }
sivuu 50:2c03357de7cc 214 else{
sivuu 50:2c03357de7cc 215 d_ref = d_ref;
sivuu 50:2c03357de7cc 216 }
sivuu 50:2c03357de7cc 217
sivuu 50:2c03357de7cc 218 }
sivuu 50:2c03357de7cc 219
sivuu 50:2c03357de7cc 220 void m1_controller(){
sivuu 50:2c03357de7cc 221 error1 = d_ref-rev_counts_motor1_rad;
sivuu 51:b344a92b6a5f 222 controlOutput = PID_controller.step(error1);
sivuu 50:2c03357de7cc 223 }
sivuu 50:2c03357de7cc 224
daniQQue 49:818a0e90ed9c 225 //======================================================================
daniQQue 0:34c739fcc3e0 226 //program
daniQQue 49:818a0e90ed9c 227 //======================================================================
daniQQue 0:34c739fcc3e0 228 int main()
daniQQue 0:34c739fcc3e0 229 {
daniQQue 40:187ef29de53d 230
daniQQue 49:818a0e90ed9c 231 pc.baud(115200); //connect with pc with baudrate 115200
daniQQue 49:818a0e90ed9c 232 green=1; //led is off (1), at beginning
daniQQue 49:818a0e90ed9c 233 red=0; //led is on (0), at beginning
daniQQue 49:818a0e90ed9c 234
daniQQue 49:818a0e90ed9c 235 //attach tickers to functions
sivuu 50:2c03357de7cc 236 sample_timer.attach(&filter, Ts); //continously execute the EMG reader and filter, it ensures that filter and sampling is executed every 1/frequency seconds
daniQQue 45:08bddea67bd8 237 ticker_switch.attach(&switch_function,1.0);
sivuu 50:2c03357de7cc 238 ticker_referenceangle.attach(&reference, Ts);
sivuu 50:2c03357de7cc 239 ticker_controllerm1.attach(&m1_controller, Ts);
sivuu 50:2c03357de7cc 240
sivuu 50:2c03357de7cc 241 //PID controller
sivuu 50:2c03357de7cc 242 PID_controller.PIDF(Kp,Ki,Kd,N,Ts);
daniQQue 44:c79e5944ac91 243
sivuu 51:b344a92b6a5f 244 //Encoder
sivuu 51:b344a92b6a5f 245 QEI Encoder1(D13,D12, NC, rev_rond,QEI::X4_ENCODING);
sivuu 51:b344a92b6a5f 246
daniQQue 49:818a0e90ed9c 247 //Show the user what the starting motor will be and what will happen
daniQQue 47:ddaa59d48aca 248 pc.printf("We will start the demonstration\r\n");
daniQQue 49:818a0e90ed9c 249 pc.printf("\r\n\r\n\r\n");
daniQQue 49:818a0e90ed9c 250
daniQQue 47:ddaa59d48aca 251 if (switch_signal%2==0)
daniQQue 47:ddaa59d48aca 252 {pc.printf("If you contract the biceps, the robot will go right \r\n");
daniQQue 47:ddaa59d48aca 253 pc.printf("If you contract the triceps, the robot will go left \r\n");
daniQQue 47:ddaa59d48aca 254 pc.printf("\r\n");
daniQQue 47:ddaa59d48aca 255 }
daniQQue 47:ddaa59d48aca 256
daniQQue 47:ddaa59d48aca 257
daniQQue 47:ddaa59d48aca 258 else
daniQQue 45:08bddea67bd8 259 {pc.printf("If you contract the biceps, the robot will go up \r\n");
daniQQue 45:08bddea67bd8 260 pc.printf("If you contract the triceps, the robot will go down \r\n");
daniQQue 47:ddaa59d48aca 261 pc.printf("\r\n");
daniQQue 45:08bddea67bd8 262 }
daniQQue 45:08bddea67bd8 263
daniQQue 44:c79e5944ac91 264 //==============================================================================================
daniQQue 0:34c739fcc3e0 265 //endless loop
daniQQue 0:34c739fcc3e0 266
daniQQue 40:187ef29de53d 267
daniQQue 49:818a0e90ed9c 268 while (true) { // neverending loop
daniQQue 8:cd0cb71b69f2 269
sivuu 51:b344a92b6a5f 270 counts_encoder1 = Encoder1.getPulses();
sivuu 51:b344a92b6a5f 271 rev_counts_motor1=(float)counts_encoder1/(gearboxratio*rev_rond);
sivuu 51:b344a92b6a5f 272 rev_counts_motor1_rad=rev_counts_motor1*6.28318530718;
sivuu 52:0135deb3b07f 273
sivuu 52:0135deb3b07f 274 pc.printf("%f \r\n", d_ref);
sivuu 52:0135deb3b07f 275 pc.printf("%f ", rev_counts_motor1_rad);
sivuu 51:b344a92b6a5f 276
daniQQue 45:08bddea67bd8 277 if (onoffsignal_biceps==-1) //left biceps contracted
daniQQue 40:187ef29de53d 278 {
daniQQue 49:818a0e90ed9c 279 if (switch_signal%2==0) //switch even
daniQQue 40:187ef29de53d 280 {
sivuu 51:b344a92b6a5f 281 speedmotor1=controlOutput;
sivuu 51:b344a92b6a5f 282 //richting_motor1 = ccw; //motor 1, left
sivuu 51:b344a92b6a5f 283 //pwm_motor1 = speedmotor1; //speed of motor 1
sivuu 51:b344a92b6a5f 284 if (speedmotor1<0){
sivuu 52:0135deb3b07f 285 richting_motor1 = cw;
sivuu 51:b344a92b6a5f 286 }
sivuu 51:b344a92b6a5f 287 else {
sivuu 52:0135deb3b07f 288 richting_motor1 = ccw;
sivuu 51:b344a92b6a5f 289 }
sivuu 51:b344a92b6a5f 290 pwm_motor1 = fabs(speedmotor1); //speed of motor 1
sivuu 52:0135deb3b07f 291 // pc.printf("%f\r\n", pwm_motor1.read());
daniQQue 40:187ef29de53d 292 }
sivuu 51:b344a92b6a5f 293
daniQQue 40:187ef29de53d 294
daniQQue 49:818a0e90ed9c 295 else //switch odd
daniQQue 40:187ef29de53d 296 {
daniQQue 49:818a0e90ed9c 297 richting_motor2 = ccw; //motor 2, up
daniQQue 49:818a0e90ed9c 298 pwm_motor2 = speedmotor2;//speed of motor 2
daniQQue 40:187ef29de53d 299
daniQQue 40:187ef29de53d 300 }
daniQQue 40:187ef29de53d 301
daniQQue 8:cd0cb71b69f2 302 }
sivuu 52:0135deb3b07f 303 else if (onoffsignal_biceps==1) //right biceps contracted
daniQQue 40:187ef29de53d 304 {
daniQQue 49:818a0e90ed9c 305 if (switch_signal%2==0) //switch signal even
daniQQue 40:187ef29de53d 306 {
sivuu 51:b344a92b6a5f 307 speedmotor1=controlOutput;
sivuu 51:b344a92b6a5f 308 //richting_motor1 = ccw; //motor 1, left
sivuu 51:b344a92b6a5f 309 //pwm_motor1 = speedmotor1; //speed of motor 1
sivuu 51:b344a92b6a5f 310 if (speedmotor1<0){
sivuu 52:0135deb3b07f 311 richting_motor1 = cw;
sivuu 51:b344a92b6a5f 312 }
sivuu 51:b344a92b6a5f 313 else {
sivuu 52:0135deb3b07f 314 richting_motor1 = ccw;
sivuu 51:b344a92b6a5f 315 }
sivuu 51:b344a92b6a5f 316 pwm_motor1 = fabs(speedmotor1); //speed of motor 1
sivuu 52:0135deb3b07f 317 // pc.printf("%f\r\n", pwm_motor1.read());
daniQQue 40:187ef29de53d 318 }
daniQQue 49:818a0e90ed9c 319 else //switch signal odd
daniQQue 40:187ef29de53d 320 {
daniQQue 49:818a0e90ed9c 321 richting_motor2 = cw; //motor 2. down
daniQQue 49:818a0e90ed9c 322 pwm_motor2 = speedmotor2; //speed motor 2
daniQQue 40:187ef29de53d 323
daniQQue 40:187ef29de53d 324 }
daniQQue 40:187ef29de53d 325 }
daniQQue 49:818a0e90ed9c 326 else{
daniQQue 49:818a0e90ed9c 327 //no contraction of biceps
daniQQue 40:187ef29de53d 328 pwm_motor2=0;
daniQQue 40:187ef29de53d 329 pwm_motor1=0;
daniQQue 40:187ef29de53d 330 }
daniQQue 40:187ef29de53d 331
daniQQue 49:818a0e90ed9c 332 }//while true closed
daniQQue 0:34c739fcc3e0 333
daniQQue 49:818a0e90ed9c 334 } //int main closed
daniQQue 49:818a0e90ed9c 335
daniQQue 49:818a0e90ed9c 336 //=============================================================================================1