pid gecomment
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed
Fork of a_pid_kal_end_def by
main.cpp@59:1725a3f02f37, 2016-11-08 (annotated)
- Committer:
- FloorC
- Date:
- Tue Nov 08 20:31:45 2016 +0000
- Revision:
- 59:1725a3f02f37
- Parent:
- 58:c91723359f62
alle pid gecomment
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
daniQQue | 57:c546edf67c5c | 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 | 57:c546edf67c5c | 10 | //======================================================================================================================================================= |
daniQQue | 0:34c739fcc3e0 | 11 | //Define objects |
daniQQue | 49:818a0e90ed9c | 12 | |
daniQQue | 49:818a0e90ed9c | 13 | //EMG |
FloorC | 59:1725a3f02f37 | 14 | AnalogIn emg_biceps_right_in (A0); //analog in to get EMG biceps (r) in to c++ |
FloorC | 59:1725a3f02f37 | 15 | AnalogIn emg_triceps_right_in(A1); //analog in to get EMG triceps (r) in to c++ |
FloorC | 59:1725a3f02f37 | 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 | 57:c546edf67c5c | 23 | Ticker ticker_encoder; //ticker for encoderfunction motor 1 |
daniQQue | 57:c546edf67c5c | 24 | Ticker ticker_calibration_biceps; //ticker for calibration biceps |
daniQQue | 57:c546edf67c5c | 25 | Ticker ticker_calibration_triceps; //ticker for calibation triceps |
FloorC | 55:d742332ced11 | 26 | |
FloorC | 55:d742332ced11 | 27 | //Timer |
FloorC | 55:d742332ced11 | 28 | Timer timer; |
daniQQue | 49:818a0e90ed9c | 29 | |
daniQQue | 49:818a0e90ed9c | 30 | //Monitoring |
daniQQue | 46:4a8889f9dc9f | 31 | HIDScope scope(5); //open 5 channels in hidscope |
daniQQue | 46:4a8889f9dc9f | 32 | MODSERIAL pc(USBTX, USBRX); //pc connection |
daniQQue | 49:818a0e90ed9c | 33 | DigitalOut red(LED_RED); //LED on K64F board, 1 is out; 0 is on |
daniQQue | 49:818a0e90ed9c | 34 | DigitalOut green(LED_GREEN); //LED on K64f board, 1 is out; o is on |
daniQQue | 57:c546edf67c5c | 35 | DigitalOut blue(LED_BLUE); //LED on K64f board, 1 is out; o is on |
daniQQue | 57:c546edf67c5c | 36 | |
daniQQue | 57:c546edf67c5c | 37 | //buttons |
FloorC | 59:1725a3f02f37 | 38 | DigitalIn button_calibration_biceps (SW3); //button to start calibration biceps |
FloorC | 59:1725a3f02f37 | 39 | DigitalIn button_calibration_triceps (SW2); // button to start calibration triceps |
daniQQue | 40:187ef29de53d | 40 | |
daniQQue | 40:187ef29de53d | 41 | //motors |
FloorC | 59:1725a3f02f37 | 42 | DigitalOut richting_motor1(D7); //motor 1 connected to motor 1 at k64f board; for turningtable |
daniQQue | 47:ddaa59d48aca | 43 | PwmOut pwm_motor1(D6); |
FloorC | 59:1725a3f02f37 | 44 | DigitalOut richting_motor2(D4); //motor 2 connected to motor 2 at k64f board; for linear actuator |
daniQQue | 47:ddaa59d48aca | 45 | PwmOut pwm_motor2(D5); |
daniQQue | 0:34c739fcc3e0 | 46 | |
sivuu | 50:2c03357de7cc | 47 | //encoders |
sivuu | 50:2c03357de7cc | 48 | DigitalIn encoder1A(D13); |
sivuu | 50:2c03357de7cc | 49 | DigitalIn encoder1B(D12); |
sivuu | 50:2c03357de7cc | 50 | |
sivuu | 50:2c03357de7cc | 51 | //controller |
FloorC | 59:1725a3f02f37 | 52 | BiQuad PID_controller; |
sivuu | 50:2c03357de7cc | 53 | |
daniQQue | 57:c546edf67c5c | 54 | //======================================================================================================================================================= |
daniQQue | 0:34c739fcc3e0 | 55 | //define variables |
daniQQue | 47:ddaa59d48aca | 56 | |
sivuu | 50:2c03357de7cc | 57 | //thresholds |
FloorC | 59:1725a3f02f37 | 58 | double treshold_biceps_right = 0.04; //common values that work. |
FloorC | 59:1725a3f02f37 | 59 | double treshold_biceps_left = -0.04; //tested on multiple persons |
FloorC | 59:1725a3f02f37 | 60 | double treshold_triceps = -0.04; //triceps and left biceps is specified negative, thus negative treshold |
daniQQue | 49:818a0e90ed9c | 61 | |
daniQQue | 57:c546edf67c5c | 62 | |
daniQQue | 57:c546edf67c5c | 63 | //calibration variables |
daniQQue | 57:c546edf67c5c | 64 | const float percentage_max_triceps=0.25; //percentage from max to calculate new treshold |
daniQQue | 57:c546edf67c5c | 65 | const float percentage_max_biceps =0.3; //percentage from max to calculate new treshold |
daniQQue | 57:c546edf67c5c | 66 | double max_biceps; //calibration maximum biceps |
daniQQue | 57:c546edf67c5c | 67 | double max_triceps; //calibration maximum triceps |
daniQQue | 57:c546edf67c5c | 68 | |
daniQQue | 49:818a0e90ed9c | 69 | //on/off and switch signals |
FloorC | 59:1725a3f02f37 | 70 | int switch_signal = 0; //start of counter, switch made by even and odd numbers |
FloorC | 59:1725a3f02f37 | 71 | int onoffsignal_biceps; //on/off signal created by the bicepssignal. (-1: left biceps contract, 0: nothing contracted, 1: right biceps contracted) |
daniQQue | 45:08bddea67bd8 | 72 | int switch_signal_triceps; |
daniQQue | 46:4a8889f9dc9f | 73 | |
daniQQue | 49:818a0e90ed9c | 74 | //motorvariables |
FloorC | 59:1725a3f02f37 | 75 | float speedmotor1=0.18; //speed of motor 1 is 0.18 pwm at start |
daniQQue | 49:818a0e90ed9c | 76 | float speedmotor2=1.0; //speed of motor 2 is 1.0 pwm at start |
daniQQue | 46:4a8889f9dc9f | 77 | |
daniQQue | 49:818a0e90ed9c | 78 | int cw=0; //clockwise direction |
daniQQue | 49:818a0e90ed9c | 79 | int ccw=1; //counterclockwise direction |
sivuu | 50:2c03357de7cc | 80 | |
sivuu | 50:2c03357de7cc | 81 | //encoder |
FloorC | 59:1725a3f02f37 | 82 | int counts_encoder1; //variable to count the pulses given by the encoder, 1 indicates motor 1 |
FloorC | 59:1725a3f02f37 | 83 | float rev_counts_motor1; //calculated revolutions |
daniQQue | 57:c546edf67c5c | 84 | float rev_counts_motor1_rad; //calculated revolutions in rad! |
FloorC | 59:1725a3f02f37 | 85 | const float gearboxratio=131.25; //gearboxratio from encoder to motor |
FloorC | 59:1725a3f02f37 | 86 | const float rev_rond=64.0; //number of revolutions per rotation |
daniQQue | 57:c546edf67c5c | 87 | |
FloorC | 59:1725a3f02f37 | 88 | QEI Encoder1(D13,D12,NC,rev_rond,QEI::X4_ENCODING); //To set the encoder |
sivuu | 50:2c03357de7cc | 89 | |
sivuu | 50:2c03357de7cc | 90 | //reference |
FloorC | 59:1725a3f02f37 | 91 | volatile float d_ref = 0; //reference angle, starts off 0 |
FloorC | 59:1725a3f02f37 | 92 | const float w_ref = 1.5; //reference speed, constant |
FloorC | 59:1725a3f02f37 | 93 | volatile double t_start; //starttime of the timer |
FloorC | 59:1725a3f02f37 | 94 | volatile double w_var; //variable reference speed for making the reference signal |
FloorC | 59:1725a3f02f37 | 95 | const double Ts = 0.001; //time step for diverse tickers. It is comparable to a frequency of 1000Hz |
sivuu | 50:2c03357de7cc | 96 | |
sivuu | 50:2c03357de7cc | 97 | //controller |
FloorC | 59:1725a3f02f37 | 98 | const double Kp = 1.2614; //calculated value for the proportional action of the PID |
FloorC | 59:1725a3f02f37 | 99 | const double Ki = 0.4219; //calculated value for the integral action of the PID |
FloorC | 59:1725a3f02f37 | 100 | const double Kd = 0.8312; //calculated value for the derivative action of the PID |
FloorC | 59:1725a3f02f37 | 101 | const double N = 100; //specified value for the filter coefficient of the PID |
FloorC | 59:1725a3f02f37 | 102 | volatile double error1; //calculated error |
FloorC | 59:1725a3f02f37 | 103 | volatile double controlOutput; //output of the PID-controller |
FloorC | 59:1725a3f02f37 | 104 | bool start_motor = true; //bool to start the reference when the motor turns |
FloorC | 59:1725a3f02f37 | 105 | |
daniQQue | 57:c546edf67c5c | 106 | |
daniQQue | 57:c546edf67c5c | 107 | //======================================================================================================================================================= |
daniQQue | 44:c79e5944ac91 | 108 | //filter coefficients |
daniQQue | 40:187ef29de53d | 109 | |
daniQQue | 40:187ef29de53d | 110 | //b1 = biceps right arm |
FloorC | 59:1725a3f02f37 | 111 | 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 10 Hz |
FloorC | 59:1725a3f02f37 | 112 | BiQuad filternotch1_b1 (9.5654e-01, -1.9131e+00, 9.5654e-01 ,-1.9112e+00 ,9.1498e-01; // IIRnotch filter, with frequency of 50 Hz |
daniQQue | 40:187ef29de53d | 113 | |
daniQQue | 40:187ef29de53d | 114 | //t1= triceps right arm |
FloorC | 59:1725a3f02f37 | 115 | 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 10 Hz |
FloorC | 59:1725a3f02f37 | 116 | BiQuad filternotch1_t1 (9.5654e-01, -1.9131e+00, 9.5654e-01 ,-1.9112e+00 ,9.1498e-01; // IIRnotch filter, with frequency of 50 Hz |
daniQQue | 5:688b1b5530d8 | 117 | |
daniQQue | 40:187ef29de53d | 118 | //b2= biceps left arm |
FloorC | 59:1725a3f02f37 | 119 | 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 10 Hz |
FloorC | 59:1725a3f02f37 | 120 | BiQuad filternotch1_b2 (9.5654e-01, -1.9131e+00, 9.5654e-01 ,-1.9112e+00 ,9.1498e-01; // IIRnotch filter, with frequency of 50 Hz |
daniQQue | 40:187ef29de53d | 121 | |
daniQQue | 40:187ef29de53d | 122 | //after abs filtering |
FloorC | 59:1725a3f02f37 | 123 | 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 2 Hz |
FloorC | 59:1725a3f02f37 | 124 | 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 2 Hz |
FloorC | 59:1725a3f02f37 | 125 | 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 2 Hz |
daniQQue | 10:7255b59224cc | 126 | |
daniQQue | 57:c546edf67c5c | 127 | //======================================================================================================================================================= |
daniQQue | 45:08bddea67bd8 | 128 | //voids |
daniQQue | 57:c546edf67c5c | 129 | //======================================================================================================================================================= |
daniQQue | 45:08bddea67bd8 | 130 | |
daniQQue | 40:187ef29de53d | 131 | //function teller |
FloorC | 59:1725a3f02f37 | 132 | void switch_function() { // The switch function. Makes it possible to switch between the motors. It simply adds one at switch_signal. |
FloorC | 59:1725a3f02f37 | 133 | if(switch_signal_triceps==1){ |
daniQQue | 45:08bddea67bd8 | 134 | switch_signal++; |
daniQQue | 49:818a0e90ed9c | 135 | |
FloorC | 59:1725a3f02f37 | 136 | // 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 | 137 | |
daniQQue | 40:187ef29de53d | 138 | green=!green; |
daniQQue | 40:187ef29de53d | 139 | red=!red; |
daniQQue | 47:ddaa59d48aca | 140 | |
FloorC | 59:1725a3f02f37 | 141 | if (switch_signal%2==0){ |
FloorC | 59:1725a3f02f37 | 142 | pc.printf("If you contract the biceps, the robot will go right \r\n"); |
FloorC | 59:1725a3f02f37 | 143 | pc.printf("If you contract the triceps, the robot will go left \r\n"); |
FloorC | 59:1725a3f02f37 | 144 | pc.printf("\r\n"); |
FloorC | 59:1725a3f02f37 | 145 | } |
daniQQue | 47:ddaa59d48aca | 146 | |
daniQQue | 47:ddaa59d48aca | 147 | |
FloorC | 59:1725a3f02f37 | 148 | else{ |
FloorC | 59:1725a3f02f37 | 149 | pc.printf("If you contract the biceps, the robot will go up \r\n"); |
FloorC | 59:1725a3f02f37 | 150 | pc.printf("If you contract the triceps, the robot will go down \r\n"); |
FloorC | 59:1725a3f02f37 | 151 | pc.printf("\r\n"); |
FloorC | 59:1725a3f02f37 | 152 | } |
daniQQue | 47:ddaa59d48aca | 153 | |
daniQQue | 40:187ef29de53d | 154 | } |
FloorC | 59:1725a3f02f37 | 155 | } |
daniQQue | 49:818a0e90ed9c | 156 | |
daniQQue | 57:c546edf67c5c | 157 | //======================================================================================================================================================= |
daniQQue | 49:818a0e90ed9c | 158 | //functions which are called in ticker to sample the analog signal and make the on/off and switch signal. |
daniQQue | 40:187ef29de53d | 159 | |
daniQQue | 57:c546edf67c5c | 160 | //Filter void :// funciton which is called in ticker to sample the analog signal and make the on/off and switch signal. |
daniQQue | 0:34c739fcc3e0 | 161 | void filter(){ |
daniQQue | 40:187ef29de53d | 162 | //biceps right arm read+filtering |
daniQQue | 49:818a0e90ed9c | 163 | double emg_biceps_right=emg_biceps_right_in.read(); //read the emg value from the elektrodes |
daniQQue | 49:818a0e90ed9c | 164 | double emg_filtered_high_biceps_right= filterhigh_b1.step(emg_biceps_right); //high pass filter, to remove offset |
daniQQue | 49:818a0e90ed9c | 165 | double emg_filtered_high_notch_1_biceps_right=filternotch1_b1.step(emg_filtered_high_biceps_right); //notch filter, to remove noise |
daniQQue | 49:818a0e90ed9c | 166 | double emg_abs_biceps_right=fabs(emg_filtered_high_notch_1_biceps_right); //rectify the signal, fabs because float |
daniQQue | 49:818a0e90ed9c | 167 | double emg_filtered_biceps_right=filterlow_b1.step(emg_abs_biceps_right); //low pass filter to envelope the signal |
daniQQue | 40:187ef29de53d | 168 | |
daniQQue | 40:187ef29de53d | 169 | //triceps right arm read+filtering |
daniQQue | 49:818a0e90ed9c | 170 | double emg_triceps_right=emg_triceps_right_in.read(); //read the emg value from the elektrodes |
daniQQue | 49:818a0e90ed9c | 171 | double emg_filtered_high_triceps_right= filterhigh_t1.step(emg_triceps_right); //high pass filter, to remove offset |
daniQQue | 49:818a0e90ed9c | 172 | double emg_filtered_high_notch_1_triceps_right=filternotch1_t1.step(emg_filtered_high_triceps_right); //notch filter, to remove noise |
daniQQue | 49:818a0e90ed9c | 173 | double emg_abs_triceps_right=fabs(emg_filtered_high_notch_1_triceps_right); //rectify the signal, fabs because float |
daniQQue | 49:818a0e90ed9c | 174 | double emg_filtered_triceps_right=filterlow_t1.step(emg_abs_triceps_right); //low pass filter to envelope the signal |
daniQQue | 7:42d0e38196f1 | 175 | |
daniQQue | 40:187ef29de53d | 176 | //biceps left arm read+filtering |
daniQQue | 49:818a0e90ed9c | 177 | double emg_biceps_left=emg_biceps_left_in.read(); //read the emg value from the elektrodes |
daniQQue | 49:818a0e90ed9c | 178 | double emg_filtered_high_biceps_left= filterhigh_b2.step(emg_biceps_left); //high pass filter, to remove offset |
daniQQue | 49:818a0e90ed9c | 179 | double emg_filtered_high_notch_1_biceps_left=filternotch1_b2.step(emg_filtered_high_biceps_left); //notch filter, to remove noise |
daniQQue | 49:818a0e90ed9c | 180 | double emg_abs_biceps_left=fabs(emg_filtered_high_notch_1_biceps_left); //rectify the signal, fabs because float |
daniQQue | 49:818a0e90ed9c | 181 | double emg_filtered_biceps_left=filterlow_b2.step(emg_abs_biceps_left); //low pass filter to envelope the signal |
daniQQue | 40:187ef29de53d | 182 | |
daniQQue | 40:187ef29de53d | 183 | //creating of on/off signal with the created on/off signals, with if statement for right arm! |
daniQQue | 44:c79e5944ac91 | 184 | //signal substraction of filter biceps and triceps. right Biceps + left biceps - |
daniQQue | 44:c79e5944ac91 | 185 | double signal_biceps_sum=emg_filtered_biceps_right-emg_filtered_biceps_left; |
daniQQue | 44:c79e5944ac91 | 186 | double bicepstriceps_rightarm=emg_filtered_biceps_right-emg_filtered_triceps_right; |
daniQQue | 45:08bddea67bd8 | 187 | |
daniQQue | 44:c79e5944ac91 | 188 | //creating of on/off signal with the created on/off signals, with if statement for right arm! |
FloorC | 59:1725a3f02f37 | 189 | if (signal_biceps_sum>treshold_biceps_right){ |
FloorC | 59:1725a3f02f37 | 190 | onoffsignal_biceps=1; |
FloorC | 59:1725a3f02f37 | 191 | } |
daniQQue | 7:42d0e38196f1 | 192 | |
FloorC | 59:1725a3f02f37 | 193 | else if (signal_biceps_sum<treshold_biceps_left){ |
FloorC | 59:1725a3f02f37 | 194 | onoffsignal_biceps=-1; |
FloorC | 59:1725a3f02f37 | 195 | } |
daniQQue | 40:187ef29de53d | 196 | |
FloorC | 59:1725a3f02f37 | 197 | else{ |
FloorC | 59:1725a3f02f37 | 198 | onoffsignal_biceps=0; |
FloorC | 59:1725a3f02f37 | 199 | } |
daniQQue | 7:42d0e38196f1 | 200 | |
daniQQue | 40:187ef29de53d | 201 | //creating on/off signal for switch (left arm) |
daniQQue | 40:187ef29de53d | 202 | |
FloorC | 59:1725a3f02f37 | 203 | if (bicepstriceps_rightarm<treshold_triceps){ |
FloorC | 59:1725a3f02f37 | 204 | switch_signal_triceps=1; |
FloorC | 59:1725a3f02f37 | 205 | } |
daniQQue | 40:187ef29de53d | 206 | |
FloorC | 59:1725a3f02f37 | 207 | else{ |
FloorC | 59:1725a3f02f37 | 208 | switch_signal_triceps=0; |
FloorC | 59:1725a3f02f37 | 209 | } |
daniQQue | 49:818a0e90ed9c | 210 | |
daniQQue | 49:818a0e90ed9c | 211 | //send signals to scope to monitor the EMG signals |
FloorC | 59:1725a3f02f37 | 212 | scope.set(0, emg_filtered_biceps_right); //set emg signal of right biceps to scope in channel 0 |
FloorC | 59:1725a3f02f37 | 213 | scope.set(1, emg_filtered_triceps_right); // set emg signal of right triceps to scope in channel 1 |
FloorC | 59:1725a3f02f37 | 214 | scope.set(2, emg_filtered_biceps_left); // set emg signal of left biceps to scope in channel 2 |
FloorC | 59:1725a3f02f37 | 215 | scope.set(3, bicepstriceps_rightarm); // set on/off signal for the motors to scope in channel 3 |
FloorC | 59:1725a3f02f37 | 216 | scope.set(4, switch_signal_triceps); // set the switch signal to scope in channel 4 |
daniQQue | 40:187ef29de53d | 217 | |
FloorC | 59:1725a3f02f37 | 218 | scope.send(); //send all the signals to the scope |
FloorC | 59:1725a3f02f37 | 219 | } |
daniQQue | 57:c546edf67c5c | 220 | //======================================================================================================================================================= |
daniQQue | 57:c546edf67c5c | 221 | |
daniQQue | 57:c546edf67c5c | 222 | //reference void makes the reference that the controllor should follow. There is only a controller for motor 1. |
FloorC | 59:1725a3f02f37 | 223 | void reference(){ |
FloorC | 59:1725a3f02f37 | 224 | if (start_motor == true){ //bool that is true when the motor starts turning |
FloorC | 59:1725a3f02f37 | 225 | timer.start(); //timer that starts counting in milliseconds |
sivuu | 50:2c03357de7cc | 226 | } |
FloorC | 59:1725a3f02f37 | 227 | if (onoffsignal_biceps==-1 && switch_signal%2==0){ //the signal of the biceps is -1 and the switch is even, so motor 1 is being controlled |
FloorC | 59:1725a3f02f37 | 228 | t_start = timer.read_ms(); //read the current time passed from the timer |
FloorC | 59:1725a3f02f37 | 229 | start_motor = false; //it means that the motor is not running or has started up |
FloorC | 55:d742332ced11 | 230 | |
FloorC | 59:1725a3f02f37 | 231 | if (t_start < 1.0){ //the time passed is less than one second |
FloorC | 59:1725a3f02f37 | 232 | w_var = t_start*1.5; //the reference velocity is the time passed multiplied with the eventual constant velocity it should reach |
FloorC | 59:1725a3f02f37 | 233 | } |
FloorC | 55:d742332ced11 | 234 | |
FloorC | 59:1725a3f02f37 | 235 | else{ |
FloorC | 59:1725a3f02f37 | 236 | w_var = 1.5; //if the time passed is more than one second, the velocity is constant |
FloorC | 59:1725a3f02f37 | 237 | } |
FloorC | 59:1725a3f02f37 | 238 | |
FloorC | 59:1725a3f02f37 | 239 | d_ref = d_ref + w_var * Ts; //makes the reference angle |
FloorC | 59:1725a3f02f37 | 240 | |
FloorC | 59:1725a3f02f37 | 241 | } |
FloorC | 59:1725a3f02f37 | 242 | if (d_ref > 12){ //set the restrictions |
FloorC | 59:1725a3f02f37 | 243 | d_ref = 12; |
FloorC | 59:1725a3f02f37 | 244 | start_motor = true; //after the restriction is reached the motor (if turned the other way) will start up again so the bool has to be set to true |
FloorC | 55:d742332ced11 | 245 | } |
FloorC | 55:d742332ced11 | 246 | |
sivuu | 50:2c03357de7cc | 247 | else{ |
FloorC | 59:1725a3f02f37 | 248 | d_ref = d_ref; //if there is no signal, the referance angle is constant |
sivuu | 50:2c03357de7cc | 249 | } |
sivuu | 50:2c03357de7cc | 250 | |
FloorC | 59:1725a3f02f37 | 251 | if (onoffsignal_biceps==1 && switch_signal%2==0){ //the signal of the biceps is -1 and the switch is even, so motor 1 is being controlled |
FloorC | 55:d742332ced11 | 252 | t_start = timer.read_ms(); |
FloorC | 55:d742332ced11 | 253 | start_motor = false; |
FloorC | 55:d742332ced11 | 254 | |
FloorC | 55:d742332ced11 | 255 | if (t_start < 1.0){ |
FloorC | 55:d742332ced11 | 256 | w_var = t_start*1.5; |
FloorC | 59:1725a3f02f37 | 257 | } |
FloorC | 59:1725a3f02f37 | 258 | |
FloorC | 55:d742332ced11 | 259 | else { |
FloorC | 55:d742332ced11 | 260 | w_var = 1.5; |
FloorC | 59:1725a3f02f37 | 261 | } |
FloorC | 59:1725a3f02f37 | 262 | d_ref = d_ref - w_var * Ts; //the motor should turn the other way now so the reference becomes negative |
sivuu | 50:2c03357de7cc | 263 | } |
FloorC | 59:1725a3f02f37 | 264 | |
FloorC | 59:1725a3f02f37 | 265 | if (d_ref < -12){ //negative restriction |
FloorC | 59:1725a3f02f37 | 266 | d_ref = -12; |
FloorC | 59:1725a3f02f37 | 267 | start_motor = true; |
FloorC | 55:d742332ced11 | 268 | } |
FloorC | 59:1725a3f02f37 | 269 | |
sivuu | 50:2c03357de7cc | 270 | else{ |
sivuu | 50:2c03357de7cc | 271 | d_ref = d_ref; |
sivuu | 50:2c03357de7cc | 272 | } |
sivuu | 50:2c03357de7cc | 273 | |
sivuu | 50:2c03357de7cc | 274 | } |
daniQQue | 57:c546edf67c5c | 275 | //======================================================================================================================================================= |
daniQQue | 57:c546edf67c5c | 276 | //This void calculates the error and makes the control output. |
sivuu | 50:2c03357de7cc | 277 | void m1_controller(){ |
FloorC | 59:1725a3f02f37 | 278 | error1 = d_ref-rev_counts_motor1_rad; //calculate the error = reference-output |
FloorC | 59:1725a3f02f37 | 279 | controlOutput = PID_controller.step(error1); //give the error as input to the controller |
sivuu | 50:2c03357de7cc | 280 | } |
daniQQue | 57:c546edf67c5c | 281 | //======================================================================================================================================================= |
sivuu | 50:2c03357de7cc | 282 | |
daniQQue | 57:c546edf67c5c | 283 | //This void calculated the number of rotations that the motor has done in rad. It is put in a void because with the ticker, this ensures that it is updated continuously. |
FloorC | 55:d742332ced11 | 284 | void encoders(){ |
FloorC | 55:d742332ced11 | 285 | counts_encoder1 = Encoder1.getPulses(); |
FloorC | 55:d742332ced11 | 286 | rev_counts_motor1 = (float)counts_encoder1/(gearboxratio*rev_rond); |
FloorC | 59:1725a3f02f37 | 287 | rev_counts_motor1_rad = rev_counts_motor1*6.28318530718; //calculate the angle in radians |
FloorC | 55:d742332ced11 | 288 | } |
FloorC | 55:d742332ced11 | 289 | |
daniQQue | 57:c546edf67c5c | 290 | //======================================================================================================================================================= |
daniQQue | 57:c546edf67c5c | 291 | |
daniQQue | 57:c546edf67c5c | 292 | //The calibration of the Biceps threshold is started by a button. |
daniQQue | 57:c546edf67c5c | 293 | //It determines the maximum reachable EMG signal and takes a percentage of this to determine the new threshold. |
daniQQue | 57:c546edf67c5c | 294 | void calibration_biceps(){ |
daniQQue | 57:c546edf67c5c | 295 | if (button_calibration_biceps==0){ //only runs when button is pressed |
daniQQue | 57:c546edf67c5c | 296 | |
FloorC | 59:1725a3f02f37 | 297 | //detach tickers of other voids that control the switched and motors. To avoid unwanted moving and switching of the motors. |
FloorC | 59:1725a3f02f37 | 298 | ticker_switch.detach(); |
FloorC | 59:1725a3f02f37 | 299 | sample_timer.detach(); |
FloorC | 59:1725a3f02f37 | 300 | |
FloorC | 59:1725a3f02f37 | 301 | //let the user know what is happening, blue led on: calibration is going. |
FloorC | 59:1725a3f02f37 | 302 | pc.printf("start of calibration biceps, contract maximal \r\n"); |
FloorC | 59:1725a3f02f37 | 303 | pc.printf("\r\n"); |
FloorC | 59:1725a3f02f37 | 304 | red=1; |
FloorC | 59:1725a3f02f37 | 305 | green=1; |
FloorC | 59:1725a3f02f37 | 306 | blue=0; |
daniQQue | 57:c546edf67c5c | 307 | |
daniQQue | 57:c546edf67c5c | 308 | //start callibration of biceps |
FloorC | 59:1725a3f02f37 | 309 | for(int n =0; n<1500;n++){ //read for 1500 samples as calibration |
FloorC | 59:1725a3f02f37 | 310 | |
FloorC | 59:1725a3f02f37 | 311 | //biceps right arm read+filtering |
FloorC | 59:1725a3f02f37 | 312 | double emg_biceps_right=emg_biceps_right_in.read(); //read the emg value from the elektrodes |
FloorC | 59:1725a3f02f37 | 313 | double emg_filtered_high_biceps_right= filterhigh_b1.step(emg_biceps_right); //high pass filter, to remove offset |
FloorC | 59:1725a3f02f37 | 314 | double emg_filtered_high_notch_1_biceps_right=filternotch1_b1.step(emg_filtered_high_biceps_right); //notch filter, to remove noise |
FloorC | 59:1725a3f02f37 | 315 | double emg_abs_biceps_right=fabs(emg_filtered_high_notch_1_biceps_right); //rectify the signal, fabs because float |
FloorC | 59:1725a3f02f37 | 316 | double emg_filtered_biceps_right=filterlow_b1.step(emg_abs_biceps_right); //low pass filter to envelope the signal |
FloorC | 59:1725a3f02f37 | 317 | |
FloorC | 59:1725a3f02f37 | 318 | //triceps right arm read+filtering |
FloorC | 59:1725a3f02f37 | 319 | double emg_triceps_right=emg_triceps_right_in.read(); //read the emg value from the elektrodes |
FloorC | 59:1725a3f02f37 | 320 | double emg_filtered_high_triceps_right= filterhigh_t1.step(emg_triceps_right); //high pass filter, to remove offset |
FloorC | 59:1725a3f02f37 | 321 | double emg_filtered_high_notch_1_triceps_right=filternotch1_t1.step(emg_filtered_high_triceps_right); //notch filter, to remove noise |
FloorC | 59:1725a3f02f37 | 322 | double emg_abs_triceps_right=fabs(emg_filtered_high_notch_1_triceps_right); //rectify the signal, fabs because float |
FloorC | 59:1725a3f02f37 | 323 | double emg_filtered_triceps_right=filterlow_t1.step(emg_abs_triceps_right); //low pass filter to envelope the signal |
FloorC | 59:1725a3f02f37 | 324 | |
FloorC | 59:1725a3f02f37 | 325 | //biceps is +, triceps is - |
FloorC | 59:1725a3f02f37 | 326 | double bicepstriceps_rightarm=emg_filtered_biceps_right-emg_filtered_triceps_right; |
daniQQue | 57:c546edf67c5c | 327 | |
FloorC | 59:1725a3f02f37 | 328 | if (bicepstriceps_rightarm > max_biceps){ //determine what the highest reachable emg signal is |
FloorC | 59:1725a3f02f37 | 329 | |
FloorC | 59:1725a3f02f37 | 330 | max_biceps = bicepstriceps_rightarm; |
daniQQue | 57:c546edf67c5c | 331 | |
FloorC | 59:1725a3f02f37 | 332 | } |
daniQQue | 57:c546edf67c5c | 333 | wait(0.001f); //to sample at same freq; 1000Hz |
FloorC | 59:1725a3f02f37 | 334 | } |
FloorC | 59:1725a3f02f37 | 335 | |
daniQQue | 57:c546edf67c5c | 336 | treshold_biceps_right=percentage_max_biceps*max_biceps; //determine new treshold, right biceps is + |
daniQQue | 57:c546edf67c5c | 337 | treshold_biceps_left=-treshold_biceps_right; //determine new treshold, right biceps is - |
daniQQue | 57:c546edf67c5c | 338 | |
daniQQue | 57:c546edf67c5c | 339 | //toggle lights to see the calibration is done. Also show in putty that the calibration is done. |
daniQQue | 57:c546edf67c5c | 340 | blue=!blue; |
daniQQue | 57:c546edf67c5c | 341 | |
daniQQue | 57:c546edf67c5c | 342 | pc.printf(" end of calibration\r\n",treshold_biceps_right ); |
daniQQue | 57:c546edf67c5c | 343 | pc.printf(" change of cv biceps: %f ",treshold_biceps_right ); |
daniQQue | 57:c546edf67c5c | 344 | |
daniQQue | 57:c546edf67c5c | 345 | wait(0.2f); |
daniQQue | 57:c546edf67c5c | 346 | |
daniQQue | 57:c546edf67c5c | 347 | //remind the person of what motor will go on an which direction |
FloorC | 59:1725a3f02f37 | 348 | if (switch_signal%2==0){ |
FloorC | 59:1725a3f02f37 | 349 | green=0; |
FloorC | 59:1725a3f02f37 | 350 | red=1; |
FloorC | 59:1725a3f02f37 | 351 | } |
daniQQue | 57:c546edf67c5c | 352 | |
FloorC | 59:1725a3f02f37 | 353 | else{ |
FloorC | 59:1725a3f02f37 | 354 | green=1; |
FloorC | 59:1725a3f02f37 | 355 | red=0; |
daniQQue | 57:c546edf67c5c | 356 | } |
FloorC | 59:1725a3f02f37 | 357 | } |
FloorC | 59:1725a3f02f37 | 358 | //reattach the functions to the tickers that were detached. |
FloorC | 59:1725a3f02f37 | 359 | ticker_switch.attach(&switch_function,1.0); |
FloorC | 59:1725a3f02f37 | 360 | sample_timer.attach(&filter, 0.001); |
FloorC | 59:1725a3f02f37 | 361 | } |
daniQQue | 57:c546edf67c5c | 362 | //======================================================================================================================================================= |
daniQQue | 57:c546edf67c5c | 363 | |
FloorC | 59:1725a3f02f37 | 364 | //The calibration of the triceps threshold is started by a button. |
daniQQue | 57:c546edf67c5c | 365 | //It determines the maximum reachable EMG signal and takes a percentage of this to determine the new threshold. |
daniQQue | 57:c546edf67c5c | 366 | void calibration_triceps(){ |
daniQQue | 57:c546edf67c5c | 367 | if(button_calibration_triceps==0){ //only runs when button is pressed |
daniQQue | 57:c546edf67c5c | 368 | |
FloorC | 59:1725a3f02f37 | 369 | //detach tickers of other voids that control the switched and motors. To avoid unwanted moving and switching of the motors. |
FloorC | 59:1725a3f02f37 | 370 | ticker_switch.detach(); |
FloorC | 59:1725a3f02f37 | 371 | sample_timer.detach(); |
FloorC | 59:1725a3f02f37 | 372 | |
FloorC | 59:1725a3f02f37 | 373 | //toggel LEDS and let the user know that callibration of triceps is starting. |
FloorC | 59:1725a3f02f37 | 374 | red=1; |
FloorC | 59:1725a3f02f37 | 375 | green=1; |
FloorC | 59:1725a3f02f37 | 376 | blue=0; |
FloorC | 59:1725a3f02f37 | 377 | |
FloorC | 59:1725a3f02f37 | 378 | pc.printf("start of calibration triceps\r\n"); |
FloorC | 59:1725a3f02f37 | 379 | pc.printf("\r\n"); |
daniQQue | 57:c546edf67c5c | 380 | |
FloorC | 59:1725a3f02f37 | 381 | //start calibration of triceps |
FloorC | 59:1725a3f02f37 | 382 | for(int n =0; n<1500;n++){ //read for 2000 samples as calibration |
FloorC | 59:1725a3f02f37 | 383 | |
FloorC | 59:1725a3f02f37 | 384 | //biceps right arm read+filtering |
FloorC | 59:1725a3f02f37 | 385 | double emg_biceps_right=emg_biceps_right_in.read(); //read the emg value from the elektrodes |
FloorC | 59:1725a3f02f37 | 386 | double emg_filtered_high_biceps_right= filterhigh_b1.step(emg_biceps_right); //high pass filter, to remove offset |
FloorC | 59:1725a3f02f37 | 387 | double emg_filtered_high_notch_1_biceps_right=filternotch1_b1.step(emg_filtered_high_biceps_right); //notch filter, to remove noise |
FloorC | 59:1725a3f02f37 | 388 | double emg_abs_biceps_right=fabs(emg_filtered_high_notch_1_biceps_right); //rectify the signal, fabs because float |
FloorC | 59:1725a3f02f37 | 389 | double emg_filtered_biceps_right=filterlow_b1.step(emg_abs_biceps_right); //low pass filter to envelope the signal |
FloorC | 59:1725a3f02f37 | 390 | |
FloorC | 59:1725a3f02f37 | 391 | //triceps right arm read+filtering |
FloorC | 59:1725a3f02f37 | 392 | double emg_triceps_right=emg_triceps_right_in.read(); //read the emg value from the elektrodes |
FloorC | 59:1725a3f02f37 | 393 | double emg_filtered_high_triceps_right= filterhigh_t1.step(emg_triceps_right); //high pass filter, to remove offset |
FloorC | 59:1725a3f02f37 | 394 | double emg_filtered_high_notch_1_triceps_right=filternotch1_t1.step(emg_filtered_high_triceps_right); //notch filter, to remove noise |
FloorC | 59:1725a3f02f37 | 395 | double emg_abs_triceps_right=fabs(emg_filtered_high_notch_1_triceps_right); //rectify the signal, fabs because float |
FloorC | 59:1725a3f02f37 | 396 | double emg_filtered_triceps_right=filterlow_t1.step(emg_abs_triceps_right); //low pass filter to envelope the signal |
FloorC | 59:1725a3f02f37 | 397 | |
FloorC | 59:1725a3f02f37 | 398 | //biceps is +, triceps is - |
FloorC | 59:1725a3f02f37 | 399 | double bicepstriceps_rightarm=emg_filtered_biceps_right-emg_filtered_triceps_right; |
FloorC | 59:1725a3f02f37 | 400 | |
FloorC | 59:1725a3f02f37 | 401 | if (bicepstriceps_rightarm < max_triceps){ //determine what the lowest reachable emg of triceps (max in negative part) signal is |
FloorC | 59:1725a3f02f37 | 402 | |
FloorC | 59:1725a3f02f37 | 403 | max_triceps = bicepstriceps_rightarm; |
FloorC | 59:1725a3f02f37 | 404 | |
FloorC | 59:1725a3f02f37 | 405 | } |
FloorC | 59:1725a3f02f37 | 406 | wait(0.001f); //to sample at same freq; 1000Hz |
daniQQue | 57:c546edf67c5c | 407 | } |
daniQQue | 57:c546edf67c5c | 408 | treshold_triceps=percentage_max_triceps*max_triceps; //calculate the new treshold. This is a negative number due to the sum! |
FloorC | 59:1725a3f02f37 | 409 | |
daniQQue | 57:c546edf67c5c | 410 | //Let the user know that the calibration is done. |
daniQQue | 57:c546edf67c5c | 411 | pc.printf(" end of calibration\r\n"); |
daniQQue | 57:c546edf67c5c | 412 | pc.printf(" change of cv triceps: %f ",treshold_triceps ); |
daniQQue | 57:c546edf67c5c | 413 | blue=!blue; |
daniQQue | 57:c546edf67c5c | 414 | wait(0.2f); |
FloorC | 59:1725a3f02f37 | 415 | if (switch_signal%2==0){ |
FloorC | 59:1725a3f02f37 | 416 | green=0; |
FloorC | 59:1725a3f02f37 | 417 | red=1; |
daniQQue | 57:c546edf67c5c | 418 | } |
FloorC | 59:1725a3f02f37 | 419 | |
FloorC | 59:1725a3f02f37 | 420 | else{ |
FloorC | 59:1725a3f02f37 | 421 | green=1; |
FloorC | 59:1725a3f02f37 | 422 | red=0; |
FloorC | 59:1725a3f02f37 | 423 | } |
FloorC | 59:1725a3f02f37 | 424 | } |
daniQQue | 57:c546edf67c5c | 425 | |
FloorC | 59:1725a3f02f37 | 426 | //reattach the functions to the tickers that were detached. |
FloorC | 59:1725a3f02f37 | 427 | sample_timer.attach(&filter, 0.001); |
FloorC | 59:1725a3f02f37 | 428 | ticker_switch.attach(&switch_function,1.0); |
FloorC | 59:1725a3f02f37 | 429 | } |
daniQQue | 57:c546edf67c5c | 430 | //======================================================================================================================================================= |
daniQQue | 57:c546edf67c5c | 431 | |
daniQQue | 57:c546edf67c5c | 432 | //======================================================================================================================================================= |
daniQQue | 0:34c739fcc3e0 | 433 | //program |
daniQQue | 57:c546edf67c5c | 434 | //======================================================================================================================================================= |
FloorC | 59:1725a3f02f37 | 435 | int main(){ |
daniQQue | 49:818a0e90ed9c | 436 | |
FloorC | 59:1725a3f02f37 | 437 | pc.baud(115200); //connect with pc with baudrate 115200 |
FloorC | 59:1725a3f02f37 | 438 | green=1; //led is off (1), at beginning |
FloorC | 59:1725a3f02f37 | 439 | blue=1; //led is off (1), at beginning |
FloorC | 59:1725a3f02f37 | 440 | red=0; //led is on (0), at beginning |
FloorC | 59:1725a3f02f37 | 441 | |
FloorC | 59:1725a3f02f37 | 442 | //attach tickers to functions |
FloorC | 59:1725a3f02f37 | 443 | sample_timer.attach(&filter, Ts); //continously execute the EMG reader and filter, it ensures that filter and sampling is executed every 1/frequency seconds |
FloorC | 59:1725a3f02f37 | 444 | ticker_switch.attach(&switch_function,1.0); //it is possible to switch only once in a second, this ensures that the switch is not reacting on one signal multiple times. |
FloorC | 59:1725a3f02f37 | 445 | ticker_referenceangle.attach(&reference, Ts); |
FloorC | 59:1725a3f02f37 | 446 | ticker_controllerm1.attach(&m1_controller, Ts); |
FloorC | 59:1725a3f02f37 | 447 | ticker_encoder.attach(&encoders, Ts); |
FloorC | 59:1725a3f02f37 | 448 | ticker_calibration_biceps.attach (&calibration_biceps,2.0); //to call calibration biceps, stop EMG sampling and switch |
FloorC | 59:1725a3f02f37 | 449 | ticker_calibration_triceps.attach(&calibration_triceps,2.0); //to call calibration triceps, stop EMG sampling and switch |
FloorC | 59:1725a3f02f37 | 450 | |
FloorC | 59:1725a3f02f37 | 451 | //PID controller |
FloorC | 59:1725a3f02f37 | 452 | PID_controller.PIDF(Kp,Ki,Kd,N,Ts); |
FloorC | 59:1725a3f02f37 | 453 | |
FloorC | 59:1725a3f02f37 | 454 | //Encoder |
FloorC | 59:1725a3f02f37 | 455 | //QEI Encoder1(D13,D12, NC, rev_rond,QEI::X4_ENCODING); |
FloorC | 59:1725a3f02f37 | 456 | |
FloorC | 59:1725a3f02f37 | 457 | //Show the user what the starting motor will be and what will happen |
FloorC | 59:1725a3f02f37 | 458 | pc.printf("We will start the demonstration\r\n"); |
FloorC | 59:1725a3f02f37 | 459 | pc.printf("\r\n\r\n\r\n"); |
daniQQue | 44:c79e5944ac91 | 460 | |
FloorC | 59:1725a3f02f37 | 461 | if (switch_signal%2==0){ |
FloorC | 59:1725a3f02f37 | 462 | pc.printf("If you contract the biceps, the robot will go right \r\n"); |
FloorC | 59:1725a3f02f37 | 463 | pc.printf("If you contract the triceps, the robot will go left \r\n"); |
FloorC | 59:1725a3f02f37 | 464 | pc.printf("\r\n"); |
FloorC | 59:1725a3f02f37 | 465 | } |
daniQQue | 47:ddaa59d48aca | 466 | |
daniQQue | 47:ddaa59d48aca | 467 | |
FloorC | 59:1725a3f02f37 | 468 | else{ |
FloorC | 59:1725a3f02f37 | 469 | pc.printf("If you contract the biceps, the robot will go up \r\n"); |
FloorC | 59:1725a3f02f37 | 470 | pc.printf("If you contract the triceps, the robot will go down \r\n"); |
FloorC | 59:1725a3f02f37 | 471 | pc.printf("\r\n"); |
FloorC | 59:1725a3f02f37 | 472 | } |
daniQQue | 45:08bddea67bd8 | 473 | |
daniQQue | 57:c546edf67c5c | 474 | //======================================================================================================================================================= |
daniQQue | 0:34c739fcc3e0 | 475 | //endless loop |
daniQQue | 0:34c739fcc3e0 | 476 | |
daniQQue | 40:187ef29de53d | 477 | |
FloorC | 59:1725a3f02f37 | 478 | while (true) { //neverending loop |
daniQQue | 8:cd0cb71b69f2 | 479 | |
FloorC | 59:1725a3f02f37 | 480 | if (onoffsignal_biceps==-1){ //left biceps contracted |
FloorC | 56:a38412383477 | 481 | |
FloorC | 59:1725a3f02f37 | 482 | if (switch_signal%2==0){ //switch even |
FloorC | 56:a38412383477 | 483 | |
FloorC | 59:1725a3f02f37 | 484 | speedmotor1=controlOutput; //output PID-controller is the speed for motor1 |
daniQQue | 57:c546edf67c5c | 485 | |
FloorC | 59:1725a3f02f37 | 486 | if (speedmotor1<0){ //if the output of the controller is negative, the direction is clockwise |
FloorC | 59:1725a3f02f37 | 487 | richting_motor1 = cw; //motor 1, right |
sivuu | 51:b344a92b6a5f | 488 | } |
FloorC | 59:1725a3f02f37 | 489 | else { //if the output is positive, the direction is counterclockwise |
FloorC | 59:1725a3f02f37 | 490 | richting_motor1 = ccw; //motor 1, left |
sivuu | 51:b344a92b6a5f | 491 | } |
FloorC | 59:1725a3f02f37 | 492 | pwm_motor1 = fabs(speedmotor1); //speed of motor 1, absolute because pwm cannot be negative |
daniQQue | 57:c546edf67c5c | 493 | |
FloorC | 59:1725a3f02f37 | 494 | } |
sivuu | 51:b344a92b6a5f | 495 | |
daniQQue | 40:187ef29de53d | 496 | |
FloorC | 59:1725a3f02f37 | 497 | else{ //switch odd |
FloorC | 59:1725a3f02f37 | 498 | |
FloorC | 59:1725a3f02f37 | 499 | richting_motor2 = ccw; //motor 2, up |
FloorC | 59:1725a3f02f37 | 500 | pwm_motor2 = speedmotor2; //speed of motor 2 |
daniQQue | 40:187ef29de53d | 501 | |
FloorC | 59:1725a3f02f37 | 502 | } |
daniQQue | 40:187ef29de53d | 503 | |
FloorC | 59:1725a3f02f37 | 504 | } |
FloorC | 59:1725a3f02f37 | 505 | else if (onoffsignal_biceps==1){ //right biceps contracted |
FloorC | 59:1725a3f02f37 | 506 | |
FloorC | 59:1725a3f02f37 | 507 | if (switch_signal%2==0){ //switch signal even |
FloorC | 59:1725a3f02f37 | 508 | speedmotor1=controlOutput; |
daniQQue | 57:c546edf67c5c | 509 | |
FloorC | 59:1725a3f02f37 | 510 | if (speedmotor1<0){ //the same as for the left biceps, the robot turns in the right direction because of the reference signal |
FloorC | 59:1725a3f02f37 | 511 | richting_motor1 = cw; //motor 1, right |
sivuu | 51:b344a92b6a5f | 512 | } |
FloorC | 59:1725a3f02f37 | 513 | else { |
FloorC | 59:1725a3f02f37 | 514 | richting_motor1 = ccw; //motor 1, left |
sivuu | 51:b344a92b6a5f | 515 | } |
sivuu | 51:b344a92b6a5f | 516 | pwm_motor1 = fabs(speedmotor1); //speed of motor 1 |
daniQQue | 57:c546edf67c5c | 517 | |
FloorC | 59:1725a3f02f37 | 518 | } |
FloorC | 59:1725a3f02f37 | 519 | else{ //switch signal odd |
FloorC | 59:1725a3f02f37 | 520 | richting_motor2 = cw; //motor 2, down |
FloorC | 59:1725a3f02f37 | 521 | pwm_motor2 = speedmotor2; //speed motor 2 |
daniQQue | 40:187ef29de53d | 522 | |
FloorC | 59:1725a3f02f37 | 523 | } |
FloorC | 59:1725a3f02f37 | 524 | } |
FloorC | 59:1725a3f02f37 | 525 | else{ |
FloorC | 59:1725a3f02f37 | 526 | //no contraction of biceps, thus no motoraction. |
FloorC | 59:1725a3f02f37 | 527 | pwm_motor2=0; |
FloorC | 59:1725a3f02f37 | 528 | pwm_motor1=0; |
FloorC | 59:1725a3f02f37 | 529 | start_motor = true; //every time the motor is off, the bool is reset so that the reference void can start when the motor starts |
FloorC | 56:a38412383477 | 530 | |
FloorC | 59:1725a3f02f37 | 531 | } |
daniQQue | 40:187ef29de53d | 532 | |
FloorC | 59:1725a3f02f37 | 533 | } //while true closed |
daniQQue | 0:34c739fcc3e0 | 534 | |
daniQQue | 49:818a0e90ed9c | 535 | } //int main closed |
daniQQue | 49:818a0e90ed9c | 536 | |
daniQQue | 57:c546edf67c5c | 537 | //======================================================================================================================================================= |