Sem Bertelink / Mbed 2 deprecated MOD09_Biorobotics_Groep_14lol

lololololol

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

main.cpp@28:8c90a46b613e, 2019-10-29 (annotated)

Committer:
SjorsdeBruin
Date:
Tue Oct 29 17:14:23 2019 +0000
Revision:
28:8c90a46b613e
Parent:
27:d37b3a0e0f2b
Child:
29:5a846abba59e
Kinematica + EMG

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