lololololol
Dependencies: mbed QEI HIDScope biquadFilter MODSERIAL FastPWM
main.cpp@30:a16519224d58, 2019-10-30 (annotated)
- Committer:
- SjorsdeBruin
- Date:
- Wed Oct 30 14:43:34 2019 +0000
- Revision:
- 30:a16519224d58
- Parent:
- 29:5a846abba59e
- Child:
- 31:465a6e1e1db6
nog steeds niet werkend
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Hendrikvg | 17:cacf9e75eda7 | 1 | #include "QEI.h" |
Hendrikvg | 15:80b3ac2b8448 | 2 | #include "mbed.h" |
Hendrikvg | 20:ac1b4ffa3323 | 3 | #include "BiQuad.h" |
Hendrikvg | 14:20f11bb58244 | 4 | #include "FastPWM.h" |
Hendrikvg | 17:cacf9e75eda7 | 5 | #include "HIDScope.h" |
Hendrikvg | 16:40183eeadb6d | 6 | #include "MODSERIAL.h" |
Hendrikvg | 9:12b9865e7373 | 7 | |
Hendrikvg | 21:394a7a1deb73 | 8 | // Pins |
Hendrikvg | 21:394a7a1deb73 | 9 | MODSERIAL pc(USBTX, USBRX); |
SjorsdeBruin | 26:088e397ec26f | 10 | |
SjorsdeBruin | 26:088e397ec26f | 11 | QEI encoder_1(D10,D11,NC,8400,QEI::X4_ENCODING); |
SjorsdeBruin | 26:088e397ec26f | 12 | QEI encoder_2(D12,D13,NC,8400,QEI::X4_ENCODING); |
Hendrikvg | 24:a9ec9b836fd9 | 13 | |
Hendrikvg | 21:394a7a1deb73 | 14 | FastPWM PWM_motor_1(D6); |
Hendrikvg | 23:78898ddfb103 | 15 | FastPWM PWM_motor_2(D5); |
Hendrikvg | 24:a9ec9b836fd9 | 16 | |
Hendrikvg | 20:ac1b4ffa3323 | 17 | DigitalOut direction_motor_1(D7); |
Hendrikvg | 23:78898ddfb103 | 18 | DigitalOut direction_motor_2(D4); |
SjorsdeBruin | 26:088e397ec26f | 19 | |
Hendrikvg | 24:a9ec9b836fd9 | 20 | DigitalOut led_red(LED1); |
Hendrikvg | 24:a9ec9b836fd9 | 21 | DigitalOut led_green(LED2); |
Hendrikvg | 24:a9ec9b836fd9 | 22 | DigitalOut led_blue(LED3); |
Hendrikvg | 24:a9ec9b836fd9 | 23 | |
SjorsdeBruin | 26:088e397ec26f | 24 | AnalogIn emg_bl(A0); |
SjorsdeBruin | 26:088e397ec26f | 25 | AnalogIn emg_br(A1); |
SjorsdeBruin | 26:088e397ec26f | 26 | AnalogIn emg_leg(A2); |
Hendrikvg | 24:a9ec9b836fd9 | 27 | |
Hendrikvg | 21:394a7a1deb73 | 28 | InterruptIn button_1(SW2); |
Hendrikvg | 21:394a7a1deb73 | 29 | InterruptIn button_2(SW3); |
Hendrikvg | 17:cacf9e75eda7 | 30 | |
Hendrikvg | 24:a9ec9b836fd9 | 31 | |
Hendrikvg | 15:80b3ac2b8448 | 32 | // variables |
SjorsdeBruin | 28:8c90a46b613e | 33 | const float pi = 3.1416; |
SjorsdeBruin | 28:8c90a46b613e | 34 | const float l = 0.535; |
Hendrikvg | 21:394a7a1deb73 | 35 | Ticker TickerStateMachine; |
Hendrikvg | 23:78898ddfb103 | 36 | Ticker motor_control; |
Hendrikvg | 23:78898ddfb103 | 37 | Ticker write_scope; |
Hendrikvg | 21:394a7a1deb73 | 38 | Timer sinus_time; |
Hendrikvg | 24:a9ec9b836fd9 | 39 | Timeout rest_timeout; |
Hendrikvg | 24:a9ec9b836fd9 | 40 | Timeout mvc_timeout; |
SjorsdeBruin | 26:088e397ec26f | 41 | Timeout led_timeout; |
Hendrikvg | 21:394a7a1deb73 | 42 | enum states {start, motor_calibration, demo_mode, emg_calibration, vertical_movement, horizontal_movement}; |
Hendrikvg | 21:394a7a1deb73 | 43 | states CurrentState = start; |
Hendrikvg | 21:394a7a1deb73 | 44 | bool StateChanged = true; |
Hendrikvg | 24:a9ec9b836fd9 | 45 | enum substates {rest_biceps_left, mvc_biceps_left, rest_biceps_right, mvc_biceps_right, rest_biceps_leg, mvc_biceps_leg}; |
Hendrikvg | 24:a9ec9b836fd9 | 46 | substates CurrentSubstate = rest_biceps_left; |
Hendrikvg | 24:a9ec9b836fd9 | 47 | bool SubstateChanged = true; |
SjorsdeBruin | 26:088e397ec26f | 48 | volatile bool pressed_1 = false; |
SjorsdeBruin | 26:088e397ec26f | 49 | volatile bool pressed_2 = false; |
SjorsdeBruin | 27:d37b3a0e0f2b | 50 | HIDScope scope(6); |
SjorsdeBruin | 26:088e397ec26f | 51 | |
SjorsdeBruin | 29:5a846abba59e | 52 | volatile float theta_ref1; |
SjorsdeBruin | 29:5a846abba59e | 53 | volatile float theta_ref2; |
SjorsdeBruin | 26:088e397ec26f | 54 | float Ts = 0.01; |
Hendrikvg | 23:78898ddfb103 | 55 | float Kp; |
Hendrikvg | 23:78898ddfb103 | 56 | float Ki; |
Hendrikvg | 23:78898ddfb103 | 57 | float Kd; |
SjorsdeBruin | 29:5a846abba59e | 58 | float theta_1 = (40.0f*pi)/180.0f; |
SjorsdeBruin | 29:5a846abba59e | 59 | float theta_2 = (175.0f*pi)/180.0f; |
SjorsdeBruin | 29:5a846abba59e | 60 | float theta_error1; |
SjorsdeBruin | 29:5a846abba59e | 61 | float theta_error2; |
SjorsdeBruin | 29:5a846abba59e | 62 | float torque_1; |
SjorsdeBruin | 29:5a846abba59e | 63 | float torque_2; |
SjorsdeBruin | 28:8c90a46b613e | 64 | float x; |
SjorsdeBruin | 28:8c90a46b613e | 65 | float y; |
SjorsdeBruin | 28:8c90a46b613e | 66 | volatile float EMGx_velocity=0.02; |
SjorsdeBruin | 28:8c90a46b613e | 67 | volatile float EMGy_velocity=0; |
SjorsdeBruin | 29:5a846abba59e | 68 | char beweging; |
SjorsdeBruin | 26:088e397ec26f | 69 | |
SjorsdeBruin | 30:a16519224d58 | 70 | |
SjorsdeBruin | 27:d37b3a0e0f2b | 71 | BiQuad Lowpass_bl ( 1.55148e-04, 3.10297e-04, 1.55148e-04, -1.96446e+00, 9.65081e-01 ); |
SjorsdeBruin | 27:d37b3a0e0f2b | 72 | BiQuad Highpass_bl ( 9.40809e-01, -9.40809e-01, 0.00000e+00, -8.81619e-01, 0.00000e+00); |
SjorsdeBruin | 27:d37b3a0e0f2b | 73 | BiQuad notch_bl (9.98432e-01, -1.89913e+00, 9.98432e-01, -1.89913e+00, 9.96886e-01); |
SjorsdeBruin | 27:d37b3a0e0f2b | 74 | BiQuad Lowpass_br ( 1.55148e-04, 3.10297e-04, 1.55148e-04, -1.96446e+00, 9.65081e-01 ); |
SjorsdeBruin | 27:d37b3a0e0f2b | 75 | BiQuad Highpass_br ( 9.40809e-01, -9.40809e-01, 0.00000e+00, -8.81619e-01, 0.00000e+00); |
SjorsdeBruin | 27:d37b3a0e0f2b | 76 | BiQuad notch_br (9.98432e-01, -1.89913e+00, 9.98432e-01, -1.89913e+00, 9.96886e-01); |
SjorsdeBruin | 27:d37b3a0e0f2b | 77 | BiQuad Lowpass_leg ( 1.55148e-04, 3.10297e-04, 1.55148e-04, -1.96446e+00, 9.65081e-01 ); |
SjorsdeBruin | 27:d37b3a0e0f2b | 78 | BiQuad Highpass_leg ( 9.40809e-01, -9.40809e-01, 0.00000e+00, -8.81619e-01, 0.00000e+00); |
SjorsdeBruin | 27:d37b3a0e0f2b | 79 | BiQuad notch_leg (9.98432e-01, -1.89913e+00, 9.98432e-01, -1.89913e+00, 9.96886e-01); |
SjorsdeBruin | 26:088e397ec26f | 80 | |
Hendrikvg | 24:a9ec9b836fd9 | 81 | int n = 0; |
Hendrikvg | 24:a9ec9b836fd9 | 82 | |
SjorsdeBruin | 26:088e397ec26f | 83 | float emgFiltered_bl; |
SjorsdeBruin | 26:088e397ec26f | 84 | float emgFiltered_br; |
SjorsdeBruin | 26:088e397ec26f | 85 | float emgFiltered_leg; |
SjorsdeBruin | 26:088e397ec26f | 86 | float emg; |
SjorsdeBruin | 26:088e397ec26f | 87 | float xmvc_value = 1e-11; |
Hendrikvg | 24:a9ec9b836fd9 | 88 | |
Hendrikvg | 24:a9ec9b836fd9 | 89 | float sum = 0; |
SjorsdeBruin | 26:088e397ec26f | 90 | float xrest_value; |
Hendrikvg | 24:a9ec9b836fd9 | 91 | float rest_value_bl; |
Hendrikvg | 24:a9ec9b836fd9 | 92 | float rest_value_br; |
Hendrikvg | 24:a9ec9b836fd9 | 93 | float rest_value_leg; |
Hendrikvg | 24:a9ec9b836fd9 | 94 | |
Hendrikvg | 24:a9ec9b836fd9 | 95 | float mvc_value_bl; |
Hendrikvg | 24:a9ec9b836fd9 | 96 | float mvc_value_br; |
Hendrikvg | 24:a9ec9b836fd9 | 97 | float mvc_value_leg; |
Hendrikvg | 24:a9ec9b836fd9 | 98 | |
SjorsdeBruin | 28:8c90a46b613e | 99 | float treshold_bl = 0.5; |
SjorsdeBruin | 28:8c90a46b613e | 100 | float treshold_br = 0.5; |
SjorsdeBruin | 28:8c90a46b613e | 101 | float treshold_leg = 0.5; |
SjorsdeBruin | 28:8c90a46b613e | 102 | |
SjorsdeBruin | 28:8c90a46b613e | 103 | bool previous_value_emg_leg; |
SjorsdeBruin | 28:8c90a46b613e | 104 | bool current_value_emg_leg; |
SjorsdeBruin | 28:8c90a46b613e | 105 | |
SjorsdeBruin | 28:8c90a46b613e | 106 | |
Hendrikvg | 21:394a7a1deb73 | 107 | // functies |
SjorsdeBruin | 26:088e397ec26f | 108 | void ledred() |
SjorsdeBruin | 26:088e397ec26f | 109 | { |
SjorsdeBruin | 26:088e397ec26f | 110 | led_red = 0; |
SjorsdeBruin | 26:088e397ec26f | 111 | led_green = 1; |
SjorsdeBruin | 26:088e397ec26f | 112 | led_blue = 1; |
SjorsdeBruin | 26:088e397ec26f | 113 | } |
SjorsdeBruin | 26:088e397ec26f | 114 | void ledgreen() |
SjorsdeBruin | 26:088e397ec26f | 115 | { |
SjorsdeBruin | 26:088e397ec26f | 116 | led_green=0; |
SjorsdeBruin | 26:088e397ec26f | 117 | led_blue=1; |
SjorsdeBruin | 26:088e397ec26f | 118 | led_red=1; |
SjorsdeBruin | 26:088e397ec26f | 119 | } |
SjorsdeBruin | 26:088e397ec26f | 120 | void ledblue() |
SjorsdeBruin | 26:088e397ec26f | 121 | { |
SjorsdeBruin | 26:088e397ec26f | 122 | led_green=1; |
SjorsdeBruin | 26:088e397ec26f | 123 | led_blue=0; |
SjorsdeBruin | 26:088e397ec26f | 124 | led_red=1; |
SjorsdeBruin | 26:088e397ec26f | 125 | } |
SjorsdeBruin | 26:088e397ec26f | 126 | void ledyellow() |
SjorsdeBruin | 26:088e397ec26f | 127 | { |
SjorsdeBruin | 26:088e397ec26f | 128 | led_green=0; |
SjorsdeBruin | 26:088e397ec26f | 129 | led_blue=1; |
SjorsdeBruin | 26:088e397ec26f | 130 | led_red=0; |
SjorsdeBruin | 26:088e397ec26f | 131 | } |
SjorsdeBruin | 26:088e397ec26f | 132 | void ledmagenta() |
SjorsdeBruin | 26:088e397ec26f | 133 | { |
SjorsdeBruin | 26:088e397ec26f | 134 | led_green=1; |
SjorsdeBruin | 26:088e397ec26f | 135 | led_blue=0; |
SjorsdeBruin | 26:088e397ec26f | 136 | led_red=0; |
SjorsdeBruin | 26:088e397ec26f | 137 | } |
SjorsdeBruin | 26:088e397ec26f | 138 | void ledcyan() |
SjorsdeBruin | 26:088e397ec26f | 139 | { |
SjorsdeBruin | 26:088e397ec26f | 140 | led_green=0; |
SjorsdeBruin | 26:088e397ec26f | 141 | led_blue=0; |
SjorsdeBruin | 26:088e397ec26f | 142 | led_red=1; |
SjorsdeBruin | 26:088e397ec26f | 143 | } |
SjorsdeBruin | 26:088e397ec26f | 144 | void ledwhite() |
SjorsdeBruin | 26:088e397ec26f | 145 | { |
SjorsdeBruin | 26:088e397ec26f | 146 | led_green=0; |
SjorsdeBruin | 26:088e397ec26f | 147 | led_blue=0; |
SjorsdeBruin | 26:088e397ec26f | 148 | led_red=0; |
SjorsdeBruin | 26:088e397ec26f | 149 | } |
SjorsdeBruin | 26:088e397ec26f | 150 | void ledoff() |
SjorsdeBruin | 26:088e397ec26f | 151 | { |
SjorsdeBruin | 26:088e397ec26f | 152 | led_green=1; |
SjorsdeBruin | 26:088e397ec26f | 153 | led_blue=1; |
SjorsdeBruin | 26:088e397ec26f | 154 | led_red=1; |
SjorsdeBruin | 26:088e397ec26f | 155 | } |
SjorsdeBruin | 26:088e397ec26f | 156 | |
SjorsdeBruin | 29:5a846abba59e | 157 | void Controller() |
Hendrikvg | 21:394a7a1deb73 | 158 | { |
SjorsdeBruin | 29:5a846abba59e | 159 | float K = 1; |
SjorsdeBruin | 29:5a846abba59e | 160 | float ti = 0.1; |
SjorsdeBruin | 29:5a846abba59e | 161 | float td = 10; |
SjorsdeBruin | 29:5a846abba59e | 162 | Kp = K*(1+td/ti); |
SjorsdeBruin | 29:5a846abba59e | 163 | Ki = K/ti; |
SjorsdeBruin | 29:5a846abba59e | 164 | Kd = K*td; |
SjorsdeBruin | 29:5a846abba59e | 165 | |
SjorsdeBruin | 30:a16519224d58 | 166 | theta_error1 = theta_ref1-theta_1; |
SjorsdeBruin | 30:a16519224d58 | 167 | theta_error2 = theta_ref2-theta_2; |
SjorsdeBruin | 30:a16519224d58 | 168 | |
SjorsdeBruin | 29:5a846abba59e | 169 | float error_integral1 = 0; |
SjorsdeBruin | 29:5a846abba59e | 170 | float error_integral2 = 0; |
SjorsdeBruin | 29:5a846abba59e | 171 | float error_prev1 = 0; |
SjorsdeBruin | 29:5a846abba59e | 172 | float error_prev2 = 0; |
Hendrikvg | 21:394a7a1deb73 | 173 | static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241); |
Hendrikvg | 24:a9ec9b836fd9 | 174 | |
Hendrikvg | 20:ac1b4ffa3323 | 175 | // Proportional part: |
SjorsdeBruin | 29:5a846abba59e | 176 | float torque_p1 = Kp * theta_error1; |
SjorsdeBruin | 29:5a846abba59e | 177 | float torque_p2 = Kp * theta_error2; |
SjorsdeBruin | 29:5a846abba59e | 178 | |
Hendrikvg | 20:ac1b4ffa3323 | 179 | // Integral part: |
SjorsdeBruin | 29:5a846abba59e | 180 | error_integral1 = error_integral1 + theta_error1 * Ts; |
SjorsdeBruin | 29:5a846abba59e | 181 | error_integral2 = error_integral2 + theta_error2 * Ts; |
SjorsdeBruin | 29:5a846abba59e | 182 | float torque_i1 = Ki * error_integral1; |
SjorsdeBruin | 29:5a846abba59e | 183 | float torque_i2 = Ki * error_integral2; |
Hendrikvg | 24:a9ec9b836fd9 | 184 | |
Hendrikvg | 20:ac1b4ffa3323 | 185 | // Derivative part: |
SjorsdeBruin | 29:5a846abba59e | 186 | float error_derivative1 = (theta_error1 - error_prev1)/Ts; |
SjorsdeBruin | 29:5a846abba59e | 187 | float error_derivative2 = (theta_error2 - error_prev2)/Ts; |
SjorsdeBruin | 29:5a846abba59e | 188 | float filtered_error_derivative1 = LowPassFilter.step(error_derivative1); |
SjorsdeBruin | 29:5a846abba59e | 189 | float filtered_error_derivative2 = LowPassFilter.step(error_derivative2); |
SjorsdeBruin | 29:5a846abba59e | 190 | float torque_d1 = Kd * filtered_error_derivative1; |
SjorsdeBruin | 29:5a846abba59e | 191 | float torque_d2 = Kd * filtered_error_derivative2; |
SjorsdeBruin | 29:5a846abba59e | 192 | error_prev1 = theta_error1; |
SjorsdeBruin | 29:5a846abba59e | 193 | error_prev2 = theta_error2; |
Hendrikvg | 24:a9ec9b836fd9 | 194 | |
Hendrikvg | 20:ac1b4ffa3323 | 195 | // Sum all parts and return it |
SjorsdeBruin | 29:5a846abba59e | 196 | torque_1 = torque_p1 + torque_i1 + torque_d1; |
SjorsdeBruin | 29:5a846abba59e | 197 | torque_2 = torque_p2 + torque_i2 + torque_d2; |
Hendrikvg | 21:394a7a1deb73 | 198 | } |
Hendrikvg | 16:40183eeadb6d | 199 | |
SjorsdeBruin | 30:a16519224d58 | 200 | void Kinematics() |
SjorsdeBruin | 28:8c90a46b613e | 201 | { |
SjorsdeBruin | 28:8c90a46b613e | 202 | float DET_jacobian= 1.0f/((-l*sin(theta_1)-l*sin(theta_1+theta_2))*(l*cos(theta_1+theta_2))-(-l*sin(theta_1+theta_2))*(l*cos(theta_1)+l*cos(theta_1+theta_2))); |
SjorsdeBruin | 30:a16519224d58 | 203 | float thetav_1=DET_jacobian*l*cos(theta_1+theta_2)*EMGx_velocity + DET_jacobian*l*sin(theta_1+theta_2)*EMGy_velocity; |
SjorsdeBruin | 30:a16519224d58 | 204 | float thetav_2= DET_jacobian*-l*cos(theta_1)-l*cos(theta_1+theta_2)*EMGx_velocity + DET_jacobian*-l*sin(theta_1)-l*sin(theta_1+theta_2)*EMGy_velocity; |
SjorsdeBruin | 29:5a846abba59e | 205 | theta_ref1=theta_1+thetav_1*Ts; |
SjorsdeBruin | 29:5a846abba59e | 206 | theta_ref2=theta_2+thetav_2*Ts; |
SjorsdeBruin | 28:8c90a46b613e | 207 | x=cos(theta_ref1)*l+cos(theta_ref1+theta_ref2)*l; |
SjorsdeBruin | 28:8c90a46b613e | 208 | y=sin(theta_ref1)*l+sin(theta_ref1+theta_ref2)*l; |
SjorsdeBruin | 29:5a846abba59e | 209 | if (sqrt(pow(x,2)+pow(y,2))>1.0f) { |
SjorsdeBruin | 30:a16519224d58 | 210 | theta_ref1 = theta_1; |
SjorsdeBruin | 30:a16519224d58 | 211 | theta_ref2 = theta_2; |
SjorsdeBruin | 28:8c90a46b613e | 212 | } |
SjorsdeBruin | 28:8c90a46b613e | 213 | } |
Hendrikvg | 24:a9ec9b836fd9 | 214 | void CalculateDirectionMotor() |
Hendrikvg | 21:394a7a1deb73 | 215 | { |
SjorsdeBruin | 30:a16519224d58 | 216 | Kinematics(); |
SjorsdeBruin | 29:5a846abba59e | 217 | Controller(); |
SjorsdeBruin | 29:5a846abba59e | 218 | direction_motor_1 = torque_1 <= 0.0f; |
SjorsdeBruin | 29:5a846abba59e | 219 | direction_motor_2 = torque_2 <= 0.0f; |
Hendrikvg | 21:394a7a1deb73 | 220 | } |
Hendrikvg | 20:ac1b4ffa3323 | 221 | |
Hendrikvg | 23:78898ddfb103 | 222 | void ReadEncoder() |
Hendrikvg | 23:78898ddfb103 | 223 | { |
SjorsdeBruin | 30:a16519224d58 | 224 | theta_1 = (((2.0f*pi)/64.0f)*(float)encoder_1.getPulses())/131.25f; // 360/64 voor de 64 CPR Encoder, 131.25 omdat de gear ratio 131.25:1 is. Hiermee is 1 omwenteling 360. |
SjorsdeBruin | 30:a16519224d58 | 225 | theta_2 = (((2.0f*pi)/64.0f)*(float)encoder_2.getPulses())/131.25f; |
Hendrikvg | 23:78898ddfb103 | 226 | } |
Hendrikvg | 16:40183eeadb6d | 227 | |
Hendrikvg | 24:a9ec9b836fd9 | 228 | void MotorControl() |
Hendrikvg | 23:78898ddfb103 | 229 | { |
Hendrikvg | 21:394a7a1deb73 | 230 | ReadEncoder(); |
Hendrikvg | 22:6cc93216b323 | 231 | CalculateDirectionMotor(); |
SjorsdeBruin | 30:a16519224d58 | 232 | PWM_motor_1.write(fabs(torque_1)/(2.0f*pi)); |
SjorsdeBruin | 30:a16519224d58 | 233 | PWM_motor_2.write(fabs(torque_2)/(2.0f*pi)); |
Hendrikvg | 21:394a7a1deb73 | 234 | } |
Hendrikvg | 21:394a7a1deb73 | 235 | |
Hendrikvg | 21:394a7a1deb73 | 236 | void go_next_1() |
Hendrikvg | 21:394a7a1deb73 | 237 | { |
Hendrikvg | 24:a9ec9b836fd9 | 238 | pressed_1 = !pressed_1; |
Hendrikvg | 21:394a7a1deb73 | 239 | } |
Hendrikvg | 21:394a7a1deb73 | 240 | |
Hendrikvg | 21:394a7a1deb73 | 241 | void go_next_2() |
Hendrikvg | 21:394a7a1deb73 | 242 | { |
Hendrikvg | 24:a9ec9b836fd9 | 243 | pressed_2 = !pressed_2; |
Hendrikvg | 24:a9ec9b836fd9 | 244 | } |
Hendrikvg | 24:a9ec9b836fd9 | 245 | |
SjorsdeBruin | 27:d37b3a0e0f2b | 246 | bool emg_switch(float treshold, float emg_input) { |
SjorsdeBruin | 27:d37b3a0e0f2b | 247 | if(emg_input > treshold){ |
SjorsdeBruin | 28:8c90a46b613e | 248 | current_value_emg_leg = true; |
SjorsdeBruin | 28:8c90a46b613e | 249 | } else { |
SjorsdeBruin | 28:8c90a46b613e | 250 | current_value_emg_leg = false; |
SjorsdeBruin | 28:8c90a46b613e | 251 | } |
SjorsdeBruin | 28:8c90a46b613e | 252 | if(current_value_emg_leg == true && previous_value_emg_leg == false) { |
SjorsdeBruin | 28:8c90a46b613e | 253 | previous_value_emg_leg = current_value_emg_leg; |
SjorsdeBruin | 28:8c90a46b613e | 254 | return true; |
SjorsdeBruin | 28:8c90a46b613e | 255 | } else { |
SjorsdeBruin | 28:8c90a46b613e | 256 | previous_value_emg_leg = current_value_emg_leg; |
SjorsdeBruin | 28:8c90a46b613e | 257 | return false; |
SjorsdeBruin | 28:8c90a46b613e | 258 | } |
SjorsdeBruin | 28:8c90a46b613e | 259 | } |
SjorsdeBruin | 28:8c90a46b613e | 260 | |
SjorsdeBruin | 28:8c90a46b613e | 261 | bool emg_trigger(float treshold, float emg_input) { |
SjorsdeBruin | 28:8c90a46b613e | 262 | if(emg_input > treshold) { |
SjorsdeBruin | 27:d37b3a0e0f2b | 263 | return true; |
SjorsdeBruin | 27:d37b3a0e0f2b | 264 | } else { |
SjorsdeBruin | 27:d37b3a0e0f2b | 265 | return false; |
SjorsdeBruin | 27:d37b3a0e0f2b | 266 | } |
SjorsdeBruin | 27:d37b3a0e0f2b | 267 | } |
SjorsdeBruin | 27:d37b3a0e0f2b | 268 | |
SjorsdeBruin | 26:088e397ec26f | 269 | float EmgCalibration(float emgFiltered, float mvc_value, float rest_value) |
Hendrikvg | 24:a9ec9b836fd9 | 270 | { |
Hendrikvg | 24:a9ec9b836fd9 | 271 | float emgCalibrated; |
Hendrikvg | 24:a9ec9b836fd9 | 272 | if (emgFiltered <= rest_value) { |
SjorsdeBruin | 27:d37b3a0e0f2b | 273 | return 0.0f; |
SjorsdeBruin | 27:d37b3a0e0f2b | 274 | //emgCalibrated = 0; |
Hendrikvg | 24:a9ec9b836fd9 | 275 | } |
SjorsdeBruin | 27:d37b3a0e0f2b | 276 | |
Hendrikvg | 24:a9ec9b836fd9 | 277 | if (emgFiltered >= mvc_value) { |
SjorsdeBruin | 27:d37b3a0e0f2b | 278 | return 1.1f; |
SjorsdeBruin | 27:d37b3a0e0f2b | 279 | //emgCalibrated = 1; |
Hendrikvg | 24:a9ec9b836fd9 | 280 | } else { |
Hendrikvg | 24:a9ec9b836fd9 | 281 | emgCalibrated = (emgFiltered-rest_value)/(mvc_value-rest_value); |
Hendrikvg | 24:a9ec9b836fd9 | 282 | } |
Hendrikvg | 24:a9ec9b836fd9 | 283 | return emgCalibrated; |
Hendrikvg | 24:a9ec9b836fd9 | 284 | } |
Hendrikvg | 24:a9ec9b836fd9 | 285 | |
Hendrikvg | 24:a9ec9b836fd9 | 286 | void emgsample() |
Hendrikvg | 24:a9ec9b836fd9 | 287 | { |
SjorsdeBruin | 27:d37b3a0e0f2b | 288 | emgFiltered_bl = Highpass_bl.step(emg_bl.read()); |
SjorsdeBruin | 27:d37b3a0e0f2b | 289 | emgFiltered_bl = notch_bl.step(emgFiltered_bl); |
Hendrikvg | 24:a9ec9b836fd9 | 290 | emgFiltered_bl = fabs(emgFiltered_bl); |
SjorsdeBruin | 27:d37b3a0e0f2b | 291 | emgFiltered_bl = Lowpass_bl.step(emgFiltered_bl); |
Hendrikvg | 24:a9ec9b836fd9 | 292 | |
SjorsdeBruin | 27:d37b3a0e0f2b | 293 | emgFiltered_br = Highpass_br.step(emg_br.read()); |
SjorsdeBruin | 27:d37b3a0e0f2b | 294 | emgFiltered_br = notch_br.step(emgFiltered_br); |
Hendrikvg | 24:a9ec9b836fd9 | 295 | emgFiltered_br = fabs(emgFiltered_br); |
SjorsdeBruin | 27:d37b3a0e0f2b | 296 | emgFiltered_br = Lowpass_br.step(emgFiltered_br); |
Hendrikvg | 24:a9ec9b836fd9 | 297 | |
SjorsdeBruin | 27:d37b3a0e0f2b | 298 | emgFiltered_leg = Highpass_leg.step(emg_leg.read()); |
SjorsdeBruin | 27:d37b3a0e0f2b | 299 | emgFiltered_leg = notch_leg.step(emgFiltered_leg); |
Hendrikvg | 24:a9ec9b836fd9 | 300 | emgFiltered_leg = fabs(emgFiltered_leg); |
SjorsdeBruin | 27:d37b3a0e0f2b | 301 | emgFiltered_leg = Lowpass_leg.step(emgFiltered_leg); |
Hendrikvg | 24:a9ec9b836fd9 | 302 | } |
Hendrikvg | 24:a9ec9b836fd9 | 303 | |
Hendrikvg | 24:a9ec9b836fd9 | 304 | void rest() |
Hendrikvg | 24:a9ec9b836fd9 | 305 | { |
SjorsdeBruin | 26:088e397ec26f | 306 | if (CurrentSubstate == rest_biceps_left) { |
Hendrikvg | 24:a9ec9b836fd9 | 307 | emg = emgFiltered_bl; |
SjorsdeBruin | 26:088e397ec26f | 308 | //pc.printf("emg: %f \n\r",emgFiltered_bl); |
Hendrikvg | 24:a9ec9b836fd9 | 309 | } |
SjorsdeBruin | 26:088e397ec26f | 310 | if (CurrentSubstate == rest_biceps_right) { |
Hendrikvg | 24:a9ec9b836fd9 | 311 | emg = emgFiltered_br; |
Hendrikvg | 24:a9ec9b836fd9 | 312 | } |
SjorsdeBruin | 26:088e397ec26f | 313 | if (CurrentSubstate == rest_biceps_leg) { |
Hendrikvg | 24:a9ec9b836fd9 | 314 | emg = emgFiltered_leg; |
Hendrikvg | 24:a9ec9b836fd9 | 315 | } |
SjorsdeBruin | 26:088e397ec26f | 316 | if (n < 500) { |
SjorsdeBruin | 26:088e397ec26f | 317 | ledred(); |
Hendrikvg | 24:a9ec9b836fd9 | 318 | sum = sum + emg; |
SjorsdeBruin | 26:088e397ec26f | 319 | //pc.printf("sum: %f \n\r",sum); |
Hendrikvg | 24:a9ec9b836fd9 | 320 | n++; |
SjorsdeBruin | 26:088e397ec26f | 321 | rest_timeout.attach(rest,0.001f); |
Hendrikvg | 24:a9ec9b836fd9 | 322 | } |
SjorsdeBruin | 26:088e397ec26f | 323 | if (n == 500) { |
Hendrikvg | 24:a9ec9b836fd9 | 324 | sum = sum + emg; |
SjorsdeBruin | 26:088e397ec26f | 325 | //pc.printf("sum: %f \n\r",sum); |
Hendrikvg | 24:a9ec9b836fd9 | 326 | n++; |
SjorsdeBruin | 26:088e397ec26f | 327 | xrest_value = float (sum/n); |
SjorsdeBruin | 26:088e397ec26f | 328 | if (CurrentSubstate == rest_biceps_left) { |
SjorsdeBruin | 26:088e397ec26f | 329 | rest_value_bl = xrest_value; |
SjorsdeBruin | 26:088e397ec26f | 330 | pc.printf("rest_value_bl %f \n\r", rest_value_bl); |
Hendrikvg | 24:a9ec9b836fd9 | 331 | CurrentSubstate = mvc_biceps_left; |
Hendrikvg | 24:a9ec9b836fd9 | 332 | SubstateChanged = true; |
SjorsdeBruin | 26:088e397ec26f | 333 | ledblue(); |
SjorsdeBruin | 26:088e397ec26f | 334 | |
Hendrikvg | 24:a9ec9b836fd9 | 335 | } |
SjorsdeBruin | 26:088e397ec26f | 336 | if (CurrentSubstate == rest_biceps_right) { |
SjorsdeBruin | 26:088e397ec26f | 337 | rest_value_br = xrest_value; |
SjorsdeBruin | 26:088e397ec26f | 338 | pc.printf("rest_value_br %f \n\r", rest_value_br); |
Hendrikvg | 24:a9ec9b836fd9 | 339 | CurrentSubstate = mvc_biceps_right; |
Hendrikvg | 24:a9ec9b836fd9 | 340 | SubstateChanged = true; |
SjorsdeBruin | 26:088e397ec26f | 341 | ledmagenta(); |
Hendrikvg | 24:a9ec9b836fd9 | 342 | } |
SjorsdeBruin | 26:088e397ec26f | 343 | if (CurrentSubstate == rest_biceps_leg) { |
SjorsdeBruin | 26:088e397ec26f | 344 | rest_value_leg = xrest_value; |
SjorsdeBruin | 26:088e397ec26f | 345 | pc.printf("rest_value_leg %f \n\r", rest_value_leg); |
SjorsdeBruin | 26:088e397ec26f | 346 | pc.printf("rest_value_bl %f \n\r", rest_value_bl); |
Hendrikvg | 24:a9ec9b836fd9 | 347 | CurrentSubstate = mvc_biceps_leg; |
Hendrikvg | 24:a9ec9b836fd9 | 348 | SubstateChanged = true; |
SjorsdeBruin | 26:088e397ec26f | 349 | ledwhite(); |
Hendrikvg | 24:a9ec9b836fd9 | 350 | } |
Hendrikvg | 24:a9ec9b836fd9 | 351 | } |
Hendrikvg | 24:a9ec9b836fd9 | 352 | } |
Hendrikvg | 24:a9ec9b836fd9 | 353 | |
Hendrikvg | 24:a9ec9b836fd9 | 354 | void mvc() |
Hendrikvg | 24:a9ec9b836fd9 | 355 | { |
SjorsdeBruin | 26:088e397ec26f | 356 | if (CurrentSubstate == mvc_biceps_left) { |
Hendrikvg | 24:a9ec9b836fd9 | 357 | emg = emgFiltered_bl; |
Hendrikvg | 24:a9ec9b836fd9 | 358 | } |
SjorsdeBruin | 26:088e397ec26f | 359 | if (CurrentSubstate == mvc_biceps_right) { |
Hendrikvg | 24:a9ec9b836fd9 | 360 | emg = emgFiltered_br; |
Hendrikvg | 24:a9ec9b836fd9 | 361 | } |
SjorsdeBruin | 26:088e397ec26f | 362 | if (CurrentSubstate == mvc_biceps_leg) { |
Hendrikvg | 24:a9ec9b836fd9 | 363 | emg = emgFiltered_leg; |
Hendrikvg | 24:a9ec9b836fd9 | 364 | } |
SjorsdeBruin | 26:088e397ec26f | 365 | if (emg >= xmvc_value) { |
Hendrikvg | 24:a9ec9b836fd9 | 366 | xmvc_value = emg; |
SjorsdeBruin | 26:088e397ec26f | 367 | } |
Hendrikvg | 24:a9ec9b836fd9 | 368 | n++; |
SjorsdeBruin | 26:088e397ec26f | 369 | if (n < 1000) { |
SjorsdeBruin | 26:088e397ec26f | 370 | mvc_timeout.attach(mvc,0.001f); |
SjorsdeBruin | 26:088e397ec26f | 371 | ledred(); |
Hendrikvg | 24:a9ec9b836fd9 | 372 | } |
SjorsdeBruin | 26:088e397ec26f | 373 | if (n == 1000) { |
SjorsdeBruin | 26:088e397ec26f | 374 | if (CurrentSubstate == mvc_biceps_left) { |
Hendrikvg | 24:a9ec9b836fd9 | 375 | mvc_value_bl = xmvc_value; |
SjorsdeBruin | 26:088e397ec26f | 376 | pc.printf("mvc_value_bl %f \n\r", mvc_value_bl); |
Hendrikvg | 24:a9ec9b836fd9 | 377 | CurrentSubstate = rest_biceps_right; |
Hendrikvg | 24:a9ec9b836fd9 | 378 | SubstateChanged = true; |
SjorsdeBruin | 26:088e397ec26f | 379 | ledyellow(); |
Hendrikvg | 24:a9ec9b836fd9 | 380 | } |
SjorsdeBruin | 26:088e397ec26f | 381 | if (CurrentSubstate == mvc_biceps_right) { |
Hendrikvg | 24:a9ec9b836fd9 | 382 | mvc_value_br = xmvc_value; |
SjorsdeBruin | 26:088e397ec26f | 383 | pc.printf("mvc_value_br %f \n\r", mvc_value_br); |
Hendrikvg | 24:a9ec9b836fd9 | 384 | CurrentSubstate = rest_biceps_leg; |
Hendrikvg | 24:a9ec9b836fd9 | 385 | SubstateChanged = true; |
SjorsdeBruin | 26:088e397ec26f | 386 | ledcyan(); |
Hendrikvg | 24:a9ec9b836fd9 | 387 | } |
SjorsdeBruin | 26:088e397ec26f | 388 | if (CurrentSubstate == mvc_biceps_leg) { |
Hendrikvg | 24:a9ec9b836fd9 | 389 | mvc_value_leg = xmvc_value; |
SjorsdeBruin | 26:088e397ec26f | 390 | pc.printf("mvc_value_leg %f \n\r", mvc_value_leg); |
Hendrikvg | 24:a9ec9b836fd9 | 391 | CurrentState = vertical_movement; |
Hendrikvg | 24:a9ec9b836fd9 | 392 | StateChanged = true; |
SjorsdeBruin | 26:088e397ec26f | 393 | ledoff(); |
Hendrikvg | 24:a9ec9b836fd9 | 394 | } |
Hendrikvg | 24:a9ec9b836fd9 | 395 | xmvc_value = 1e-11; |
Hendrikvg | 24:a9ec9b836fd9 | 396 | } |
Hendrikvg | 24:a9ec9b836fd9 | 397 | } |
Hendrikvg | 24:a9ec9b836fd9 | 398 | |
Hendrikvg | 24:a9ec9b836fd9 | 399 | void WriteScope() |
Hendrikvg | 24:a9ec9b836fd9 | 400 | { |
SjorsdeBruin | 26:088e397ec26f | 401 | emgsample(); |
Hendrikvg | 24:a9ec9b836fd9 | 402 | scope.set(0, EmgCalibration(emgFiltered_bl, mvc_value_bl, rest_value_bl)); |
SjorsdeBruin | 26:088e397ec26f | 403 | scope.set(1, EmgCalibration(emgFiltered_br, mvc_value_br, rest_value_br)); |
Hendrikvg | 24:a9ec9b836fd9 | 404 | scope.set(2, EmgCalibration(emgFiltered_leg, mvc_value_leg, rest_value_leg)); |
SjorsdeBruin | 27:d37b3a0e0f2b | 405 | scope.set(3, emgFiltered_bl); |
SjorsdeBruin | 27:d37b3a0e0f2b | 406 | scope.set(4, emgFiltered_br); |
SjorsdeBruin | 27:d37b3a0e0f2b | 407 | scope.set(5, emgFiltered_leg); |
Hendrikvg | 24:a9ec9b836fd9 | 408 | scope.send(); |
Hendrikvg | 24:a9ec9b836fd9 | 409 | } |
Hendrikvg | 24:a9ec9b836fd9 | 410 | |
Hendrikvg | 24:a9ec9b836fd9 | 411 | void SubstateTransition() |
Hendrikvg | 24:a9ec9b836fd9 | 412 | { |
SjorsdeBruin | 26:088e397ec26f | 413 | if (SubstateChanged == true) { |
SjorsdeBruin | 26:088e397ec26f | 414 | SubstateChanged = false; |
SjorsdeBruin | 26:088e397ec26f | 415 | pressed_1 = false; |
SjorsdeBruin | 26:088e397ec26f | 416 | pressed_2 = false; |
SjorsdeBruin | 26:088e397ec26f | 417 | if (CurrentSubstate == rest_biceps_left) { |
SjorsdeBruin | 26:088e397ec26f | 418 | ledgreen(); |
SjorsdeBruin | 26:088e397ec26f | 419 | pc.printf("groen \n\r"); |
SjorsdeBruin | 26:088e397ec26f | 420 | pc.printf("Initiating rest_biceps_left\n\r"); |
SjorsdeBruin | 26:088e397ec26f | 421 | } |
SjorsdeBruin | 26:088e397ec26f | 422 | if (CurrentSubstate == mvc_biceps_left) { |
SjorsdeBruin | 26:088e397ec26f | 423 | //ledblue(); |
SjorsdeBruin | 26:088e397ec26f | 424 | pc.printf("Initiating mvc_biceps_left\n\r"); |
SjorsdeBruin | 26:088e397ec26f | 425 | } |
SjorsdeBruin | 26:088e397ec26f | 426 | if (CurrentSubstate == rest_biceps_right) { |
SjorsdeBruin | 26:088e397ec26f | 427 | //ledyellow(); |
SjorsdeBruin | 26:088e397ec26f | 428 | pc.printf("Initiating rest_biceps_right\n\r"); |
SjorsdeBruin | 26:088e397ec26f | 429 | } |
SjorsdeBruin | 26:088e397ec26f | 430 | if (CurrentSubstate == mvc_biceps_right) { |
SjorsdeBruin | 26:088e397ec26f | 431 | //ledmagenta(); |
SjorsdeBruin | 26:088e397ec26f | 432 | pc.printf("Initiating mvc_biceps_right\n\r"); |
SjorsdeBruin | 26:088e397ec26f | 433 | } |
SjorsdeBruin | 26:088e397ec26f | 434 | if (CurrentSubstate == rest_biceps_leg) { |
SjorsdeBruin | 26:088e397ec26f | 435 | //ledcyan(); |
SjorsdeBruin | 26:088e397ec26f | 436 | pc.printf("Initiating rest_biceps_leg\n\r"); |
SjorsdeBruin | 26:088e397ec26f | 437 | } |
SjorsdeBruin | 26:088e397ec26f | 438 | if (CurrentSubstate == mvc_biceps_leg) { |
SjorsdeBruin | 26:088e397ec26f | 439 | //ledwhite(); |
SjorsdeBruin | 26:088e397ec26f | 440 | pc.printf("Initiating mvc_biceps_leg\n\r"); |
SjorsdeBruin | 26:088e397ec26f | 441 | } |
SjorsdeBruin | 26:088e397ec26f | 442 | } |
Hendrikvg | 21:394a7a1deb73 | 443 | } |
Hendrikvg | 21:394a7a1deb73 | 444 | |
Hendrikvg | 21:394a7a1deb73 | 445 | void while_start() |
Hendrikvg | 21:394a7a1deb73 | 446 | { |
Hendrikvg | 21:394a7a1deb73 | 447 | // Do startup stuff |
Hendrikvg | 21:394a7a1deb73 | 448 | CurrentState = motor_calibration; |
Hendrikvg | 21:394a7a1deb73 | 449 | StateChanged = true; |
Hendrikvg | 21:394a7a1deb73 | 450 | } |
Hendrikvg | 21:394a7a1deb73 | 451 | |
Hendrikvg | 21:394a7a1deb73 | 452 | void while_motor_calibration() |
Hendrikvg | 21:394a7a1deb73 | 453 | { |
SjorsdeBruin | 29:5a846abba59e | 454 | |
Hendrikvg | 21:394a7a1deb73 | 455 | // Do motor calibration stuff |
SjorsdeBruin | 29:5a846abba59e | 456 | |
Hendrikvg | 24:a9ec9b836fd9 | 457 | if (pressed_1) { // bool aanmaken voor demo (switch oid aanmaken) |
Hendrikvg | 21:394a7a1deb73 | 458 | CurrentState = demo_mode; |
Hendrikvg | 21:394a7a1deb73 | 459 | StateChanged = true; |
Hendrikvg | 21:394a7a1deb73 | 460 | } |
Hendrikvg | 24:a9ec9b836fd9 | 461 | if (pressed_2) { // bool aanmaken voor EMG (switch oid aanmaken) |
Hendrikvg | 21:394a7a1deb73 | 462 | CurrentState = emg_calibration; |
Hendrikvg | 21:394a7a1deb73 | 463 | StateChanged = true; |
Hendrikvg | 21:394a7a1deb73 | 464 | } |
Hendrikvg | 21:394a7a1deb73 | 465 | } |
Hendrikvg | 21:394a7a1deb73 | 466 | |
Hendrikvg | 21:394a7a1deb73 | 467 | void while_demo_mode() |
Hendrikvg | 21:394a7a1deb73 | 468 | { |
Hendrikvg | 21:394a7a1deb73 | 469 | // Do demo mode stuff |
SjorsdeBruin | 26:088e397ec26f | 470 | if ((pressed_1) || (pressed_2)) { |
Hendrikvg | 21:394a7a1deb73 | 471 | CurrentState = emg_calibration; |
Hendrikvg | 21:394a7a1deb73 | 472 | StateChanged = true; |
Hendrikvg | 21:394a7a1deb73 | 473 | } |
Hendrikvg | 21:394a7a1deb73 | 474 | } |
Hendrikvg | 21:394a7a1deb73 | 475 | |
Hendrikvg | 21:394a7a1deb73 | 476 | void while_emg_calibration() |
Hendrikvg | 21:394a7a1deb73 | 477 | { |
Hendrikvg | 21:394a7a1deb73 | 478 | // Do emg calibration stuff |
Hendrikvg | 24:a9ec9b836fd9 | 479 | switch (CurrentSubstate) { |
Hendrikvg | 24:a9ec9b836fd9 | 480 | case rest_biceps_left: |
Hendrikvg | 24:a9ec9b836fd9 | 481 | SubstateTransition(); |
SjorsdeBruin | 26:088e397ec26f | 482 | if ((pressed_1) || (pressed_2)) { |
SjorsdeBruin | 26:088e397ec26f | 483 | pressed_1 = false; |
SjorsdeBruin | 26:088e397ec26f | 484 | pressed_2 = false; |
SjorsdeBruin | 26:088e397ec26f | 485 | n = 0; |
SjorsdeBruin | 26:088e397ec26f | 486 | sum = 0; |
Hendrikvg | 24:a9ec9b836fd9 | 487 | rest(); |
Hendrikvg | 24:a9ec9b836fd9 | 488 | } |
Hendrikvg | 24:a9ec9b836fd9 | 489 | break; |
Hendrikvg | 24:a9ec9b836fd9 | 490 | case mvc_biceps_left: |
Hendrikvg | 24:a9ec9b836fd9 | 491 | SubstateTransition(); |
SjorsdeBruin | 26:088e397ec26f | 492 | if ((pressed_1) || (pressed_2)) { |
SjorsdeBruin | 26:088e397ec26f | 493 | pressed_1 = false; |
SjorsdeBruin | 26:088e397ec26f | 494 | pressed_2 = false; |
SjorsdeBruin | 26:088e397ec26f | 495 | n = 0; |
Hendrikvg | 24:a9ec9b836fd9 | 496 | mvc(); |
Hendrikvg | 24:a9ec9b836fd9 | 497 | } |
Hendrikvg | 24:a9ec9b836fd9 | 498 | break; |
Hendrikvg | 24:a9ec9b836fd9 | 499 | case rest_biceps_right: |
Hendrikvg | 24:a9ec9b836fd9 | 500 | SubstateTransition(); |
SjorsdeBruin | 26:088e397ec26f | 501 | if ((pressed_1) || (pressed_2)) { |
SjorsdeBruin | 26:088e397ec26f | 502 | pressed_1 = false; |
SjorsdeBruin | 26:088e397ec26f | 503 | pressed_2 = false; |
SjorsdeBruin | 26:088e397ec26f | 504 | n = 0; |
SjorsdeBruin | 26:088e397ec26f | 505 | sum = 0; |
Hendrikvg | 24:a9ec9b836fd9 | 506 | rest(); |
Hendrikvg | 24:a9ec9b836fd9 | 507 | } |
Hendrikvg | 24:a9ec9b836fd9 | 508 | break; |
Hendrikvg | 24:a9ec9b836fd9 | 509 | case mvc_biceps_right: |
Hendrikvg | 24:a9ec9b836fd9 | 510 | SubstateTransition(); |
SjorsdeBruin | 26:088e397ec26f | 511 | if ((pressed_1) || (pressed_2)) { |
SjorsdeBruin | 26:088e397ec26f | 512 | pressed_1 = false; |
SjorsdeBruin | 26:088e397ec26f | 513 | pressed_2 = false; |
SjorsdeBruin | 26:088e397ec26f | 514 | n = 0; |
Hendrikvg | 24:a9ec9b836fd9 | 515 | mvc(); |
Hendrikvg | 24:a9ec9b836fd9 | 516 | } |
Hendrikvg | 24:a9ec9b836fd9 | 517 | break; |
Hendrikvg | 24:a9ec9b836fd9 | 518 | case rest_biceps_leg: |
Hendrikvg | 24:a9ec9b836fd9 | 519 | SubstateTransition(); |
SjorsdeBruin | 26:088e397ec26f | 520 | if ((pressed_1) || (pressed_2)) { |
SjorsdeBruin | 26:088e397ec26f | 521 | pressed_1 = false; |
SjorsdeBruin | 26:088e397ec26f | 522 | pressed_2 = false; |
SjorsdeBruin | 26:088e397ec26f | 523 | n = 0; |
SjorsdeBruin | 26:088e397ec26f | 524 | sum = 0; |
Hendrikvg | 24:a9ec9b836fd9 | 525 | rest(); |
Hendrikvg | 24:a9ec9b836fd9 | 526 | } |
Hendrikvg | 24:a9ec9b836fd9 | 527 | break; |
Hendrikvg | 24:a9ec9b836fd9 | 528 | case mvc_biceps_leg: |
Hendrikvg | 24:a9ec9b836fd9 | 529 | SubstateTransition(); |
SjorsdeBruin | 26:088e397ec26f | 530 | if ((pressed_1) || (pressed_2)) { |
SjorsdeBruin | 26:088e397ec26f | 531 | pressed_1 = false; |
SjorsdeBruin | 26:088e397ec26f | 532 | pressed_2 = false; |
SjorsdeBruin | 26:088e397ec26f | 533 | n = 0; |
Hendrikvg | 24:a9ec9b836fd9 | 534 | mvc(); |
Hendrikvg | 24:a9ec9b836fd9 | 535 | } |
Hendrikvg | 24:a9ec9b836fd9 | 536 | break; |
Hendrikvg | 24:a9ec9b836fd9 | 537 | default: |
Hendrikvg | 24:a9ec9b836fd9 | 538 | pc.printf("Unknown or unimplemented state reached!\n\r"); |
Hendrikvg | 21:394a7a1deb73 | 539 | } |
Hendrikvg | 21:394a7a1deb73 | 540 | } |
Hendrikvg | 21:394a7a1deb73 | 541 | |
Hendrikvg | 21:394a7a1deb73 | 542 | void while_vertical_movement() |
Hendrikvg | 21:394a7a1deb73 | 543 | { |
SjorsdeBruin | 29:5a846abba59e | 544 | /* |
Hendrikvg | 21:394a7a1deb73 | 545 | // Do vertical movement stuff |
SjorsdeBruin | 28:8c90a46b613e | 546 | if (emg_trigger(treshold_bl,EmgCalibration(emgFiltered_bl, mvc_value_bl, rest_value_bl))) { |
SjorsdeBruin | 28:8c90a46b613e | 547 | EMGy_velocity = -0.02f; |
SjorsdeBruin | 28:8c90a46b613e | 548 | } |
SjorsdeBruin | 28:8c90a46b613e | 549 | else if (emg_trigger(treshold_br,EmgCalibration(emgFiltered_br, mvc_value_br, rest_value_br))) { |
SjorsdeBruin | 28:8c90a46b613e | 550 | EMGy_velocity = 0.02f; |
SjorsdeBruin | 28:8c90a46b613e | 551 | } |
SjorsdeBruin | 28:8c90a46b613e | 552 | else { |
SjorsdeBruin | 28:8c90a46b613e | 553 | EMGy_velocity = 0.0f; |
SjorsdeBruin | 28:8c90a46b613e | 554 | } |
SjorsdeBruin | 28:8c90a46b613e | 555 | if ((pressed_1) || (pressed_2) || (emg_switch(treshold_leg,EmgCalibration(emgFiltered_leg, mvc_value_leg, rest_value_leg)))) { // EMG gebaseerde threshold aanmaken |
Hendrikvg | 21:394a7a1deb73 | 556 | CurrentState = horizontal_movement; |
Hendrikvg | 21:394a7a1deb73 | 557 | StateChanged = true; |
SjorsdeBruin | 29:5a846abba59e | 558 | }*/ |
SjorsdeBruin | 30:a16519224d58 | 559 | |
SjorsdeBruin | 30:a16519224d58 | 560 | while (beweging =='s') { |
SjorsdeBruin | 29:5a846abba59e | 561 | EMGy_velocity = -0.02f; |
SjorsdeBruin | 30:a16519224d58 | 562 | pc.printf("beweging %c \n\r", beweging); |
SjorsdeBruin | 30:a16519224d58 | 563 | pc.printf("beweging %f \n\r", EMGy_velocity); |
SjorsdeBruin | 30:a16519224d58 | 564 | |
Hendrikvg | 21:394a7a1deb73 | 565 | } |
SjorsdeBruin | 30:a16519224d58 | 566 | while (beweging == 'w') { |
SjorsdeBruin | 29:5a846abba59e | 567 | EMGy_velocity = 0.02f; |
SjorsdeBruin | 30:a16519224d58 | 568 | pc.printf("beweging %c \n\r", beweging); |
SjorsdeBruin | 30:a16519224d58 | 569 | pc.printf("beweging %f \n\r", EMGy_velocity); |
SjorsdeBruin | 29:5a846abba59e | 570 | } |
SjorsdeBruin | 30:a16519224d58 | 571 | while(beweging != 's' || beweging !='w'){ |
SjorsdeBruin | 29:5a846abba59e | 572 | EMGy_velocity = 0.0f; |
SjorsdeBruin | 29:5a846abba59e | 573 | } |
SjorsdeBruin | 29:5a846abba59e | 574 | if ((pressed_1) || (pressed_2) /*|| (emg_switch(treshold_leg,EmgCalibration(emgFiltered_leg, mvc_value_leg, rest_value_leg)))*/) { // EMG gebaseerde threshold aanmaken |
SjorsdeBruin | 29:5a846abba59e | 575 | CurrentState = horizontal_movement; |
SjorsdeBruin | 29:5a846abba59e | 576 | StateChanged = true; |
SjorsdeBruin | 29:5a846abba59e | 577 | } |
Hendrikvg | 21:394a7a1deb73 | 578 | } |
Hendrikvg | 17:cacf9e75eda7 | 579 | |
Hendrikvg | 21:394a7a1deb73 | 580 | void while_horizontal_movement() |
Hendrikvg | 21:394a7a1deb73 | 581 | { |
Hendrikvg | 21:394a7a1deb73 | 582 | // Do horizontal movement stuff |
SjorsdeBruin | 29:5a846abba59e | 583 | /* |
SjorsdeBruin | 28:8c90a46b613e | 584 | if (emg_trigger(treshold_bl,EmgCalibration(emgFiltered_bl, mvc_value_bl, rest_value_bl))) { |
SjorsdeBruin | 28:8c90a46b613e | 585 | EMGx_velocity = -0.02f; |
SjorsdeBruin | 28:8c90a46b613e | 586 | } |
SjorsdeBruin | 28:8c90a46b613e | 587 | else if (emg_trigger(treshold_br,EmgCalibration(emgFiltered_br, mvc_value_br, rest_value_br))) { |
SjorsdeBruin | 28:8c90a46b613e | 588 | EMGx_velocity = 0.02f; |
SjorsdeBruin | 28:8c90a46b613e | 589 | } |
SjorsdeBruin | 28:8c90a46b613e | 590 | else { |
SjorsdeBruin | 28:8c90a46b613e | 591 | EMGx_velocity = 0.0f; |
SjorsdeBruin | 28:8c90a46b613e | 592 | } |
SjorsdeBruin | 28:8c90a46b613e | 593 | if ((pressed_1) || (pressed_2) || (emg_switch(treshold_leg,EmgCalibration(emgFiltered_leg, mvc_value_leg, rest_value_leg)))) { // EMG gebaseerde threshold aanmaken |
Hendrikvg | 21:394a7a1deb73 | 594 | CurrentState = vertical_movement; |
Hendrikvg | 21:394a7a1deb73 | 595 | StateChanged = true; |
Hendrikvg | 21:394a7a1deb73 | 596 | } |
SjorsdeBruin | 29:5a846abba59e | 597 | */ |
SjorsdeBruin | 29:5a846abba59e | 598 | if (beweging == 'a') { |
SjorsdeBruin | 29:5a846abba59e | 599 | EMGx_velocity = -0.02f; |
SjorsdeBruin | 29:5a846abba59e | 600 | pc.printf(" you pressed %c \n\r" , beweging); |
SjorsdeBruin | 29:5a846abba59e | 601 | } |
SjorsdeBruin | 30:a16519224d58 | 602 | if (beweging == 'd') { |
SjorsdeBruin | 29:5a846abba59e | 603 | EMGx_velocity = 0.02f; |
SjorsdeBruin | 30:a16519224d58 | 604 | pc.printf(" you pressed %c \n\r" , beweging); |
SjorsdeBruin | 29:5a846abba59e | 605 | } |
SjorsdeBruin | 29:5a846abba59e | 606 | else { |
SjorsdeBruin | 29:5a846abba59e | 607 | EMGx_velocity = 0.0f; |
SjorsdeBruin | 29:5a846abba59e | 608 | } |
SjorsdeBruin | 29:5a846abba59e | 609 | if ((pressed_1) || (pressed_2) /*|| (emg_switch(treshold_leg,EmgCalibration(emgFiltered_leg, mvc_value_leg, rest_value_leg)))*/) { // EMG gebaseerde threshold aanmaken |
SjorsdeBruin | 29:5a846abba59e | 610 | CurrentState = vertical_movement; |
SjorsdeBruin | 29:5a846abba59e | 611 | StateChanged = true; |
SjorsdeBruin | 29:5a846abba59e | 612 | } |
SjorsdeBruin | 29:5a846abba59e | 613 | } |
SjorsdeBruin | 29:5a846abba59e | 614 | |
Hendrikvg | 21:394a7a1deb73 | 615 | |
Hendrikvg | 21:394a7a1deb73 | 616 | void StateTransition() |
Hendrikvg | 21:394a7a1deb73 | 617 | { |
Hendrikvg | 24:a9ec9b836fd9 | 618 | if (StateChanged) { |
SjorsdeBruin | 26:088e397ec26f | 619 | pressed_1 = false; |
SjorsdeBruin | 26:088e397ec26f | 620 | pressed_2 = false; |
Hendrikvg | 24:a9ec9b836fd9 | 621 | if (CurrentState == start) { |
Hendrikvg | 21:394a7a1deb73 | 622 | pc.printf("Initiating start.\n\r"); |
Hendrikvg | 21:394a7a1deb73 | 623 | } |
Hendrikvg | 24:a9ec9b836fd9 | 624 | if (CurrentState == motor_calibration) { |
Hendrikvg | 21:394a7a1deb73 | 625 | pc.printf("Initiating motor_calibration.\n\r"); |
Hendrikvg | 21:394a7a1deb73 | 626 | } |
Hendrikvg | 24:a9ec9b836fd9 | 627 | if (CurrentState == demo_mode) { |
Hendrikvg | 21:394a7a1deb73 | 628 | pc.printf("Initiating demo_mode.\n\r"); |
Hendrikvg | 21:394a7a1deb73 | 629 | } |
Hendrikvg | 24:a9ec9b836fd9 | 630 | if (CurrentState == emg_calibration) { |
Hendrikvg | 21:394a7a1deb73 | 631 | pc.printf("Initiating emg_calibration.\n\r"); |
Hendrikvg | 21:394a7a1deb73 | 632 | } |
Hendrikvg | 24:a9ec9b836fd9 | 633 | if (CurrentState == vertical_movement) { |
Hendrikvg | 21:394a7a1deb73 | 634 | pc.printf("Initiating vertical_movement.\n\r"); |
Hendrikvg | 21:394a7a1deb73 | 635 | } |
Hendrikvg | 24:a9ec9b836fd9 | 636 | if (CurrentState == horizontal_movement) { |
Hendrikvg | 21:394a7a1deb73 | 637 | pc.printf("Initiating horizontal_movement.\n\r"); |
Hendrikvg | 21:394a7a1deb73 | 638 | } |
Hendrikvg | 21:394a7a1deb73 | 639 | StateChanged = false; |
Hendrikvg | 21:394a7a1deb73 | 640 | } |
Hendrikvg | 21:394a7a1deb73 | 641 | } |
Hendrikvg | 21:394a7a1deb73 | 642 | |
Hendrikvg | 21:394a7a1deb73 | 643 | void StateMachine() |
Hendrikvg | 21:394a7a1deb73 | 644 | { |
Hendrikvg | 24:a9ec9b836fd9 | 645 | switch(CurrentState) { |
Hendrikvg | 21:394a7a1deb73 | 646 | case start: |
Hendrikvg | 21:394a7a1deb73 | 647 | StateTransition(); |
Hendrikvg | 21:394a7a1deb73 | 648 | while_start(); |
Hendrikvg | 21:394a7a1deb73 | 649 | break; |
Hendrikvg | 21:394a7a1deb73 | 650 | case motor_calibration: |
Hendrikvg | 21:394a7a1deb73 | 651 | StateTransition(); |
Hendrikvg | 21:394a7a1deb73 | 652 | while_motor_calibration(); |
Hendrikvg | 21:394a7a1deb73 | 653 | break; |
Hendrikvg | 21:394a7a1deb73 | 654 | case demo_mode: |
Hendrikvg | 21:394a7a1deb73 | 655 | StateTransition(); |
Hendrikvg | 21:394a7a1deb73 | 656 | while_demo_mode(); |
Hendrikvg | 21:394a7a1deb73 | 657 | break; |
Hendrikvg | 21:394a7a1deb73 | 658 | case emg_calibration: |
Hendrikvg | 21:394a7a1deb73 | 659 | StateTransition(); |
Hendrikvg | 21:394a7a1deb73 | 660 | while_emg_calibration(); |
Hendrikvg | 21:394a7a1deb73 | 661 | break; |
Hendrikvg | 21:394a7a1deb73 | 662 | case vertical_movement: |
Hendrikvg | 21:394a7a1deb73 | 663 | StateTransition(); |
Hendrikvg | 21:394a7a1deb73 | 664 | while_vertical_movement(); |
Hendrikvg | 21:394a7a1deb73 | 665 | break; |
Hendrikvg | 21:394a7a1deb73 | 666 | case horizontal_movement: |
Hendrikvg | 21:394a7a1deb73 | 667 | StateTransition(); |
Hendrikvg | 21:394a7a1deb73 | 668 | while_horizontal_movement(); |
Hendrikvg | 21:394a7a1deb73 | 669 | break; |
Hendrikvg | 21:394a7a1deb73 | 670 | default: |
Hendrikvg | 21:394a7a1deb73 | 671 | pc.printf("Unknown or unimplemented state reached!\n\r"); |
Hendrikvg | 21:394a7a1deb73 | 672 | } |
Hendrikvg | 21:394a7a1deb73 | 673 | } |
Hendrikvg | 21:394a7a1deb73 | 674 | |
Hendrikvg | 15:80b3ac2b8448 | 675 | // main |
Hendrikvg | 24:a9ec9b836fd9 | 676 | int main() |
Hendrikvg | 24:a9ec9b836fd9 | 677 | { |
RobertoO | 0:67c50348f842 | 678 | pc.baud(115200); |
Hendrikvg | 17:cacf9e75eda7 | 679 | pc.printf("Hello World!\n\r"); |
SjorsdeBruin | 26:088e397ec26f | 680 | ledoff(); |
Hendrikvg | 21:394a7a1deb73 | 681 | button_1.fall(go_next_1); |
Hendrikvg | 21:394a7a1deb73 | 682 | button_2.fall(go_next_2); |
SjorsdeBruin | 26:088e397ec26f | 683 | //sinus_time.start(); |
SjorsdeBruin | 29:5a846abba59e | 684 | PWM_motor_1.period_ms(10); |
SjorsdeBruin | 29:5a846abba59e | 685 | PWM_motor_2.period_ms(10); |
SjorsdeBruin | 29:5a846abba59e | 686 | motor_control.attach(&MotorControl, Ts); |
SjorsdeBruin | 27:d37b3a0e0f2b | 687 | write_scope.attach(&WriteScope, 0.01); |
SjorsdeBruin | 30:a16519224d58 | 688 | |
SjorsdeBruin | 30:a16519224d58 | 689 | void CalculateDirectionMotor(); |
SjorsdeBruin | 30:a16519224d58 | 690 | void MotorControl(); |
SjorsdeBruin | 30:a16519224d58 | 691 | |
SjorsdeBruin | 30:a16519224d58 | 692 | |
SjorsdeBruin | 29:5a846abba59e | 693 | TickerStateMachine.attach(StateMachine,1.00f); |
SjorsdeBruin | 29:5a846abba59e | 694 | beweging = pc.getc(); |
Hendrikvg | 21:394a7a1deb73 | 695 | while(true) { |
Hendrikvg | 21:394a7a1deb73 | 696 | StateMachine(); |
Hendrikvg | 21:394a7a1deb73 | 697 | } |
SjorsdeBruin | 26:088e397ec26f | 698 | } |