Sem Bertelink / Mbed 2 deprecated MOD09_Biorobotics_Groep_14lol

lololololol

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

main.cpp@36:66f500e387c4, 2019-10-31 (annotated)

Committer:
sembert
Date:
Thu Oct 31 21:49:14 2019 +0000
Revision:
36:66f500e387c4
Parent:
35:ab9b1c9b6d08 
lololol;

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