Sem Bertelink / Mbed 2 deprecated MOD09_Biorobotics_Groep_14lol

lololololol

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

main.cpp@33:1da600f06862, 2019-10-31 (annotated)

Committer:
SjorsdeBruin
Date:
Thu Oct 31 10:05:31 2019 +0000
Revision:
33:1da600f06862
Parent:
32:7355524d862f
Child:
34:89a424fd37ce
hahaha het werkt niet

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