Sem Bertelink / Mbed 2 deprecated MOD09_Biorobotics_Groep_14lol

lololololol

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

main.cpp@26:088e397ec26f, 2019-10-29 (annotated)

Committer:
SjorsdeBruin
Date:
Tue Oct 29 11:13:15 2019 +0000
Revision:
26:088e397ec26f
Parent:
24:a9ec9b836fd9
Child:
27:d37b3a0e0f2b
aids emg werkt niet, reden onbekend

Who changed what in which revision?

UserRevisionLine numberNew 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
Hendrikvg 21:394a7a1deb73 33 Ticker TickerStateMachine;
Hendrikvg 23:78898ddfb103 34 Ticker motor_control;
Hendrikvg 23:78898ddfb103 35 Ticker write_scope;
Hendrikvg 21:394a7a1deb73 36 Timer sinus_time;
Hendrikvg 24:a9ec9b836fd9 37 Timeout rest_timeout;
Hendrikvg 24:a9ec9b836fd9 38 Timeout mvc_timeout;
SjorsdeBruin 26:088e397ec26f 39 Timeout led_timeout;
Hendrikvg 21:394a7a1deb73 40 enum states {start, motor_calibration, demo_mode, emg_calibration, vertical_movement, horizontal_movement};
Hendrikvg 21:394a7a1deb73 41 states CurrentState = start;
Hendrikvg 21:394a7a1deb73 42 bool StateChanged = true;
Hendrikvg 24:a9ec9b836fd9 43 enum substates {rest_biceps_left, mvc_biceps_left, rest_biceps_right, mvc_biceps_right, rest_biceps_leg, mvc_biceps_leg};
Hendrikvg 24:a9ec9b836fd9 44 substates CurrentSubstate = rest_biceps_left;
Hendrikvg 24:a9ec9b836fd9 45 bool SubstateChanged = true;
SjorsdeBruin 26:088e397ec26f 46 volatile bool pressed_1 = false;
SjorsdeBruin 26:088e397ec26f 47 volatile bool pressed_2 = false;
Hendrikvg 21:394a7a1deb73 48 HIDScope scope(3);
SjorsdeBruin 26:088e397ec26f 49
SjorsdeBruin 26:088e397ec26f 50 volatile float theta_1;
Hendrikvg 23:78898ddfb103 51 //volatile float theta_error_1;
Hendrikvg 22:6cc93216b323 52 volatile float theta_reference_1;
SjorsdeBruin 26:088e397ec26f 53 volatile float theta_2;
Hendrikvg 23:78898ddfb103 54 //volatile float theta_error_2;
Hendrikvg 23:78898ddfb103 55 volatile float theta_reference_2;
SjorsdeBruin 26:088e397ec26f 56 float Ts = 0.01;
Hendrikvg 23:78898ddfb103 57 float Kp;
Hendrikvg 23:78898ddfb103 58 float Ki;
Hendrikvg 23:78898ddfb103 59 float Kd;
Hendrikvg 20:ac1b4ffa3323 60
SjorsdeBruin 26:088e397ec26f 61
SjorsdeBruin 26:088e397ec26f 62 BiQuad Lowpass ( 1.55148e-04, 3.10297e-04, 1.55148e-04, -1.96446e+00, 9.65081e-01 );
SjorsdeBruin 26:088e397ec26f 63 BiQuad Highpass ( 9.40809e-01, -9.40809e-01, 0.00000e+00, -8.81619e-01, 0.00000e+00);
SjorsdeBruin 26:088e397ec26f 64 BiQuad notch (9.98432e-01, -1.89913e+00, 9.98432e-01, -1.89913e+00, 9.96886e-01);
SjorsdeBruin 26:088e397ec26f 65
Hendrikvg 24:a9ec9b836fd9 66 int n = 0;
Hendrikvg 24:a9ec9b836fd9 67
SjorsdeBruin 26:088e397ec26f 68 float emgFiltered_bl;
SjorsdeBruin 26:088e397ec26f 69 float emgFiltered_br;
SjorsdeBruin 26:088e397ec26f 70 float emgFiltered_leg;
SjorsdeBruin 26:088e397ec26f 71 float emg;
SjorsdeBruin 26:088e397ec26f 72 float xmvc_value = 1e-11;
Hendrikvg 24:a9ec9b836fd9 73
Hendrikvg 24:a9ec9b836fd9 74 float sum = 0;
SjorsdeBruin 26:088e397ec26f 75 float xrest_value;
Hendrikvg 24:a9ec9b836fd9 76 float rest_value_bl;
Hendrikvg 24:a9ec9b836fd9 77 float rest_value_br;
Hendrikvg 24:a9ec9b836fd9 78 float rest_value_leg;
Hendrikvg 24:a9ec9b836fd9 79
Hendrikvg 24:a9ec9b836fd9 80 float mvc_value_bl;
Hendrikvg 24:a9ec9b836fd9 81 float mvc_value_br;
Hendrikvg 24:a9ec9b836fd9 82 float mvc_value_leg;
Hendrikvg 24:a9ec9b836fd9 83
Hendrikvg 21:394a7a1deb73 84 // functies
SjorsdeBruin 26:088e397ec26f 85 void ledred()
SjorsdeBruin 26:088e397ec26f 86 {
SjorsdeBruin 26:088e397ec26f 87 led_red = 0;
SjorsdeBruin 26:088e397ec26f 88 led_green = 1;
SjorsdeBruin 26:088e397ec26f 89 led_blue = 1;
SjorsdeBruin 26:088e397ec26f 90 }
SjorsdeBruin 26:088e397ec26f 91 void ledgreen()
SjorsdeBruin 26:088e397ec26f 92 {
SjorsdeBruin 26:088e397ec26f 93 led_green=0;
SjorsdeBruin 26:088e397ec26f 94 led_blue=1;
SjorsdeBruin 26:088e397ec26f 95 led_red=1;
SjorsdeBruin 26:088e397ec26f 96 }
SjorsdeBruin 26:088e397ec26f 97 void ledblue()
SjorsdeBruin 26:088e397ec26f 98 {
SjorsdeBruin 26:088e397ec26f 99 led_green=1;
SjorsdeBruin 26:088e397ec26f 100 led_blue=0;
SjorsdeBruin 26:088e397ec26f 101 led_red=1;
SjorsdeBruin 26:088e397ec26f 102 }
SjorsdeBruin 26:088e397ec26f 103 void ledyellow()
SjorsdeBruin 26:088e397ec26f 104 {
SjorsdeBruin 26:088e397ec26f 105 led_green=0;
SjorsdeBruin 26:088e397ec26f 106 led_blue=1;
SjorsdeBruin 26:088e397ec26f 107 led_red=0;
SjorsdeBruin 26:088e397ec26f 108 }
SjorsdeBruin 26:088e397ec26f 109 void ledmagenta()
SjorsdeBruin 26:088e397ec26f 110 {
SjorsdeBruin 26:088e397ec26f 111 led_green=1;
SjorsdeBruin 26:088e397ec26f 112 led_blue=0;
SjorsdeBruin 26:088e397ec26f 113 led_red=0;
SjorsdeBruin 26:088e397ec26f 114 }
SjorsdeBruin 26:088e397ec26f 115 void ledcyan()
SjorsdeBruin 26:088e397ec26f 116 {
SjorsdeBruin 26:088e397ec26f 117 led_green=0;
SjorsdeBruin 26:088e397ec26f 118 led_blue=0;
SjorsdeBruin 26:088e397ec26f 119 led_red=1;
SjorsdeBruin 26:088e397ec26f 120 }
SjorsdeBruin 26:088e397ec26f 121 void ledwhite()
SjorsdeBruin 26:088e397ec26f 122 {
SjorsdeBruin 26:088e397ec26f 123 led_green=0;
SjorsdeBruin 26:088e397ec26f 124 led_blue=0;
SjorsdeBruin 26:088e397ec26f 125 led_red=0;
SjorsdeBruin 26:088e397ec26f 126 }
SjorsdeBruin 26:088e397ec26f 127 void ledoff()
SjorsdeBruin 26:088e397ec26f 128 {
SjorsdeBruin 26:088e397ec26f 129 led_green=1;
SjorsdeBruin 26:088e397ec26f 130 led_blue=1;
SjorsdeBruin 26:088e397ec26f 131 led_red=1;
SjorsdeBruin 26:088e397ec26f 132 }
SjorsdeBruin 26:088e397ec26f 133
SjorsdeBruin 26:088e397ec26f 134
Hendrikvg 22:6cc93216b323 135 float CalculateError(float theta_reference,float theta)
Hendrikvg 21:394a7a1deb73 136 {
Hendrikvg 22:6cc93216b323 137 float theta_error = theta_reference-theta;
Hendrikvg 21:394a7a1deb73 138 return theta_error;
Hendrikvg 21:394a7a1deb73 139 }
Hendrikvg 20:ac1b4ffa3323 140
Hendrikvg 23:78898ddfb103 141 float Controller(float theta_error, bool motor)
Hendrikvg 21:394a7a1deb73 142 {
Hendrikvg 24:a9ec9b836fd9 143 if (motor == false) {
Hendrikvg 23:78898ddfb103 144 float K = 1;
Hendrikvg 23:78898ddfb103 145 float ti = 0.1;
Hendrikvg 23:78898ddfb103 146 float td = 10;
Hendrikvg 23:78898ddfb103 147 Kp = K*(1+td/ti);
Hendrikvg 23:78898ddfb103 148 Ki = K/ti;
Hendrikvg 23:78898ddfb103 149 Kd = K*td;
Hendrikvg 24:a9ec9b836fd9 150 } else {
Hendrikvg 23:78898ddfb103 151 float K = 1;
Hendrikvg 23:78898ddfb103 152 float ti = 0.1;
Hendrikvg 23:78898ddfb103 153 float td = 10;
Hendrikvg 23:78898ddfb103 154 Kp = K*(1+td/ti);
Hendrikvg 23:78898ddfb103 155 Ki = K/ti;
Hendrikvg 23:78898ddfb103 156 Kd = K*td;
Hendrikvg 23:78898ddfb103 157 }
Hendrikvg 21:394a7a1deb73 158 static float error_integral = 0;
Hendrikvg 21:394a7a1deb73 159 static float error_prev = 0;
Hendrikvg 21:394a7a1deb73 160 static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
Hendrikvg 24:a9ec9b836fd9 161
Hendrikvg 20:ac1b4ffa3323 162 // Proportional part:
Hendrikvg 20:ac1b4ffa3323 163 float torque_p = Kp * theta_error;
Hendrikvg 24:a9ec9b836fd9 164
Hendrikvg 20:ac1b4ffa3323 165 // Integral part:
Hendrikvg 21:394a7a1deb73 166 error_integral = error_integral + theta_error * Ts;
Hendrikvg 21:394a7a1deb73 167 float torque_i = Ki * error_integral;
Hendrikvg 24:a9ec9b836fd9 168
Hendrikvg 20:ac1b4ffa3323 169 // Derivative part:
Hendrikvg 21:394a7a1deb73 170 float error_derivative = (theta_error - error_prev)/Ts;
Hendrikvg 21:394a7a1deb73 171 float filtered_error_derivative = LowPassFilter.step(error_derivative);
Hendrikvg 21:394a7a1deb73 172 float torque_d = Kd * filtered_error_derivative;
Hendrikvg 21:394a7a1deb73 173 error_prev = theta_error;
Hendrikvg 24:a9ec9b836fd9 174
Hendrikvg 20:ac1b4ffa3323 175 // Sum all parts and return it
Hendrikvg 21:394a7a1deb73 176 float torque = torque_p + torque_i + torque_d;
Hendrikvg 21:394a7a1deb73 177 return torque;
Hendrikvg 21:394a7a1deb73 178 }
Hendrikvg 16:40183eeadb6d 179
Hendrikvg 24:a9ec9b836fd9 180 void CalculateDirectionMotor()
Hendrikvg 21:394a7a1deb73 181 {
Hendrikvg 23:78898ddfb103 182 direction_motor_1 = Controller(CalculateError(theta_reference_1,theta_1),0) <= 0.0f;
Hendrikvg 23:78898ddfb103 183 direction_motor_2 = Controller(CalculateError(theta_reference_2,theta_2),1) <= 0.0f;
Hendrikvg 21:394a7a1deb73 184 }
Hendrikvg 20:ac1b4ffa3323 185
Hendrikvg 23:78898ddfb103 186 void ReadEncoder()
Hendrikvg 23:78898ddfb103 187 {
Hendrikvg 23:78898ddfb103 188 theta_1 = ((360.0f/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.
Hendrikvg 23:78898ddfb103 189 theta_2 = ((360.0f/64.0f)*(float)encoder_2.getPulses())/131.25f;
Hendrikvg 23:78898ddfb103 190 }
Hendrikvg 16:40183eeadb6d 191
Hendrikvg 24:a9ec9b836fd9 192 void MotorControl()
Hendrikvg 23:78898ddfb103 193 {
Hendrikvg 21:394a7a1deb73 194 ReadEncoder();
SjorsdeBruin 26:088e397ec26f 195 theta_reference_1 = 360.0f*sin(0.1f*sinus_time.read()*2.0f*3.14f); // voor test, moet weg in eindscript
Hendrikvg 22:6cc93216b323 196 CalculateDirectionMotor();
Hendrikvg 23:78898ddfb103 197 PWM_motor_1.write(fabs(Controller(CalculateError(theta_reference_1,theta_1),0)/360.0f));
Hendrikvg 23:78898ddfb103 198 PWM_motor_2.write(fabs(Controller(CalculateError(theta_reference_2,theta_2),1)/360.0f));
Hendrikvg 21:394a7a1deb73 199 }
Hendrikvg 21:394a7a1deb73 200
Hendrikvg 21:394a7a1deb73 201 void go_next_1()
Hendrikvg 21:394a7a1deb73 202 {
Hendrikvg 24:a9ec9b836fd9 203 pressed_1 = !pressed_1;
Hendrikvg 21:394a7a1deb73 204 }
Hendrikvg 21:394a7a1deb73 205
Hendrikvg 21:394a7a1deb73 206 void go_next_2()
Hendrikvg 21:394a7a1deb73 207 {
Hendrikvg 24:a9ec9b836fd9 208 pressed_2 = !pressed_2;
Hendrikvg 24:a9ec9b836fd9 209 }
Hendrikvg 24:a9ec9b836fd9 210
SjorsdeBruin 26:088e397ec26f 211 float EmgCalibration(float emgFiltered, float mvc_value, float rest_value)
Hendrikvg 24:a9ec9b836fd9 212 {
Hendrikvg 24:a9ec9b836fd9 213 float emgCalibrated;
Hendrikvg 24:a9ec9b836fd9 214 if (emgFiltered <= rest_value) {
Hendrikvg 24:a9ec9b836fd9 215 emgCalibrated = 0;
Hendrikvg 24:a9ec9b836fd9 216 }
Hendrikvg 24:a9ec9b836fd9 217 if (emgFiltered >= mvc_value) {
Hendrikvg 24:a9ec9b836fd9 218 emgCalibrated = 1;
Hendrikvg 24:a9ec9b836fd9 219 } else {
Hendrikvg 24:a9ec9b836fd9 220 emgCalibrated = (emgFiltered-rest_value)/(mvc_value-rest_value);
Hendrikvg 24:a9ec9b836fd9 221 }
Hendrikvg 24:a9ec9b836fd9 222 return emgCalibrated;
Hendrikvg 24:a9ec9b836fd9 223 }
Hendrikvg 24:a9ec9b836fd9 224
Hendrikvg 24:a9ec9b836fd9 225 void emgsample()
Hendrikvg 24:a9ec9b836fd9 226 {
SjorsdeBruin 26:088e397ec26f 227 emgFiltered_bl = Highpass.step(emg_bl.read());
SjorsdeBruin 26:088e397ec26f 228 emgFiltered_bl = notch.step(emgFiltered_bl);
Hendrikvg 24:a9ec9b836fd9 229 emgFiltered_bl = fabs(emgFiltered_bl);
SjorsdeBruin 26:088e397ec26f 230 emgFiltered_bl = Lowpass.step(emgFiltered_bl);
Hendrikvg 24:a9ec9b836fd9 231
SjorsdeBruin 26:088e397ec26f 232 emgFiltered_br = Highpass.step(emg_br.read());
SjorsdeBruin 26:088e397ec26f 233 emgFiltered_br = notch.step(emgFiltered_br);
Hendrikvg 24:a9ec9b836fd9 234 emgFiltered_br = fabs(emgFiltered_br);
SjorsdeBruin 26:088e397ec26f 235 emgFiltered_br = Lowpass.step(emgFiltered_br);
Hendrikvg 24:a9ec9b836fd9 236
SjorsdeBruin 26:088e397ec26f 237 emgFiltered_leg = Highpass.step(emg_leg.read());
SjorsdeBruin 26:088e397ec26f 238 emgFiltered_leg = notch.step(emgFiltered_leg);
Hendrikvg 24:a9ec9b836fd9 239 emgFiltered_leg = fabs(emgFiltered_leg);
SjorsdeBruin 26:088e397ec26f 240 emgFiltered_leg = Lowpass.step(emgFiltered_leg);
Hendrikvg 24:a9ec9b836fd9 241 }
Hendrikvg 24:a9ec9b836fd9 242
Hendrikvg 24:a9ec9b836fd9 243 void rest()
Hendrikvg 24:a9ec9b836fd9 244 {
SjorsdeBruin 26:088e397ec26f 245 if (CurrentSubstate == rest_biceps_left) {
Hendrikvg 24:a9ec9b836fd9 246 emg = emgFiltered_bl;
SjorsdeBruin 26:088e397ec26f 247 //pc.printf("emg: %f \n\r",emgFiltered_bl);
Hendrikvg 24:a9ec9b836fd9 248 }
SjorsdeBruin 26:088e397ec26f 249 if (CurrentSubstate == rest_biceps_right) {
Hendrikvg 24:a9ec9b836fd9 250 emg = emgFiltered_br;
Hendrikvg 24:a9ec9b836fd9 251 }
SjorsdeBruin 26:088e397ec26f 252 if (CurrentSubstate == rest_biceps_leg) {
Hendrikvg 24:a9ec9b836fd9 253 emg = emgFiltered_leg;
Hendrikvg 24:a9ec9b836fd9 254 }
SjorsdeBruin 26:088e397ec26f 255 if (n < 500) {
SjorsdeBruin 26:088e397ec26f 256 ledred();
Hendrikvg 24:a9ec9b836fd9 257 sum = sum + emg;
SjorsdeBruin 26:088e397ec26f 258 //pc.printf("sum: %f \n\r",sum);
Hendrikvg 24:a9ec9b836fd9 259 n++;
SjorsdeBruin 26:088e397ec26f 260 rest_timeout.attach(rest,0.001f);
Hendrikvg 24:a9ec9b836fd9 261 }
SjorsdeBruin 26:088e397ec26f 262 if (n == 500) {
Hendrikvg 24:a9ec9b836fd9 263 sum = sum + emg;
SjorsdeBruin 26:088e397ec26f 264 //pc.printf("sum: %f \n\r",sum);
Hendrikvg 24:a9ec9b836fd9 265 n++;
SjorsdeBruin 26:088e397ec26f 266 xrest_value = float (sum/n);
SjorsdeBruin 26:088e397ec26f 267 if (CurrentSubstate == rest_biceps_left) {
SjorsdeBruin 26:088e397ec26f 268 rest_value_bl = xrest_value;
SjorsdeBruin 26:088e397ec26f 269 pc.printf("rest_value_bl %f \n\r", rest_value_bl);
Hendrikvg 24:a9ec9b836fd9 270 CurrentSubstate = mvc_biceps_left;
Hendrikvg 24:a9ec9b836fd9 271 SubstateChanged = true;
SjorsdeBruin 26:088e397ec26f 272 ledblue();
SjorsdeBruin 26:088e397ec26f 273
Hendrikvg 24:a9ec9b836fd9 274 }
SjorsdeBruin 26:088e397ec26f 275 if (CurrentSubstate == rest_biceps_right) {
SjorsdeBruin 26:088e397ec26f 276 rest_value_br = xrest_value;
SjorsdeBruin 26:088e397ec26f 277 pc.printf("rest_value_br %f \n\r", rest_value_br);
Hendrikvg 24:a9ec9b836fd9 278 CurrentSubstate = mvc_biceps_right;
Hendrikvg 24:a9ec9b836fd9 279 SubstateChanged = true;
SjorsdeBruin 26:088e397ec26f 280 ledmagenta();
Hendrikvg 24:a9ec9b836fd9 281 }
SjorsdeBruin 26:088e397ec26f 282 if (CurrentSubstate == rest_biceps_leg) {
SjorsdeBruin 26:088e397ec26f 283 rest_value_leg = xrest_value;
SjorsdeBruin 26:088e397ec26f 284 pc.printf("rest_value_leg %f \n\r", rest_value_leg);
SjorsdeBruin 26:088e397ec26f 285 pc.printf("rest_value_bl %f \n\r", rest_value_bl);
Hendrikvg 24:a9ec9b836fd9 286 CurrentSubstate = mvc_biceps_leg;
Hendrikvg 24:a9ec9b836fd9 287 SubstateChanged = true;
SjorsdeBruin 26:088e397ec26f 288 ledwhite();
Hendrikvg 24:a9ec9b836fd9 289 }
Hendrikvg 24:a9ec9b836fd9 290 }
Hendrikvg 24:a9ec9b836fd9 291 }
Hendrikvg 24:a9ec9b836fd9 292
Hendrikvg 24:a9ec9b836fd9 293 void mvc()
Hendrikvg 24:a9ec9b836fd9 294 {
SjorsdeBruin 26:088e397ec26f 295 if (CurrentSubstate == mvc_biceps_left) {
Hendrikvg 24:a9ec9b836fd9 296 emg = emgFiltered_bl;
Hendrikvg 24:a9ec9b836fd9 297 }
SjorsdeBruin 26:088e397ec26f 298 if (CurrentSubstate == mvc_biceps_right) {
Hendrikvg 24:a9ec9b836fd9 299 emg = emgFiltered_br;
Hendrikvg 24:a9ec9b836fd9 300 }
SjorsdeBruin 26:088e397ec26f 301 if (CurrentSubstate == mvc_biceps_leg) {
Hendrikvg 24:a9ec9b836fd9 302 emg = emgFiltered_leg;
Hendrikvg 24:a9ec9b836fd9 303 }
SjorsdeBruin 26:088e397ec26f 304 if (emg >= xmvc_value) {
Hendrikvg 24:a9ec9b836fd9 305 xmvc_value = emg;
SjorsdeBruin 26:088e397ec26f 306 }
Hendrikvg 24:a9ec9b836fd9 307 n++;
SjorsdeBruin 26:088e397ec26f 308 if (n < 1000) {
SjorsdeBruin 26:088e397ec26f 309 mvc_timeout.attach(mvc,0.001f);
SjorsdeBruin 26:088e397ec26f 310 ledred();
Hendrikvg 24:a9ec9b836fd9 311 }
SjorsdeBruin 26:088e397ec26f 312 if (n == 1000) {
SjorsdeBruin 26:088e397ec26f 313 if (CurrentSubstate == mvc_biceps_left) {
Hendrikvg 24:a9ec9b836fd9 314 mvc_value_bl = xmvc_value;
SjorsdeBruin 26:088e397ec26f 315 pc.printf("mvc_value_bl %f \n\r", mvc_value_bl);
Hendrikvg 24:a9ec9b836fd9 316 CurrentSubstate = rest_biceps_right;
Hendrikvg 24:a9ec9b836fd9 317 SubstateChanged = true;
SjorsdeBruin 26:088e397ec26f 318 ledyellow();
Hendrikvg 24:a9ec9b836fd9 319 }
SjorsdeBruin 26:088e397ec26f 320 if (CurrentSubstate == mvc_biceps_right) {
Hendrikvg 24:a9ec9b836fd9 321 mvc_value_br = xmvc_value;
SjorsdeBruin 26:088e397ec26f 322 pc.printf("mvc_value_br %f \n\r", mvc_value_br);
Hendrikvg 24:a9ec9b836fd9 323 CurrentSubstate = rest_biceps_leg;
Hendrikvg 24:a9ec9b836fd9 324 SubstateChanged = true;
SjorsdeBruin 26:088e397ec26f 325 ledcyan();
Hendrikvg 24:a9ec9b836fd9 326 }
SjorsdeBruin 26:088e397ec26f 327 if (CurrentSubstate == mvc_biceps_leg) {
Hendrikvg 24:a9ec9b836fd9 328 mvc_value_leg = xmvc_value;
SjorsdeBruin 26:088e397ec26f 329 pc.printf("mvc_value_leg %f \n\r", mvc_value_leg);
Hendrikvg 24:a9ec9b836fd9 330 CurrentState = vertical_movement;
Hendrikvg 24:a9ec9b836fd9 331 StateChanged = true;
SjorsdeBruin 26:088e397ec26f 332 ledoff();
Hendrikvg 24:a9ec9b836fd9 333 }
Hendrikvg 24:a9ec9b836fd9 334 xmvc_value = 1e-11;
Hendrikvg 24:a9ec9b836fd9 335 }
Hendrikvg 24:a9ec9b836fd9 336 }
Hendrikvg 24:a9ec9b836fd9 337
SjorsdeBruin 26:088e397ec26f 338 float emgCalibrated_bl;
SjorsdeBruin 26:088e397ec26f 339 float emgCalibrated_br;
SjorsdeBruin 26:088e397ec26f 340 float emgCalibrated_leg;
SjorsdeBruin 26:088e397ec26f 341
Hendrikvg 24:a9ec9b836fd9 342 void WriteScope()
Hendrikvg 24:a9ec9b836fd9 343 {
SjorsdeBruin 26:088e397ec26f 344 emgsample();
SjorsdeBruin 26:088e397ec26f 345 /*
SjorsdeBruin 26:088e397ec26f 346 if (emgFiltered_bl <= rest_value_bl) {
SjorsdeBruin 26:088e397ec26f 347 emgCalibrated_bl = 0;
SjorsdeBruin 26:088e397ec26f 348 }
SjorsdeBruin 26:088e397ec26f 349 if (emgFiltered_bl >= mvc_value_bl) {
SjorsdeBruin 26:088e397ec26f 350 emgCalibrated_bl = 1;
SjorsdeBruin 26:088e397ec26f 351 } else {
SjorsdeBruin 26:088e397ec26f 352 emgCalibrated_bl = (emgFiltered_bl - rest_value_bl) / (mvc_value_bl - rest_value_bl);
SjorsdeBruin 26:088e397ec26f 353 }
SjorsdeBruin 26:088e397ec26f 354 if (emgFiltered_br <= rest_value_br) {
SjorsdeBruin 26:088e397ec26f 355 emgCalibrated_br = 0;
SjorsdeBruin 26:088e397ec26f 356 }
SjorsdeBruin 26:088e397ec26f 357 if (emgFiltered_br >= mvc_value_br) {
SjorsdeBruin 26:088e397ec26f 358 emgCalibrated_br = 1;
SjorsdeBruin 26:088e397ec26f 359 } else {
SjorsdeBruin 26:088e397ec26f 360 emgCalibrated_br = (emgFiltered_br - rest_value_br) / (mvc_value_br - rest_value_br);
SjorsdeBruin 26:088e397ec26f 361 }
SjorsdeBruin 26:088e397ec26f 362 if (emgFiltered_leg <= rest_value_leg) {
SjorsdeBruin 26:088e397ec26f 363 emgCalibrated_leg = 0;
SjorsdeBruin 26:088e397ec26f 364 }
SjorsdeBruin 26:088e397ec26f 365 if (emgFiltered_leg >= mvc_value_leg) {
SjorsdeBruin 26:088e397ec26f 366 emgCalibrated_leg = 1;
SjorsdeBruin 26:088e397ec26f 367 } else {
SjorsdeBruin 26:088e397ec26f 368 emgCalibrated_leg = (emgFiltered_leg - rest_value_leg) / (mvc_value_leg - rest_value_leg);
SjorsdeBruin 26:088e397ec26f 369 }
SjorsdeBruin 26:088e397ec26f 370 */
SjorsdeBruin 26:088e397ec26f 371 /*
Hendrikvg 24:a9ec9b836fd9 372 scope.set(0, EmgCalibration(emgFiltered_bl, mvc_value_bl, rest_value_bl));
SjorsdeBruin 26:088e397ec26f 373 scope.set(1, EmgCalibration(emgFiltered_br, mvc_value_br, rest_value_br));
Hendrikvg 24:a9ec9b836fd9 374 scope.set(2, EmgCalibration(emgFiltered_leg, mvc_value_leg, rest_value_leg));
SjorsdeBruin 26:088e397ec26f 375 */
SjorsdeBruin 26:088e397ec26f 376 /*
SjorsdeBruin 26:088e397ec26f 377 scope.set(0, emgCalibrated_bl);
SjorsdeBruin 26:088e397ec26f 378 scope.set(1, emgCalibrated_br);
SjorsdeBruin 26:088e397ec26f 379 scope.set(2, emgCalibrated_leg);
SjorsdeBruin 26:088e397ec26f 380 */
SjorsdeBruin 26:088e397ec26f 381 scope.set(0, emg_bl.read());
SjorsdeBruin 26:088e397ec26f 382 scope.set(1, emgCalibrated_bl);
SjorsdeBruin 26:088e397ec26f 383 scope.set(2, EmgCalibration(emgFiltered_bl, mvc_value_bl, rest_value_bl));
Hendrikvg 24:a9ec9b836fd9 384 scope.send();
Hendrikvg 24:a9ec9b836fd9 385 }
Hendrikvg 24:a9ec9b836fd9 386
Hendrikvg 24:a9ec9b836fd9 387 void SubstateTransition()
Hendrikvg 24:a9ec9b836fd9 388 {
SjorsdeBruin 26:088e397ec26f 389 if (SubstateChanged == true) {
SjorsdeBruin 26:088e397ec26f 390 SubstateChanged = false;
SjorsdeBruin 26:088e397ec26f 391 pressed_1 = false;
SjorsdeBruin 26:088e397ec26f 392 pressed_2 = false;
SjorsdeBruin 26:088e397ec26f 393 if (CurrentSubstate == rest_biceps_left) {
SjorsdeBruin 26:088e397ec26f 394 ledgreen();
SjorsdeBruin 26:088e397ec26f 395 pc.printf("groen \n\r");
SjorsdeBruin 26:088e397ec26f 396 pc.printf("Initiating rest_biceps_left\n\r");
SjorsdeBruin 26:088e397ec26f 397 }
SjorsdeBruin 26:088e397ec26f 398 if (CurrentSubstate == mvc_biceps_left) {
SjorsdeBruin 26:088e397ec26f 399 //ledblue();
SjorsdeBruin 26:088e397ec26f 400 pc.printf("Initiating mvc_biceps_left\n\r");
SjorsdeBruin 26:088e397ec26f 401 }
SjorsdeBruin 26:088e397ec26f 402 if (CurrentSubstate == rest_biceps_right) {
SjorsdeBruin 26:088e397ec26f 403 //ledyellow();
SjorsdeBruin 26:088e397ec26f 404 pc.printf("Initiating rest_biceps_right\n\r");
SjorsdeBruin 26:088e397ec26f 405 }
SjorsdeBruin 26:088e397ec26f 406 if (CurrentSubstate == mvc_biceps_right) {
SjorsdeBruin 26:088e397ec26f 407 //ledmagenta();
SjorsdeBruin 26:088e397ec26f 408 pc.printf("Initiating mvc_biceps_right\n\r");
SjorsdeBruin 26:088e397ec26f 409 }
SjorsdeBruin 26:088e397ec26f 410 if (CurrentSubstate == rest_biceps_leg) {
SjorsdeBruin 26:088e397ec26f 411 //ledcyan();
SjorsdeBruin 26:088e397ec26f 412 pc.printf("Initiating rest_biceps_leg\n\r");
SjorsdeBruin 26:088e397ec26f 413 }
SjorsdeBruin 26:088e397ec26f 414 if (CurrentSubstate == mvc_biceps_leg) {
SjorsdeBruin 26:088e397ec26f 415 //ledwhite();
SjorsdeBruin 26:088e397ec26f 416 pc.printf("Initiating mvc_biceps_leg\n\r");
SjorsdeBruin 26:088e397ec26f 417 }
SjorsdeBruin 26:088e397ec26f 418 }
Hendrikvg 21:394a7a1deb73 419 }
Hendrikvg 21:394a7a1deb73 420
Hendrikvg 21:394a7a1deb73 421 void while_start()
Hendrikvg 21:394a7a1deb73 422 {
Hendrikvg 21:394a7a1deb73 423 // Do startup stuff
Hendrikvg 21:394a7a1deb73 424 CurrentState = motor_calibration;
Hendrikvg 21:394a7a1deb73 425 StateChanged = true;
Hendrikvg 21:394a7a1deb73 426 }
Hendrikvg 21:394a7a1deb73 427
Hendrikvg 21:394a7a1deb73 428 void while_motor_calibration()
Hendrikvg 21:394a7a1deb73 429 {
Hendrikvg 21:394a7a1deb73 430 // Do motor calibration stuff
Hendrikvg 24:a9ec9b836fd9 431 if (pressed_1) { // bool aanmaken voor demo (switch oid aanmaken)
Hendrikvg 21:394a7a1deb73 432 CurrentState = demo_mode;
Hendrikvg 21:394a7a1deb73 433 StateChanged = true;
Hendrikvg 21:394a7a1deb73 434 }
Hendrikvg 24:a9ec9b836fd9 435 if (pressed_2) { // bool aanmaken voor EMG (switch oid aanmaken)
Hendrikvg 21:394a7a1deb73 436 CurrentState = emg_calibration;
Hendrikvg 21:394a7a1deb73 437 StateChanged = true;
Hendrikvg 21:394a7a1deb73 438 }
Hendrikvg 21:394a7a1deb73 439 }
Hendrikvg 21:394a7a1deb73 440
Hendrikvg 21:394a7a1deb73 441 void while_demo_mode()
Hendrikvg 21:394a7a1deb73 442 {
Hendrikvg 21:394a7a1deb73 443 // Do demo mode stuff
SjorsdeBruin 26:088e397ec26f 444 if ((pressed_1) || (pressed_2)) {
Hendrikvg 21:394a7a1deb73 445 CurrentState = emg_calibration;
Hendrikvg 21:394a7a1deb73 446 StateChanged = true;
Hendrikvg 21:394a7a1deb73 447 }
Hendrikvg 21:394a7a1deb73 448 }
Hendrikvg 21:394a7a1deb73 449
Hendrikvg 21:394a7a1deb73 450 void while_emg_calibration()
Hendrikvg 21:394a7a1deb73 451 {
Hendrikvg 21:394a7a1deb73 452 // Do emg calibration stuff
Hendrikvg 24:a9ec9b836fd9 453 switch (CurrentSubstate) {
Hendrikvg 24:a9ec9b836fd9 454 case rest_biceps_left:
Hendrikvg 24:a9ec9b836fd9 455 SubstateTransition();
SjorsdeBruin 26:088e397ec26f 456 if ((pressed_1) || (pressed_2)) {
SjorsdeBruin 26:088e397ec26f 457 pressed_1 = false;
SjorsdeBruin 26:088e397ec26f 458 pressed_2 = false;
SjorsdeBruin 26:088e397ec26f 459 n = 0;
SjorsdeBruin 26:088e397ec26f 460 sum = 0;
Hendrikvg 24:a9ec9b836fd9 461 rest();
Hendrikvg 24:a9ec9b836fd9 462 }
Hendrikvg 24:a9ec9b836fd9 463 break;
Hendrikvg 24:a9ec9b836fd9 464 case mvc_biceps_left:
Hendrikvg 24:a9ec9b836fd9 465 SubstateTransition();
SjorsdeBruin 26:088e397ec26f 466 if ((pressed_1) || (pressed_2)) {
SjorsdeBruin 26:088e397ec26f 467 pressed_1 = false;
SjorsdeBruin 26:088e397ec26f 468 pressed_2 = false;
SjorsdeBruin 26:088e397ec26f 469 n = 0;
Hendrikvg 24:a9ec9b836fd9 470 mvc();
Hendrikvg 24:a9ec9b836fd9 471 }
Hendrikvg 24:a9ec9b836fd9 472 break;
Hendrikvg 24:a9ec9b836fd9 473 case rest_biceps_right:
Hendrikvg 24:a9ec9b836fd9 474 SubstateTransition();
SjorsdeBruin 26:088e397ec26f 475 if ((pressed_1) || (pressed_2)) {
SjorsdeBruin 26:088e397ec26f 476 pressed_1 = false;
SjorsdeBruin 26:088e397ec26f 477 pressed_2 = false;
SjorsdeBruin 26:088e397ec26f 478 n = 0;
SjorsdeBruin 26:088e397ec26f 479 sum = 0;
Hendrikvg 24:a9ec9b836fd9 480 rest();
Hendrikvg 24:a9ec9b836fd9 481 }
Hendrikvg 24:a9ec9b836fd9 482 break;
Hendrikvg 24:a9ec9b836fd9 483 case mvc_biceps_right:
Hendrikvg 24:a9ec9b836fd9 484 SubstateTransition();
SjorsdeBruin 26:088e397ec26f 485 if ((pressed_1) || (pressed_2)) {
SjorsdeBruin 26:088e397ec26f 486 pressed_1 = false;
SjorsdeBruin 26:088e397ec26f 487 pressed_2 = false;
SjorsdeBruin 26:088e397ec26f 488 n = 0;
Hendrikvg 24:a9ec9b836fd9 489 mvc();
Hendrikvg 24:a9ec9b836fd9 490 }
Hendrikvg 24:a9ec9b836fd9 491 break;
Hendrikvg 24:a9ec9b836fd9 492 case rest_biceps_leg:
Hendrikvg 24:a9ec9b836fd9 493 SubstateTransition();
SjorsdeBruin 26:088e397ec26f 494 if ((pressed_1) || (pressed_2)) {
SjorsdeBruin 26:088e397ec26f 495 pressed_1 = false;
SjorsdeBruin 26:088e397ec26f 496 pressed_2 = false;
SjorsdeBruin 26:088e397ec26f 497 n = 0;
SjorsdeBruin 26:088e397ec26f 498 sum = 0;
Hendrikvg 24:a9ec9b836fd9 499 rest();
Hendrikvg 24:a9ec9b836fd9 500 }
Hendrikvg 24:a9ec9b836fd9 501 break;
Hendrikvg 24:a9ec9b836fd9 502 case mvc_biceps_leg:
Hendrikvg 24:a9ec9b836fd9 503 SubstateTransition();
SjorsdeBruin 26:088e397ec26f 504 if ((pressed_1) || (pressed_2)) {
SjorsdeBruin 26:088e397ec26f 505 pressed_1 = false;
SjorsdeBruin 26:088e397ec26f 506 pressed_2 = false;
SjorsdeBruin 26:088e397ec26f 507 n = 0;
Hendrikvg 24:a9ec9b836fd9 508 mvc();
Hendrikvg 24:a9ec9b836fd9 509 }
Hendrikvg 24:a9ec9b836fd9 510 break;
Hendrikvg 24:a9ec9b836fd9 511 default:
Hendrikvg 24:a9ec9b836fd9 512 pc.printf("Unknown or unimplemented state reached!\n\r");
Hendrikvg 21:394a7a1deb73 513 }
Hendrikvg 21:394a7a1deb73 514 }
Hendrikvg 21:394a7a1deb73 515
Hendrikvg 21:394a7a1deb73 516 void while_vertical_movement()
Hendrikvg 21:394a7a1deb73 517 {
Hendrikvg 21:394a7a1deb73 518 // Do vertical movement stuff
SjorsdeBruin 26:088e397ec26f 519 if ((pressed_1) || (pressed_2)) { // EMG gebaseerde threshold aanmaken
Hendrikvg 21:394a7a1deb73 520 CurrentState = horizontal_movement;
Hendrikvg 21:394a7a1deb73 521 StateChanged = true;
Hendrikvg 21:394a7a1deb73 522 }
Hendrikvg 21:394a7a1deb73 523 }
Hendrikvg 17:cacf9e75eda7 524
Hendrikvg 21:394a7a1deb73 525 void while_horizontal_movement()
Hendrikvg 21:394a7a1deb73 526 {
Hendrikvg 21:394a7a1deb73 527 // Do horizontal movement stuff
SjorsdeBruin 26:088e397ec26f 528 if ((pressed_1) || (pressed_2)) { // EMG gebaseerde threshold aanmaken
Hendrikvg 21:394a7a1deb73 529 CurrentState = vertical_movement;
Hendrikvg 21:394a7a1deb73 530 StateChanged = true;
Hendrikvg 21:394a7a1deb73 531 }
Hendrikvg 21:394a7a1deb73 532 }
Hendrikvg 21:394a7a1deb73 533
Hendrikvg 21:394a7a1deb73 534 void StateTransition()
Hendrikvg 21:394a7a1deb73 535 {
Hendrikvg 24:a9ec9b836fd9 536 if (StateChanged) {
SjorsdeBruin 26:088e397ec26f 537 pressed_1 = false;
SjorsdeBruin 26:088e397ec26f 538 pressed_2 = false;
Hendrikvg 24:a9ec9b836fd9 539 if (CurrentState == start) {
Hendrikvg 21:394a7a1deb73 540 pc.printf("Initiating start.\n\r");
Hendrikvg 21:394a7a1deb73 541 }
Hendrikvg 24:a9ec9b836fd9 542 if (CurrentState == motor_calibration) {
Hendrikvg 21:394a7a1deb73 543 pc.printf("Initiating motor_calibration.\n\r");
Hendrikvg 21:394a7a1deb73 544 }
Hendrikvg 24:a9ec9b836fd9 545 if (CurrentState == demo_mode) {
Hendrikvg 21:394a7a1deb73 546 pc.printf("Initiating demo_mode.\n\r");
Hendrikvg 21:394a7a1deb73 547 }
Hendrikvg 24:a9ec9b836fd9 548 if (CurrentState == emg_calibration) {
Hendrikvg 21:394a7a1deb73 549 pc.printf("Initiating emg_calibration.\n\r");
Hendrikvg 21:394a7a1deb73 550 }
Hendrikvg 24:a9ec9b836fd9 551 if (CurrentState == vertical_movement) {
Hendrikvg 21:394a7a1deb73 552 pc.printf("Initiating vertical_movement.\n\r");
Hendrikvg 21:394a7a1deb73 553 }
Hendrikvg 24:a9ec9b836fd9 554 if (CurrentState == horizontal_movement) {
Hendrikvg 21:394a7a1deb73 555 pc.printf("Initiating horizontal_movement.\n\r");
Hendrikvg 21:394a7a1deb73 556 }
Hendrikvg 21:394a7a1deb73 557 StateChanged = false;
Hendrikvg 21:394a7a1deb73 558 }
Hendrikvg 21:394a7a1deb73 559 }
Hendrikvg 21:394a7a1deb73 560
Hendrikvg 21:394a7a1deb73 561 void StateMachine()
Hendrikvg 21:394a7a1deb73 562 {
Hendrikvg 24:a9ec9b836fd9 563 switch(CurrentState) {
Hendrikvg 21:394a7a1deb73 564 case start:
Hendrikvg 21:394a7a1deb73 565 StateTransition();
Hendrikvg 21:394a7a1deb73 566 while_start();
Hendrikvg 21:394a7a1deb73 567 break;
Hendrikvg 21:394a7a1deb73 568 case motor_calibration:
Hendrikvg 21:394a7a1deb73 569 StateTransition();
Hendrikvg 21:394a7a1deb73 570 while_motor_calibration();
Hendrikvg 21:394a7a1deb73 571 break;
Hendrikvg 21:394a7a1deb73 572 case demo_mode:
Hendrikvg 21:394a7a1deb73 573 StateTransition();
Hendrikvg 21:394a7a1deb73 574 while_demo_mode();
Hendrikvg 21:394a7a1deb73 575 break;
Hendrikvg 21:394a7a1deb73 576 case emg_calibration:
Hendrikvg 21:394a7a1deb73 577 StateTransition();
Hendrikvg 21:394a7a1deb73 578 while_emg_calibration();
Hendrikvg 21:394a7a1deb73 579 break;
Hendrikvg 21:394a7a1deb73 580 case vertical_movement:
Hendrikvg 21:394a7a1deb73 581 StateTransition();
Hendrikvg 21:394a7a1deb73 582 while_vertical_movement();
Hendrikvg 21:394a7a1deb73 583 break;
Hendrikvg 21:394a7a1deb73 584 case horizontal_movement:
Hendrikvg 21:394a7a1deb73 585 StateTransition();
Hendrikvg 21:394a7a1deb73 586 while_horizontal_movement();
Hendrikvg 21:394a7a1deb73 587 break;
Hendrikvg 21:394a7a1deb73 588 default:
Hendrikvg 21:394a7a1deb73 589 pc.printf("Unknown or unimplemented state reached!\n\r");
Hendrikvg 21:394a7a1deb73 590 }
Hendrikvg 21:394a7a1deb73 591 }
Hendrikvg 21:394a7a1deb73 592
Hendrikvg 15:80b3ac2b8448 593 // main
Hendrikvg 24:a9ec9b836fd9 594 int main()
Hendrikvg 24:a9ec9b836fd9 595 {
RobertoO 0:67c50348f842 596 pc.baud(115200);
Hendrikvg 17:cacf9e75eda7 597 pc.printf("Hello World!\n\r");
SjorsdeBruin 26:088e397ec26f 598 ledoff();
Hendrikvg 21:394a7a1deb73 599 button_1.fall(go_next_1);
Hendrikvg 21:394a7a1deb73 600 button_2.fall(go_next_2);
SjorsdeBruin 26:088e397ec26f 601 //sinus_time.start();
SjorsdeBruin 26:088e397ec26f 602 //PWM_motor_1.period_ms(10);
SjorsdeBruin 26:088e397ec26f 603 //motor_control.attach(&MotorControl, Ts);
SjorsdeBruin 26:088e397ec26f 604 write_scope.attach(&WriteScope, 0.001);
Hendrikvg 21:394a7a1deb73 605 //TickerStateMachine.attach(StateMachine,1.00f);
Hendrikvg 21:394a7a1deb73 606 while(true) {
Hendrikvg 21:394a7a1deb73 607 StateMachine();
Hendrikvg 21:394a7a1deb73 608 }
SjorsdeBruin 26:088e397ec26f 609 }