Wil je hier nog je PID controler kort uitleggen? Is sneller denk ik. Rest is gedaan volgens mij. Hier zit kall in.
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed
Fork of another_try_from_scratch_on_emg by
main.cpp@51:b344a92b6a5f, 2016-11-03 (annotated)
- Committer:
- sivuu
- Date:
- Thu Nov 03 10:37:29 2016 +0000
- Revision:
- 51:b344a92b6a5f
- Parent:
- 50:2c03357de7cc
- Child:
- 52:0135deb3b07f
PID controller werkt, alleen restricties werken niet meer als de andere motor is gebruikt. referentie loopt door als motor 2 wordt gebruikt
Who changed what in which revision?
User | Revision | Line number | New 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 | 50:2c03357de7cc | 51 | double treshold_biceps_left = -0.04; // tested on multiple persons |
sivuu | 50:2c03357de7cc | 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 | 51:b344a92b6a5f | 196 | if (onoffsignal_biceps==1){ //right biceps contracted{ |
sivuu | 50:2c03357de7cc | 197 | d_ref = d_ref + w_ref * Ts; |
sivuu | 50:2c03357de7cc | 198 | } |
sivuu | 50:2c03357de7cc | 199 | if (d_ref > 21.36 ){ |
sivuu | 50:2c03357de7cc | 200 | d_ref = 21.36; |
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 | 51:b344a92b6a5f | 207 | if (onoffsignal_biceps==-1){ //left biceps contracted{ |
sivuu | 50:2c03357de7cc | 208 | d_ref = d_ref - w_ref * Ts; |
sivuu | 50:2c03357de7cc | 209 | } |
sivuu | 50:2c03357de7cc | 210 | if (d_ref < -21.36){ |
sivuu | 50:2c03357de7cc | 211 | d_ref = -21.36; |
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 | 51:b344a92b6a5f | 273 | pc.printf("%f", pwm_motor1.read()); |
sivuu | 51:b344a92b6a5f | 274 | pc.printf(" %f", d_ref); |
sivuu | 51:b344a92b6a5f | 275 | pc.printf(" %f \r\n", 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 | 51:b344a92b6a5f | 285 | richting_motor1 = 0; |
sivuu | 51:b344a92b6a5f | 286 | } |
sivuu | 51:b344a92b6a5f | 287 | else { |
sivuu | 51:b344a92b6a5f | 288 | richting_motor1 = 1; |
sivuu | 51:b344a92b6a5f | 289 | } |
sivuu | 51:b344a92b6a5f | 290 | pwm_motor1 = fabs(speedmotor1); //speed of motor 1 |
daniQQue | 40:187ef29de53d | 291 | } |
sivuu | 51:b344a92b6a5f | 292 | |
daniQQue | 40:187ef29de53d | 293 | |
daniQQue | 49:818a0e90ed9c | 294 | else //switch odd |
daniQQue | 40:187ef29de53d | 295 | { |
daniQQue | 49:818a0e90ed9c | 296 | richting_motor2 = ccw; //motor 2, up |
daniQQue | 49:818a0e90ed9c | 297 | pwm_motor2 = speedmotor2;//speed of motor 2 |
daniQQue | 40:187ef29de53d | 298 | |
daniQQue | 40:187ef29de53d | 299 | } |
daniQQue | 40:187ef29de53d | 300 | |
daniQQue | 8:cd0cb71b69f2 | 301 | } |
daniQQue | 49:818a0e90ed9c | 302 | else if (onoffsignal_biceps==1) //right biceps contracted |
daniQQue | 40:187ef29de53d | 303 | { |
daniQQue | 49:818a0e90ed9c | 304 | if (switch_signal%2==0) //switch signal even |
daniQQue | 40:187ef29de53d | 305 | { |
sivuu | 51:b344a92b6a5f | 306 | speedmotor1=controlOutput; |
sivuu | 51:b344a92b6a5f | 307 | //richting_motor1 = ccw; //motor 1, left |
sivuu | 51:b344a92b6a5f | 308 | //pwm_motor1 = speedmotor1; //speed of motor 1 |
sivuu | 51:b344a92b6a5f | 309 | if (speedmotor1<0){ |
sivuu | 51:b344a92b6a5f | 310 | richting_motor1 = 0; |
sivuu | 51:b344a92b6a5f | 311 | } |
sivuu | 51:b344a92b6a5f | 312 | else { |
sivuu | 51:b344a92b6a5f | 313 | richting_motor1 = 1; |
sivuu | 51:b344a92b6a5f | 314 | } |
sivuu | 51:b344a92b6a5f | 315 | pwm_motor1 = fabs(speedmotor1); //speed of motor 1 |
daniQQue | 40:187ef29de53d | 316 | |
daniQQue | 40:187ef29de53d | 317 | } |
daniQQue | 49:818a0e90ed9c | 318 | else //switch signal odd |
daniQQue | 40:187ef29de53d | 319 | { |
daniQQue | 49:818a0e90ed9c | 320 | richting_motor2 = cw; //motor 2. down |
daniQQue | 49:818a0e90ed9c | 321 | pwm_motor2 = speedmotor2; //speed motor 2 |
daniQQue | 40:187ef29de53d | 322 | |
daniQQue | 40:187ef29de53d | 323 | } |
daniQQue | 40:187ef29de53d | 324 | } |
daniQQue | 49:818a0e90ed9c | 325 | else{ |
daniQQue | 49:818a0e90ed9c | 326 | //no contraction of biceps |
daniQQue | 40:187ef29de53d | 327 | pwm_motor2=0; |
daniQQue | 40:187ef29de53d | 328 | pwm_motor1=0; |
daniQQue | 40:187ef29de53d | 329 | } |
daniQQue | 40:187ef29de53d | 330 | |
daniQQue | 49:818a0e90ed9c | 331 | }//while true closed |
daniQQue | 0:34c739fcc3e0 | 332 | |
daniQQue | 49:818a0e90ed9c | 333 | } //int main closed |
daniQQue | 49:818a0e90ed9c | 334 | |
daniQQue | 49:818a0e90ed9c | 335 | //=============================================================================================1 |