Sem Bertelink / Mbed 2 deprecated MOD09_Biorobotics_Groep_14lol

lololololol

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

main.cpp@31:465a6e1e1db6, 2019-10-30 (annotated)

Committer:
SjorsdeBruin
Date:
Wed Oct 30 16:04:43 2019 +0000
Revision:
31:465a6e1e1db6
Parent:
30:a16519224d58
Child:
32:7355524d862f
al iets beter

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