Sem Bertelink / Mbed 2 deprecated MOD09_Biorobotics_Groep_14lol

lololololol

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

main.cpp@25:832b26afbe0b, 2019-10-25 (annotated)

Committer:
Hendrikvg
Date:
Fri Oct 25 09:46:54 2019 +0000
Revision:
25:832b26afbe0b
Parent:
24:a9ec9b836fd9 
Voor Sem

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);
Hendrikvg 21:394a7a1deb73 10 InterruptIn stepresponse(D0);
Hendrikvg 24:a9ec9b836fd9 11
Hendrikvg 21:394a7a1deb73 12 FastPWM PWM_motor_1(D6);
Hendrikvg 23:78898ddfb103 13 FastPWM PWM_motor_2(D5);
Hendrikvg 24:a9ec9b836fd9 14
Hendrikvg 20:ac1b4ffa3323 15 DigitalOut direction_motor_1(D7);
Hendrikvg 23:78898ddfb103 16 DigitalOut direction_motor_2(D4);
Hendrikvg 24:a9ec9b836fd9 17 DigitalOut led_red(LED1);
Hendrikvg 24:a9ec9b836fd9 18 DigitalOut led_green(LED2);
Hendrikvg 24:a9ec9b836fd9 19 DigitalOut led_blue(LED3);
Hendrikvg 24:a9ec9b836fd9 20
Hendrikvg 24:a9ec9b836fd9 21 AnalogIn emg_bl( A0 );
Hendrikvg 24:a9ec9b836fd9 22 AnalogIn emg_br( A1 );
Hendrikvg 24:a9ec9b836fd9 23 AnalogIn emg_leg( A2 );
Hendrikvg 24:a9ec9b836fd9 24
Hendrikvg 21:394a7a1deb73 25 InterruptIn button_1(SW2);
Hendrikvg 21:394a7a1deb73 26 InterruptIn button_2(SW3);
Hendrikvg 17:cacf9e75eda7 27
Hendrikvg 24:a9ec9b836fd9 28
Hendrikvg 15:80b3ac2b8448 29 // variables
Hendrikvg 21:394a7a1deb73 30 Ticker TickerStateMachine;
Hendrikvg 23:78898ddfb103 31 Ticker motor_control;
Hendrikvg 23:78898ddfb103 32 Ticker write_scope;
Hendrikvg 21:394a7a1deb73 33 Timer sinus_time;
Hendrikvg 24:a9ec9b836fd9 34 Timeout rest_timeout;
Hendrikvg 24:a9ec9b836fd9 35 Timeout mvc_timeout;
Hendrikvg 21:394a7a1deb73 36 enum states {start, motor_calibration, demo_mode, emg_calibration, vertical_movement, horizontal_movement};
Hendrikvg 21:394a7a1deb73 37 states CurrentState = start;
Hendrikvg 21:394a7a1deb73 38 bool StateChanged = true;
Hendrikvg 24:a9ec9b836fd9 39 enum substates {rest_biceps_left, mvc_biceps_left, rest_biceps_right, mvc_biceps_right, rest_biceps_leg, mvc_biceps_leg};
Hendrikvg 24:a9ec9b836fd9 40 substates CurrentSubstate = rest_biceps_left;
Hendrikvg 24:a9ec9b836fd9 41 bool SubstateChanged = true;
Hendrikvg 24:a9ec9b836fd9 42 bool pressed_1 = false;
Hendrikvg 24:a9ec9b836fd9 43 bool pressed_2 = false;
Hendrikvg 21:394a7a1deb73 44 HIDScope scope(3);
Hendrikvg 23:78898ddfb103 45 QEI encoder_1(D10,D11,NC,8400,QEI::X4_ENCODING);
Hendrikvg 23:78898ddfb103 46 QEI encoder_2(D12,D13,NC,8400,QEI::X4_ENCODING);
Hendrikvg 22:6cc93216b323 47 volatile double theta_1;
Hendrikvg 23:78898ddfb103 48 //volatile float theta_error_1;
Hendrikvg 22:6cc93216b323 49 volatile float theta_reference_1;
Hendrikvg 23:78898ddfb103 50 volatile double theta_2;
Hendrikvg 23:78898ddfb103 51 //volatile float theta_error_2;
Hendrikvg 23:78898ddfb103 52 volatile float theta_reference_2;
Hendrikvg 21:394a7a1deb73 53 float Ts = 0.001;
Hendrikvg 23:78898ddfb103 54 float Kp;
Hendrikvg 23:78898ddfb103 55 float Ki;
Hendrikvg 23:78898ddfb103 56 float Kd;
Hendrikvg 20:ac1b4ffa3323 57
Hendrikvg 24:a9ec9b836fd9 58 BiQuadChain highnotch;
Hendrikvg 24:a9ec9b836fd9 59 BiQuadChain low;
Hendrikvg 24:a9ec9b836fd9 60 BiQuad Lowpass1 (3.73938e-07, 7.47876e-07, 3.73938e-07, -1.90886e+00, 9.11279e-01);
Hendrikvg 24:a9ec9b836fd9 61 BiQuad Lowpass2 (1.00000e+00, 2.00000e+00, 1.00000e+00, -1.95979e+00, 9.62270e-01);
Hendrikvg 24:a9ec9b836fd9 62 BiQuad Highpass (9.69531e-01, -9.69531e-01, 0.00000e+00, -9.39063e-01, 0.00000e+00 );
Hendrikvg 24:a9ec9b836fd9 63 BiQuad Notch (9.98432e-01, -1.89913e+00, 9.98432e-01, -1.89913e+00, 9.96863e-01);
Hendrikvg 24:a9ec9b836fd9 64 int n = 0;
Hendrikvg 24:a9ec9b836fd9 65
Hendrikvg 24:a9ec9b836fd9 66 double emgFiltered_bl;
Hendrikvg 24:a9ec9b836fd9 67 double emgFiltered_br;
Hendrikvg 24:a9ec9b836fd9 68 double emgFiltered_leg;
Hendrikvg 24:a9ec9b836fd9 69 double emg;
Hendrikvg 24:a9ec9b836fd9 70 double xmvc_value = 1e-11;
Hendrikvg 24:a9ec9b836fd9 71
Hendrikvg 24:a9ec9b836fd9 72 int muscle;
Hendrikvg 24:a9ec9b836fd9 73 float sum = 0;
Hendrikvg 24:a9ec9b836fd9 74 float rest_value_bl;
Hendrikvg 24:a9ec9b836fd9 75 float rest_value_br;
Hendrikvg 24:a9ec9b836fd9 76 float rest_value_leg;
Hendrikvg 24:a9ec9b836fd9 77
Hendrikvg 24:a9ec9b836fd9 78 float mvc_value_bl;
Hendrikvg 24:a9ec9b836fd9 79 float mvc_value_br;
Hendrikvg 24:a9ec9b836fd9 80 float mvc_value_leg;
Hendrikvg 24:a9ec9b836fd9 81
Hendrikvg 21:394a7a1deb73 82 // functies
Hendrikvg 22:6cc93216b323 83 float CalculateError(float theta_reference,float theta)
Hendrikvg 21:394a7a1deb73 84 {
Hendrikvg 22:6cc93216b323 85 float theta_error = theta_reference-theta;
Hendrikvg 21:394a7a1deb73 86 return theta_error;
Hendrikvg 21:394a7a1deb73 87 }
Hendrikvg 20:ac1b4ffa3323 88
Hendrikvg 23:78898ddfb103 89 float Controller(float theta_error, bool motor)
Hendrikvg 21:394a7a1deb73 90 {
Hendrikvg 24:a9ec9b836fd9 91 if (motor == false) {
Hendrikvg 23:78898ddfb103 92 float K = 1;
Hendrikvg 23:78898ddfb103 93 float ti = 0.1;
Hendrikvg 23:78898ddfb103 94 float td = 10;
Hendrikvg 23:78898ddfb103 95 Kp = K*(1+td/ti);
Hendrikvg 23:78898ddfb103 96 Ki = K/ti;
Hendrikvg 23:78898ddfb103 97 Kd = K*td;
Hendrikvg 24:a9ec9b836fd9 98 } else {
Hendrikvg 23:78898ddfb103 99 float K = 1;
Hendrikvg 23:78898ddfb103 100 float ti = 0.1;
Hendrikvg 23:78898ddfb103 101 float td = 10;
Hendrikvg 23:78898ddfb103 102 Kp = K*(1+td/ti);
Hendrikvg 23:78898ddfb103 103 Ki = K/ti;
Hendrikvg 23:78898ddfb103 104 Kd = K*td;
Hendrikvg 23:78898ddfb103 105 }
Hendrikvg 21:394a7a1deb73 106 static float error_integral = 0;
Hendrikvg 21:394a7a1deb73 107 static float error_prev = 0;
Hendrikvg 21:394a7a1deb73 108 static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
Hendrikvg 24:a9ec9b836fd9 109
Hendrikvg 20:ac1b4ffa3323 110 // Proportional part:
Hendrikvg 20:ac1b4ffa3323 111 float torque_p = Kp * theta_error;
Hendrikvg 24:a9ec9b836fd9 112
Hendrikvg 20:ac1b4ffa3323 113 // Integral part:
Hendrikvg 21:394a7a1deb73 114 error_integral = error_integral + theta_error * Ts;
Hendrikvg 21:394a7a1deb73 115 float torque_i = Ki * error_integral;
Hendrikvg 24:a9ec9b836fd9 116
Hendrikvg 20:ac1b4ffa3323 117 // Derivative part:
Hendrikvg 21:394a7a1deb73 118 float error_derivative = (theta_error - error_prev)/Ts;
Hendrikvg 21:394a7a1deb73 119 float filtered_error_derivative = LowPassFilter.step(error_derivative);
Hendrikvg 21:394a7a1deb73 120 float torque_d = Kd * filtered_error_derivative;
Hendrikvg 21:394a7a1deb73 121 error_prev = theta_error;
Hendrikvg 24:a9ec9b836fd9 122
Hendrikvg 20:ac1b4ffa3323 123 // Sum all parts and return it
Hendrikvg 21:394a7a1deb73 124 float torque = torque_p + torque_i + torque_d;
Hendrikvg 21:394a7a1deb73 125 return torque;
Hendrikvg 21:394a7a1deb73 126 }
Hendrikvg 16:40183eeadb6d 127
Hendrikvg 24:a9ec9b836fd9 128 void CalculateDirectionMotor()
Hendrikvg 21:394a7a1deb73 129 {
Hendrikvg 23:78898ddfb103 130 direction_motor_1 = Controller(CalculateError(theta_reference_1,theta_1),0) <= 0.0f;
Hendrikvg 23:78898ddfb103 131 direction_motor_2 = Controller(CalculateError(theta_reference_2,theta_2),1) <= 0.0f;
Hendrikvg 21:394a7a1deb73 132 }
Hendrikvg 20:ac1b4ffa3323 133
Hendrikvg 23:78898ddfb103 134 void ReadEncoder()
Hendrikvg 23:78898ddfb103 135 {
Hendrikvg 23:78898ddfb103 136 theta_1 = ((360.0f/64.0f)*(float)encoder_1.getPulses())/131.25f; // 360/64 voor de 64 CPR Encoder, 131.25 omdat de gear ratio 131.25:1 is. Hiermee is 1 omwenteling 360.
Hendrikvg 23:78898ddfb103 137 theta_2 = ((360.0f/64.0f)*(float)encoder_2.getPulses())/131.25f;
Hendrikvg 23:78898ddfb103 138 }
Hendrikvg 16:40183eeadb6d 139
Hendrikvg 24:a9ec9b836fd9 140 void MotorControl()
Hendrikvg 23:78898ddfb103 141 {
Hendrikvg 21:394a7a1deb73 142 ReadEncoder();
Hendrikvg 23:78898ddfb103 143 theta_reference_1 = 360*sin(0.1f*sinus_time.read()*2*3.14f); // voor test, moet weg in eindscript
Hendrikvg 22:6cc93216b323 144 CalculateDirectionMotor();
Hendrikvg 23:78898ddfb103 145 PWM_motor_1.write(fabs(Controller(CalculateError(theta_reference_1,theta_1),0)/360.0f));
Hendrikvg 23:78898ddfb103 146 PWM_motor_2.write(fabs(Controller(CalculateError(theta_reference_2,theta_2),1)/360.0f));
Hendrikvg 21:394a7a1deb73 147 }
Hendrikvg 21:394a7a1deb73 148
Hendrikvg 21:394a7a1deb73 149 void go_next_1()
Hendrikvg 21:394a7a1deb73 150 {
Hendrikvg 24:a9ec9b836fd9 151 pressed_1 = !pressed_1;
Hendrikvg 21:394a7a1deb73 152 }
Hendrikvg 21:394a7a1deb73 153
Hendrikvg 21:394a7a1deb73 154 void go_next_2()
Hendrikvg 21:394a7a1deb73 155 {
Hendrikvg 24:a9ec9b836fd9 156 pressed_2 = !pressed_2;
Hendrikvg 24:a9ec9b836fd9 157 }
Hendrikvg 24:a9ec9b836fd9 158
Hendrikvg 24:a9ec9b836fd9 159 float EmgCalibration(double emgFiltered, float mvc_value, float rest_value)
Hendrikvg 24:a9ec9b836fd9 160 {
Hendrikvg 24:a9ec9b836fd9 161 float emgCalibrated;
Hendrikvg 24:a9ec9b836fd9 162 if (emgFiltered <= rest_value) {
Hendrikvg 24:a9ec9b836fd9 163 emgCalibrated = 0;
Hendrikvg 24:a9ec9b836fd9 164 }
Hendrikvg 24:a9ec9b836fd9 165 if (emgFiltered >= mvc_value) {
Hendrikvg 24:a9ec9b836fd9 166 emgCalibrated = 1;
Hendrikvg 24:a9ec9b836fd9 167 } else {
Hendrikvg 24:a9ec9b836fd9 168 emgCalibrated = (emgFiltered-rest_value)/(mvc_value-rest_value);
Hendrikvg 24:a9ec9b836fd9 169 }
Hendrikvg 24:a9ec9b836fd9 170 return emgCalibrated;
Hendrikvg 24:a9ec9b836fd9 171 }
Hendrikvg 24:a9ec9b836fd9 172
Hendrikvg 24:a9ec9b836fd9 173 void emgsample()
Hendrikvg 24:a9ec9b836fd9 174 {
Hendrikvg 24:a9ec9b836fd9 175 emgFiltered_bl = highnotch.step(emg_bl.read());
Hendrikvg 24:a9ec9b836fd9 176 emgFiltered_bl = fabs(emgFiltered_bl);
Hendrikvg 24:a9ec9b836fd9 177 emgFiltered_bl = low.step(emgFiltered_bl);
Hendrikvg 24:a9ec9b836fd9 178
Hendrikvg 24:a9ec9b836fd9 179 emgFiltered_br = highnotch.step(emg_br.read());
Hendrikvg 24:a9ec9b836fd9 180 emgFiltered_br = fabs(emgFiltered_br);
Hendrikvg 24:a9ec9b836fd9 181 emgFiltered_br = low.step(emgFiltered_br);
Hendrikvg 24:a9ec9b836fd9 182
Hendrikvg 24:a9ec9b836fd9 183 emgFiltered_leg = highnotch.step(emg_leg.read());
Hendrikvg 24:a9ec9b836fd9 184 emgFiltered_leg = fabs(emgFiltered_leg);
Hendrikvg 24:a9ec9b836fd9 185 emgFiltered_leg = low.step(emgFiltered_leg);
Hendrikvg 24:a9ec9b836fd9 186 }
Hendrikvg 24:a9ec9b836fd9 187
Hendrikvg 24:a9ec9b836fd9 188 void rest()
Hendrikvg 24:a9ec9b836fd9 189 {
Hendrikvg 24:a9ec9b836fd9 190 if (muscle == 0)
Hendrikvg 24:a9ec9b836fd9 191 {
Hendrikvg 24:a9ec9b836fd9 192 emg = emgFiltered_bl;
Hendrikvg 24:a9ec9b836fd9 193 }
Hendrikvg 24:a9ec9b836fd9 194 if (muscle == 2)
Hendrikvg 24:a9ec9b836fd9 195 {
Hendrikvg 24:a9ec9b836fd9 196 emg = emgFiltered_br;
Hendrikvg 24:a9ec9b836fd9 197 }
Hendrikvg 24:a9ec9b836fd9 198 if (muscle == 4)
Hendrikvg 24:a9ec9b836fd9 199 {
Hendrikvg 24:a9ec9b836fd9 200 emg = emgFiltered_leg;
Hendrikvg 24:a9ec9b836fd9 201 }
Hendrikvg 24:a9ec9b836fd9 202 if (n < 50)
Hendrikvg 24:a9ec9b836fd9 203 {
Hendrikvg 24:a9ec9b836fd9 204 sum = sum + emg;
Hendrikvg 24:a9ec9b836fd9 205 n++;
Hendrikvg 24:a9ec9b836fd9 206 rest_timeout.attach(rest,0.01f);
Hendrikvg 24:a9ec9b836fd9 207 }
Hendrikvg 24:a9ec9b836fd9 208 if (n == 50)
Hendrikvg 24:a9ec9b836fd9 209 {
Hendrikvg 24:a9ec9b836fd9 210 sum = sum + emg;
Hendrikvg 24:a9ec9b836fd9 211 n++;
Hendrikvg 24:a9ec9b836fd9 212 if (muscle == 0)
Hendrikvg 24:a9ec9b836fd9 213 {
Hendrikvg 25:832b26afbe0b 214 rest_value_bl = float (sum/n);
Hendrikvg 24:a9ec9b836fd9 215 CurrentSubstate = mvc_biceps_left;
Hendrikvg 24:a9ec9b836fd9 216 SubstateChanged = true;
Hendrikvg 24:a9ec9b836fd9 217 led_red = 1;
Hendrikvg 24:a9ec9b836fd9 218 }
Hendrikvg 24:a9ec9b836fd9 219 if (muscle == 2)
Hendrikvg 24:a9ec9b836fd9 220 {
Hendrikvg 25:832b26afbe0b 221 rest_value_br = float (sum/n);
Hendrikvg 24:a9ec9b836fd9 222 CurrentSubstate = mvc_biceps_right;
Hendrikvg 24:a9ec9b836fd9 223 SubstateChanged = true;
Hendrikvg 24:a9ec9b836fd9 224 led_red = 1;
Hendrikvg 24:a9ec9b836fd9 225 }
Hendrikvg 24:a9ec9b836fd9 226 if (muscle == 4)
Hendrikvg 24:a9ec9b836fd9 227 {
Hendrikvg 25:832b26afbe0b 228 rest_value_leg = float (sum/n);
Hendrikvg 24:a9ec9b836fd9 229 CurrentSubstate = mvc_biceps_leg;
Hendrikvg 24:a9ec9b836fd9 230 SubstateChanged = true;
Hendrikvg 24:a9ec9b836fd9 231 led_red = 1;
Hendrikvg 24:a9ec9b836fd9 232 }
Hendrikvg 25:832b26afbe0b 233 n = 0;
Hendrikvg 25:832b26afbe0b 234 sum = 0;
Hendrikvg 24:a9ec9b836fd9 235 }
Hendrikvg 24:a9ec9b836fd9 236 }
Hendrikvg 24:a9ec9b836fd9 237
Hendrikvg 24:a9ec9b836fd9 238 void mvc()
Hendrikvg 24:a9ec9b836fd9 239 {
Hendrikvg 24:a9ec9b836fd9 240 if (muscle == 1)
Hendrikvg 24:a9ec9b836fd9 241 {
Hendrikvg 24:a9ec9b836fd9 242 emg = emgFiltered_bl;
Hendrikvg 24:a9ec9b836fd9 243 }
Hendrikvg 24:a9ec9b836fd9 244 if (muscle == 3)
Hendrikvg 24:a9ec9b836fd9 245 {
Hendrikvg 24:a9ec9b836fd9 246 emg = emgFiltered_br;
Hendrikvg 24:a9ec9b836fd9 247 }
Hendrikvg 24:a9ec9b836fd9 248 if (muscle == 5)
Hendrikvg 24:a9ec9b836fd9 249 {
Hendrikvg 24:a9ec9b836fd9 250 emg = emgFiltered_leg;
Hendrikvg 24:a9ec9b836fd9 251 }
Hendrikvg 24:a9ec9b836fd9 252 if (emg >= xmvc_value)
Hendrikvg 24:a9ec9b836fd9 253 {
Hendrikvg 24:a9ec9b836fd9 254 xmvc_value = emg;
Hendrikvg 24:a9ec9b836fd9 255 }
Hendrikvg 24:a9ec9b836fd9 256 n++;
Hendrikvg 24:a9ec9b836fd9 257 if (n < 100)
Hendrikvg 24:a9ec9b836fd9 258 {
Hendrikvg 24:a9ec9b836fd9 259 mvc_timeout.attach(mvc,0.01f);
Hendrikvg 24:a9ec9b836fd9 260 }
Hendrikvg 24:a9ec9b836fd9 261 if (n == 100)
Hendrikvg 24:a9ec9b836fd9 262 {
Hendrikvg 24:a9ec9b836fd9 263 n = 0;
Hendrikvg 24:a9ec9b836fd9 264 if (muscle == 1)
Hendrikvg 24:a9ec9b836fd9 265 {
Hendrikvg 24:a9ec9b836fd9 266 mvc_value_bl = xmvc_value;
Hendrikvg 24:a9ec9b836fd9 267 CurrentSubstate = rest_biceps_right;
Hendrikvg 24:a9ec9b836fd9 268 SubstateChanged = true;
Hendrikvg 24:a9ec9b836fd9 269 led_red = 1;
Hendrikvg 24:a9ec9b836fd9 270 }
Hendrikvg 24:a9ec9b836fd9 271 if (muscle == 3)
Hendrikvg 24:a9ec9b836fd9 272 {
Hendrikvg 24:a9ec9b836fd9 273 mvc_value_br = xmvc_value;
Hendrikvg 24:a9ec9b836fd9 274 CurrentSubstate = rest_biceps_leg;
Hendrikvg 24:a9ec9b836fd9 275 SubstateChanged = true;
Hendrikvg 24:a9ec9b836fd9 276 led_red = 1;
Hendrikvg 24:a9ec9b836fd9 277 }
Hendrikvg 24:a9ec9b836fd9 278 if (muscle == 5)
Hendrikvg 24:a9ec9b836fd9 279 {
Hendrikvg 24:a9ec9b836fd9 280 mvc_value_leg = xmvc_value;
Hendrikvg 24:a9ec9b836fd9 281 CurrentState = vertical_movement;
Hendrikvg 24:a9ec9b836fd9 282 StateChanged = true;
Hendrikvg 24:a9ec9b836fd9 283 led_red = 1;
Hendrikvg 24:a9ec9b836fd9 284 }
Hendrikvg 24:a9ec9b836fd9 285 xmvc_value = 1e-11;
Hendrikvg 24:a9ec9b836fd9 286 led_red = 1;
Hendrikvg 24:a9ec9b836fd9 287 }
Hendrikvg 24:a9ec9b836fd9 288 }
Hendrikvg 24:a9ec9b836fd9 289
Hendrikvg 24:a9ec9b836fd9 290 void WriteScope()
Hendrikvg 24:a9ec9b836fd9 291 {
Hendrikvg 24:a9ec9b836fd9 292 //scope.set(0,theta_1);
Hendrikvg 24:a9ec9b836fd9 293 // scope.set(1,CalculateError(theta_reference_1,theta_1));
Hendrikvg 24:a9ec9b836fd9 294 // scope.set(2,theta_reference_1);
Hendrikvg 24:a9ec9b836fd9 295 scope.set(0, EmgCalibration(emgFiltered_bl, mvc_value_bl, rest_value_bl));
Hendrikvg 24:a9ec9b836fd9 296 scope.set(1, EmgCalibration(emgFiltered_br, mvc_value_br, rest_value_br) );
Hendrikvg 24:a9ec9b836fd9 297 scope.set(2, EmgCalibration(emgFiltered_leg, mvc_value_leg, rest_value_leg));
Hendrikvg 24:a9ec9b836fd9 298 scope.send();
Hendrikvg 24:a9ec9b836fd9 299 }
Hendrikvg 24:a9ec9b836fd9 300
Hendrikvg 24:a9ec9b836fd9 301 void SubstateTransition()
Hendrikvg 24:a9ec9b836fd9 302 {
Hendrikvg 24:a9ec9b836fd9 303 pressed_1 = false;
Hendrikvg 24:a9ec9b836fd9 304 pressed_2 = false;
Hendrikvg 24:a9ec9b836fd9 305 SubstateChanged = false;
Hendrikvg 21:394a7a1deb73 306 }
Hendrikvg 21:394a7a1deb73 307
Hendrikvg 21:394a7a1deb73 308 void while_start()
Hendrikvg 21:394a7a1deb73 309 {
Hendrikvg 21:394a7a1deb73 310 // Do startup stuff
Hendrikvg 21:394a7a1deb73 311 CurrentState = motor_calibration;
Hendrikvg 21:394a7a1deb73 312 StateChanged = true;
Hendrikvg 21:394a7a1deb73 313 }
Hendrikvg 21:394a7a1deb73 314
Hendrikvg 21:394a7a1deb73 315 void while_motor_calibration()
Hendrikvg 21:394a7a1deb73 316 {
Hendrikvg 21:394a7a1deb73 317 // Do motor calibration stuff
Hendrikvg 24:a9ec9b836fd9 318 if (pressed_1) { // bool aanmaken voor demo (switch oid aanmaken)
Hendrikvg 21:394a7a1deb73 319 CurrentState = demo_mode;
Hendrikvg 21:394a7a1deb73 320 StateChanged = true;
Hendrikvg 21:394a7a1deb73 321 }
Hendrikvg 24:a9ec9b836fd9 322 if (pressed_2) { // bool aanmaken voor EMG (switch oid aanmaken)
Hendrikvg 21:394a7a1deb73 323 CurrentState = emg_calibration;
Hendrikvg 21:394a7a1deb73 324 StateChanged = true;
Hendrikvg 21:394a7a1deb73 325 }
Hendrikvg 21:394a7a1deb73 326 }
Hendrikvg 21:394a7a1deb73 327
Hendrikvg 21:394a7a1deb73 328 void while_demo_mode()
Hendrikvg 21:394a7a1deb73 329 {
Hendrikvg 21:394a7a1deb73 330 // Do demo mode stuff
Hendrikvg 24:a9ec9b836fd9 331 if (pressed_1 || pressed_2) {
Hendrikvg 21:394a7a1deb73 332 CurrentState = emg_calibration;
Hendrikvg 21:394a7a1deb73 333 StateChanged = true;
Hendrikvg 21:394a7a1deb73 334 }
Hendrikvg 21:394a7a1deb73 335 }
Hendrikvg 21:394a7a1deb73 336
Hendrikvg 21:394a7a1deb73 337 void while_emg_calibration()
Hendrikvg 21:394a7a1deb73 338 {
Hendrikvg 21:394a7a1deb73 339 // Do emg calibration stuff
Hendrikvg 24:a9ec9b836fd9 340 switch (CurrentSubstate) {
Hendrikvg 24:a9ec9b836fd9 341 case rest_biceps_left:
Hendrikvg 24:a9ec9b836fd9 342 SubstateTransition();
Hendrikvg 24:a9ec9b836fd9 343 muscle = 0;
Hendrikvg 24:a9ec9b836fd9 344 led_green = 0;
Hendrikvg 24:a9ec9b836fd9 345 if (pressed_1 || pressed_2)
Hendrikvg 24:a9ec9b836fd9 346 {
Hendrikvg 24:a9ec9b836fd9 347 led_green = 1;
Hendrikvg 24:a9ec9b836fd9 348 led_red = 0;
Hendrikvg 24:a9ec9b836fd9 349 rest();
Hendrikvg 24:a9ec9b836fd9 350 }
Hendrikvg 24:a9ec9b836fd9 351 break;
Hendrikvg 24:a9ec9b836fd9 352 case mvc_biceps_left:
Hendrikvg 24:a9ec9b836fd9 353 SubstateTransition();
Hendrikvg 24:a9ec9b836fd9 354 muscle = 1;
Hendrikvg 24:a9ec9b836fd9 355 led_blue = 0;
Hendrikvg 24:a9ec9b836fd9 356 if (pressed_1 || pressed_2)
Hendrikvg 24:a9ec9b836fd9 357 {
Hendrikvg 24:a9ec9b836fd9 358 led_blue = 1;
Hendrikvg 24:a9ec9b836fd9 359 led_red = 0;
Hendrikvg 24:a9ec9b836fd9 360 mvc();
Hendrikvg 24:a9ec9b836fd9 361 }
Hendrikvg 24:a9ec9b836fd9 362 break;
Hendrikvg 24:a9ec9b836fd9 363 case rest_biceps_right:
Hendrikvg 24:a9ec9b836fd9 364 SubstateTransition();
Hendrikvg 24:a9ec9b836fd9 365 muscle = 2;
Hendrikvg 24:a9ec9b836fd9 366 led_red = 0;
Hendrikvg 24:a9ec9b836fd9 367 led_green = 0;
Hendrikvg 24:a9ec9b836fd9 368 if (pressed_1 || pressed_2)
Hendrikvg 24:a9ec9b836fd9 369 {
Hendrikvg 24:a9ec9b836fd9 370 led_green = 1;
Hendrikvg 24:a9ec9b836fd9 371 rest();
Hendrikvg 24:a9ec9b836fd9 372 }
Hendrikvg 24:a9ec9b836fd9 373 break;
Hendrikvg 24:a9ec9b836fd9 374 case mvc_biceps_right:
Hendrikvg 24:a9ec9b836fd9 375 SubstateTransition();
Hendrikvg 24:a9ec9b836fd9 376 muscle = 3;
Hendrikvg 24:a9ec9b836fd9 377 led_red = 0;
Hendrikvg 24:a9ec9b836fd9 378 led_blue = 0;
Hendrikvg 24:a9ec9b836fd9 379 if (pressed_1 || pressed_2)
Hendrikvg 24:a9ec9b836fd9 380 {
Hendrikvg 24:a9ec9b836fd9 381 led_blue = 1;
Hendrikvg 24:a9ec9b836fd9 382 mvc();
Hendrikvg 24:a9ec9b836fd9 383 }
Hendrikvg 24:a9ec9b836fd9 384 break;
Hendrikvg 24:a9ec9b836fd9 385 case rest_biceps_leg:
Hendrikvg 24:a9ec9b836fd9 386 SubstateTransition();
Hendrikvg 24:a9ec9b836fd9 387 muscle = 4;
Hendrikvg 24:a9ec9b836fd9 388 led_green = 0;
Hendrikvg 24:a9ec9b836fd9 389 led_blue = 0;
Hendrikvg 24:a9ec9b836fd9 390 if (pressed_1 || pressed_2)
Hendrikvg 24:a9ec9b836fd9 391 {
Hendrikvg 24:a9ec9b836fd9 392 led_green = 1;
Hendrikvg 24:a9ec9b836fd9 393 led_blue = 1;
Hendrikvg 24:a9ec9b836fd9 394 led_red = 0;
Hendrikvg 24:a9ec9b836fd9 395 rest();
Hendrikvg 24:a9ec9b836fd9 396 }
Hendrikvg 24:a9ec9b836fd9 397 break;
Hendrikvg 24:a9ec9b836fd9 398 case mvc_biceps_leg:
Hendrikvg 24:a9ec9b836fd9 399 SubstateTransition();
Hendrikvg 24:a9ec9b836fd9 400 muscle = 5;
Hendrikvg 24:a9ec9b836fd9 401 led_red = 0;
Hendrikvg 24:a9ec9b836fd9 402 led_green = 0;
Hendrikvg 24:a9ec9b836fd9 403 led_blue = 0;
Hendrikvg 24:a9ec9b836fd9 404 if (pressed_1 || pressed_2)
Hendrikvg 24:a9ec9b836fd9 405 {
Hendrikvg 24:a9ec9b836fd9 406 led_green = 1;
Hendrikvg 24:a9ec9b836fd9 407 led_blue = 1;
Hendrikvg 24:a9ec9b836fd9 408 led_red = 0;
Hendrikvg 24:a9ec9b836fd9 409 mvc();
Hendrikvg 24:a9ec9b836fd9 410 }
Hendrikvg 24:a9ec9b836fd9 411 break;
Hendrikvg 24:a9ec9b836fd9 412 default:
Hendrikvg 24:a9ec9b836fd9 413 pc.printf("Unknown or unimplemented state reached!\n\r");
Hendrikvg 21:394a7a1deb73 414 }
Hendrikvg 21:394a7a1deb73 415 }
Hendrikvg 21:394a7a1deb73 416
Hendrikvg 21:394a7a1deb73 417 void while_vertical_movement()
Hendrikvg 21:394a7a1deb73 418 {
Hendrikvg 21:394a7a1deb73 419 // Do vertical movement stuff
Hendrikvg 24:a9ec9b836fd9 420 if (pressed_1 || pressed_2) { // EMG gebaseerde threshold aanmaken
Hendrikvg 21:394a7a1deb73 421 CurrentState = horizontal_movement;
Hendrikvg 21:394a7a1deb73 422 StateChanged = true;
Hendrikvg 21:394a7a1deb73 423 }
Hendrikvg 21:394a7a1deb73 424 }
Hendrikvg 17:cacf9e75eda7 425
Hendrikvg 21:394a7a1deb73 426 void while_horizontal_movement()
Hendrikvg 21:394a7a1deb73 427 {
Hendrikvg 21:394a7a1deb73 428 // Do horizontal movement stuff
Hendrikvg 24:a9ec9b836fd9 429 if (pressed_1 || pressed_2) { // EMG gebaseerde threshold aanmaken
Hendrikvg 21:394a7a1deb73 430 CurrentState = vertical_movement;
Hendrikvg 21:394a7a1deb73 431 StateChanged = true;
Hendrikvg 21:394a7a1deb73 432 }
Hendrikvg 21:394a7a1deb73 433 }
Hendrikvg 21:394a7a1deb73 434
Hendrikvg 21:394a7a1deb73 435 void StateTransition()
Hendrikvg 21:394a7a1deb73 436 {
Hendrikvg 24:a9ec9b836fd9 437 pressed_1 = false;
Hendrikvg 24:a9ec9b836fd9 438 pressed_2 = false;
Hendrikvg 24:a9ec9b836fd9 439 if (StateChanged) {
Hendrikvg 24:a9ec9b836fd9 440 if (CurrentState == start) {
Hendrikvg 21:394a7a1deb73 441 pc.printf("Initiating start.\n\r");
Hendrikvg 21:394a7a1deb73 442 }
Hendrikvg 24:a9ec9b836fd9 443 if (CurrentState == motor_calibration) {
Hendrikvg 21:394a7a1deb73 444 pc.printf("Initiating motor_calibration.\n\r");
Hendrikvg 21:394a7a1deb73 445 }
Hendrikvg 24:a9ec9b836fd9 446 if (CurrentState == demo_mode) {
Hendrikvg 21:394a7a1deb73 447 pc.printf("Initiating demo_mode.\n\r");
Hendrikvg 21:394a7a1deb73 448 }
Hendrikvg 24:a9ec9b836fd9 449 if (CurrentState == emg_calibration) {
Hendrikvg 21:394a7a1deb73 450 pc.printf("Initiating emg_calibration.\n\r");
Hendrikvg 21:394a7a1deb73 451 }
Hendrikvg 24:a9ec9b836fd9 452 if (CurrentState == vertical_movement) {
Hendrikvg 21:394a7a1deb73 453 pc.printf("Initiating vertical_movement.\n\r");
Hendrikvg 21:394a7a1deb73 454 }
Hendrikvg 24:a9ec9b836fd9 455 if (CurrentState == horizontal_movement) {
Hendrikvg 21:394a7a1deb73 456 pc.printf("Initiating horizontal_movement.\n\r");
Hendrikvg 21:394a7a1deb73 457 }
Hendrikvg 21:394a7a1deb73 458 StateChanged = false;
Hendrikvg 21:394a7a1deb73 459 }
Hendrikvg 21:394a7a1deb73 460 }
Hendrikvg 21:394a7a1deb73 461
Hendrikvg 21:394a7a1deb73 462 void StateMachine()
Hendrikvg 21:394a7a1deb73 463 {
Hendrikvg 24:a9ec9b836fd9 464 switch(CurrentState) {
Hendrikvg 21:394a7a1deb73 465 case start:
Hendrikvg 21:394a7a1deb73 466 StateTransition();
Hendrikvg 21:394a7a1deb73 467 while_start();
Hendrikvg 21:394a7a1deb73 468 break;
Hendrikvg 21:394a7a1deb73 469 case motor_calibration:
Hendrikvg 21:394a7a1deb73 470 StateTransition();
Hendrikvg 21:394a7a1deb73 471 while_motor_calibration();
Hendrikvg 21:394a7a1deb73 472 break;
Hendrikvg 21:394a7a1deb73 473 case demo_mode:
Hendrikvg 21:394a7a1deb73 474 StateTransition();
Hendrikvg 21:394a7a1deb73 475 while_demo_mode();
Hendrikvg 21:394a7a1deb73 476 break;
Hendrikvg 21:394a7a1deb73 477 case emg_calibration:
Hendrikvg 21:394a7a1deb73 478 StateTransition();
Hendrikvg 21:394a7a1deb73 479 while_emg_calibration();
Hendrikvg 21:394a7a1deb73 480 break;
Hendrikvg 21:394a7a1deb73 481 case vertical_movement:
Hendrikvg 21:394a7a1deb73 482 StateTransition();
Hendrikvg 21:394a7a1deb73 483 while_vertical_movement();
Hendrikvg 21:394a7a1deb73 484 break;
Hendrikvg 21:394a7a1deb73 485 case horizontal_movement:
Hendrikvg 21:394a7a1deb73 486 StateTransition();
Hendrikvg 21:394a7a1deb73 487 while_horizontal_movement();
Hendrikvg 21:394a7a1deb73 488 break;
Hendrikvg 21:394a7a1deb73 489 default:
Hendrikvg 21:394a7a1deb73 490 pc.printf("Unknown or unimplemented state reached!\n\r");
Hendrikvg 21:394a7a1deb73 491 }
Hendrikvg 21:394a7a1deb73 492 }
Hendrikvg 21:394a7a1deb73 493
Hendrikvg 15:80b3ac2b8448 494 // main
Hendrikvg 24:a9ec9b836fd9 495 int main()
Hendrikvg 24:a9ec9b836fd9 496 {
RobertoO 0:67c50348f842 497 pc.baud(115200);
Hendrikvg 17:cacf9e75eda7 498 pc.printf("Hello World!\n\r");
Hendrikvg 21:394a7a1deb73 499 button_1.fall(go_next_1);
Hendrikvg 21:394a7a1deb73 500 button_2.fall(go_next_2);
Hendrikvg 21:394a7a1deb73 501 sinus_time.start();
Hendrikvg 21:394a7a1deb73 502 PWM_motor_1.period_ms(10);
Hendrikvg 23:78898ddfb103 503 motor_control.attach(&MotorControl, Ts);
Hendrikvg 23:78898ddfb103 504 write_scope.attach(&WriteScope, 0.1);
Hendrikvg 21:394a7a1deb73 505 //TickerStateMachine.attach(StateMachine,1.00f);
Hendrikvg 21:394a7a1deb73 506 while(true) {
Hendrikvg 21:394a7a1deb73 507 StateMachine();
Hendrikvg 21:394a7a1deb73 508 }
Hendrikvg 2:d9b0ebf3fcca 509 }