Sem Bertelink / Mbed 2 deprecated MOD09_Biorobotics_Groep_14lol

lololololol

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

main.cpp@34:89a424fd37ce, 2019-10-31 (annotated)

Committer:
sembert
Date:
Thu Oct 31 13:06:38 2019 +0000
Revision:
34:89a424fd37ce
Parent:
33:1da600f06862
Child:
35:ab9b1c9b6d08
kanker mongol liep over ons project heen

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