M9 Biorobotics Group 8 / Mbed 2 deprecated CARD_program

De hele robot in 1 keer bam

Dependencies:   mbed QEI Servo HIDScope biquadFilter MODSERIAL FastPWM

main.cpp@41:8e8141f355af, 2019-10-30 (annotated)

Committer:
Jellehierck
Date:
Wed Oct 30 20:44:56 2019 +0000
Revision:
41:8e8141f355af
Parent:
40:c6dffb676350
Child:
42:2937ad8f1032
EMG and motor machines are working correctly

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Jellehierck 37:806c7c8381a7 1 /*
Jellehierck 37:806c7c8381a7 2 ------------------------------ ADD LIBRARIES ------------------------------
Jellehierck 37:806c7c8381a7 3 */
Jellehierck 38:8b597ab8344f 4 #include "mbed.h" // Base library
Jellehierck 38:8b597ab8344f 5 #include "HIDScope.h" // Scope connection to PC
Jellehierck 38:8b597ab8344f 6 #include "MODSERIAL.h" // Serial connection to PC
Jellehierck 38:8b597ab8344f 7 #include "BiQuad.h" // Biquad filter management
Jellehierck 38:8b597ab8344f 8 #include <vector> // Array management
Jellehierck 40:c6dffb676350 9 #include "FastPWM.h" // PWM
Jellehierck 40:c6dffb676350 10 #include "QEI.h" // Encoder
IsaRobin 0:6972d0e91af1 11
Jellehierck 15:421d3d9c563b 12 /*
Jellehierck 37:806c7c8381a7 13 ------------------------------ DEFINE MBED CONNECTIONS ------------------------------
Jellehierck 15:421d3d9c563b 14 */
IsaRobin 0:6972d0e91af1 15
Jellehierck 38:8b597ab8344f 16 // PC connections
Jellehierck 40:c6dffb676350 17 HIDScope scope( 6 );
Jellehierck 15:421d3d9c563b 18 MODSERIAL pc(USBTX, USBRX);
IsaRobin 0:6972d0e91af1 19
Jellehierck 8:ea3de43c9e8b 20 // Buttons
Jellehierck 8:ea3de43c9e8b 21 InterruptIn button1(D11);
Jellehierck 8:ea3de43c9e8b 22 InterruptIn button2(D10);
Jellehierck 37:806c7c8381a7 23 InterruptIn switch2(SW2);
Jellehierck 37:806c7c8381a7 24 InterruptIn switch3(SW3);
Jellehierck 4:09a01d2db8f7 25
Jellehierck 38:8b597ab8344f 26 // LEDs
Jellehierck 38:8b597ab8344f 27 DigitalOut led_g(LED_GREEN);
Jellehierck 38:8b597ab8344f 28 DigitalOut led_r(LED_RED);
Jellehierck 38:8b597ab8344f 29 DigitalOut led_b(LED_BLUE);
Jellehierck 38:8b597ab8344f 30
Jellehierck 38:8b597ab8344f 31 // Analog EMG inputs
Jellehierck 40:c6dffb676350 32 AnalogIn emg1_in (A0); // Right biceps -> x axis
Jellehierck 40:c6dffb676350 33 AnalogIn emg2_in (A1); // Left biceps -> y axis
Jellehierck 40:c6dffb676350 34 AnalogIn emg3_in (A2); // Third muscle -> TBD
Jellehierck 40:c6dffb676350 35
Jellehierck 40:c6dffb676350 36 // Encoder inputs
Jellehierck 40:c6dffb676350 37 DigitalIn encA1(D9);
Jellehierck 40:c6dffb676350 38 DigitalIn encB1(D8);
Jellehierck 40:c6dffb676350 39 DigitalIn encA2(D13);
Jellehierck 40:c6dffb676350 40 DigitalIn encB2(D12);
Jellehierck 40:c6dffb676350 41
Jellehierck 40:c6dffb676350 42 // Motor outputs
Jellehierck 40:c6dffb676350 43 DigitalOut motor2Direction(D4);
Jellehierck 40:c6dffb676350 44 FastPWM motor2Power(D5);
Jellehierck 40:c6dffb676350 45 DigitalOut motor1Direction(D7);
Jellehierck 40:c6dffb676350 46 FastPWM motor1Power(D6);
Jellehierck 38:8b597ab8344f 47
Jellehierck 15:421d3d9c563b 48 /*
Jellehierck 38:8b597ab8344f 49 ------------------------------ INITIALIZE TICKERS, TIMERS & TIMEOUTS ------------------------------
Jellehierck 38:8b597ab8344f 50 */
Jellehierck 38:8b597ab8344f 51 Ticker tickGlobal; // Set global ticker
Jellehierck 38:8b597ab8344f 52 Timer timerCalibration; // Set EMG Calibration timer
Jellehierck 38:8b597ab8344f 53
Jellehierck 38:8b597ab8344f 54 /*
Jellehierck 38:8b597ab8344f 55 ------------------------------ INITIALIZE GLOBAL VARIABLES ------------------------------
Jellehierck 15:421d3d9c563b 56 */
Jellehierck 15:421d3d9c563b 57
Jellehierck 37:806c7c8381a7 58 // State machine variables
Jellehierck 38:8b597ab8344f 59 enum GLOBAL_States { global_failure, global_wait, global_emg_cal, global_motor_cal, global_operation, global_demo }; // Define global states
Jellehierck 37:806c7c8381a7 60 GLOBAL_States global_curr_state = global_wait; // Initialize global state to waiting state
Jellehierck 37:806c7c8381a7 61 bool global_state_changed = true; // Enable entry functions
Jellehierck 37:806c7c8381a7 62 bool failure_mode = false;
Jellehierck 35:e82834e62e44 63
Jellehierck 38:8b597ab8344f 64 bool emg_cal_done = false;
Jellehierck 38:8b597ab8344f 65 bool motor_cal_done = false;
Jellehierck 38:8b597ab8344f 66
Jellehierck 38:8b597ab8344f 67 // EMG Substate variables
Jellehierck 38:8b597ab8344f 68 enum EMG_States { emg_wait, emg_cal_MVC, emg_cal_rest, emg_operation }; // Define EMG substates
Jellehierck 38:8b597ab8344f 69 EMG_States emg_curr_state = emg_wait; // Initialize EMG substate variable
Jellehierck 38:8b597ab8344f 70 bool emg_state_changed = true;
Jellehierck 38:8b597ab8344f 71
Jellehierck 38:8b597ab8344f 72 bool emg_sampleNow = false;
Jellehierck 38:8b597ab8344f 73 bool emg_calibrateNow = false;
Jellehierck 38:8b597ab8344f 74 bool emg_MVC_cal_done = false;
Jellehierck 38:8b597ab8344f 75 bool emg_rest_cal_done = false;
Jellehierck 8:ea3de43c9e8b 76
Jellehierck 40:c6dffb676350 77 // Motor Substate variables
Jellehierck 40:c6dffb676350 78 enum Motor_States {motor_wait, motor_encoderset, motor_finish, motor_movement, failuremode};
Jellehierck 40:c6dffb676350 79 Motor_States motor_curr_state = motor_wait;
Jellehierck 40:c6dffb676350 80 bool motor_state_changed = true;
Jellehierck 40:c6dffb676350 81
Jellehierck 40:c6dffb676350 82 bool motor_calibration_done = false;
Jellehierck 40:c6dffb676350 83
Jellehierck 37:806c7c8381a7 84 // Button press interrupts (to prevent bounce)
Jellehierck 37:806c7c8381a7 85 bool button1_pressed = false;
Jellehierck 37:806c7c8381a7 86 bool button2_pressed = false;
Jellehierck 37:806c7c8381a7 87 bool switch2_pressed = false;
Jellehierck 7:7a088536f1c9 88
Jellehierck 38:8b597ab8344f 89 // Global constants
Jellehierck 38:8b597ab8344f 90 const double Fs = 500.0;
Jellehierck 38:8b597ab8344f 91 const double Ts = 1/Fs;
Jellehierck 35:e82834e62e44 92
Jellehierck 35:e82834e62e44 93 /*
Jellehierck 37:806c7c8381a7 94 ------------------------------ HELPER FUNCTIONS ------------------------------
Jellehierck 37:806c7c8381a7 95 */
Jellehierck 38:8b597ab8344f 96 // Empty placeholder function, needs to be deleted at end of project
Jellehierck 38:8b597ab8344f 97 void doStuff() {}
Jellehierck 38:8b597ab8344f 98
Jellehierck 38:8b597ab8344f 99 // Return max value of vector
Jellehierck 38:8b597ab8344f 100 double getMax(const vector<double> &vect)
Jellehierck 38:8b597ab8344f 101 {
Jellehierck 38:8b597ab8344f 102 double curr_max = 0.0;
Jellehierck 38:8b597ab8344f 103 int vect_n = vect.size();
Jellehierck 38:8b597ab8344f 104 for (int i = 0; i < vect_n; i++) {
Jellehierck 38:8b597ab8344f 105 if (vect[i] > curr_max) {
Jellehierck 38:8b597ab8344f 106 curr_max = vect[i];
Jellehierck 38:8b597ab8344f 107 };
Jellehierck 38:8b597ab8344f 108 }
Jellehierck 38:8b597ab8344f 109 return curr_max;
Jellehierck 38:8b597ab8344f 110 }
Jellehierck 37:806c7c8381a7 111
Jellehierck 38:8b597ab8344f 112 // Return mean of vector
Jellehierck 38:8b597ab8344f 113 double getMean(const vector<double> &vect)
Jellehierck 38:8b597ab8344f 114 {
Jellehierck 38:8b597ab8344f 115 double sum = 0.0;
Jellehierck 38:8b597ab8344f 116 int vect_n = vect.size();
Jellehierck 38:8b597ab8344f 117 for ( int i = 0; i < vect_n; i++ ) {
Jellehierck 38:8b597ab8344f 118 sum += vect[i];
Jellehierck 38:8b597ab8344f 119 }
Jellehierck 38:8b597ab8344f 120 return sum/vect_n;
Jellehierck 38:8b597ab8344f 121 }
Jellehierck 37:806c7c8381a7 122
Jellehierck 38:8b597ab8344f 123 // Return standard deviation of vector
Jellehierck 38:8b597ab8344f 124 double getStdev(const vector<double> &vect, const double vect_mean)
Jellehierck 38:8b597ab8344f 125 {
Jellehierck 38:8b597ab8344f 126 double sum2 = 0.0;
Jellehierck 38:8b597ab8344f 127 int vect_n = vect.size();
Jellehierck 38:8b597ab8344f 128 for ( int i = 0; i < vect_n; i++ ) {
Jellehierck 38:8b597ab8344f 129 sum2 += pow( vect[i] - vect_mean, 2 );
Jellehierck 38:8b597ab8344f 130 }
Jellehierck 38:8b597ab8344f 131 double output = sqrt( sum2 / vect_n );
Jellehierck 38:8b597ab8344f 132 return output;
Jellehierck 38:8b597ab8344f 133 }
Jellehierck 38:8b597ab8344f 134
Jellehierck 38:8b597ab8344f 135 // Rescale double values to certain range
Jellehierck 38:8b597ab8344f 136 double rescale(double input, double out_min, double out_max, double in_min, double in_max)
Jellehierck 38:8b597ab8344f 137 {
Jellehierck 38:8b597ab8344f 138 double output = out_min + ((input-in_min)/(in_max-in_min))*(out_max-out_min); // Based on MATLAB rescale function
Jellehierck 38:8b597ab8344f 139 return output;
Jellehierck 38:8b597ab8344f 140 }
Jellehierck 37:806c7c8381a7 141
Jellehierck 37:806c7c8381a7 142 /*
Jellehierck 37:806c7c8381a7 143 ------------------------------ BUTTON FUNCTIONS ------------------------------
Jellehierck 35:e82834e62e44 144 */
Jellehierck 35:e82834e62e44 145
Jellehierck 25:a1be4cf2ab0b 146 // Handle button press
Jellehierck 25:a1be4cf2ab0b 147 void button1Press()
Jellehierck 25:a1be4cf2ab0b 148 {
Jellehierck 25:a1be4cf2ab0b 149 button1_pressed = true;
Jellehierck 25:a1be4cf2ab0b 150 }
Jellehierck 25:a1be4cf2ab0b 151
Jellehierck 25:a1be4cf2ab0b 152 // Handle button press
Jellehierck 25:a1be4cf2ab0b 153 void button2Press()
Jellehierck 25:a1be4cf2ab0b 154 {
Jellehierck 25:a1be4cf2ab0b 155 button2_pressed = true;
Jellehierck 25:a1be4cf2ab0b 156 }
Jellehierck 25:a1be4cf2ab0b 157
Jellehierck 37:806c7c8381a7 158 void switch2Press()
Jellehierck 6:5437cc97e1e6 159 {
Jellehierck 37:806c7c8381a7 160 switch2_pressed = true;
Jellehierck 35:e82834e62e44 161 }
Jellehierck 6:5437cc97e1e6 162
Jellehierck 37:806c7c8381a7 163 void switch3Press()
Jellehierck 35:e82834e62e44 164 {
Jellehierck 37:806c7c8381a7 165 global_curr_state = global_failure;
Jellehierck 37:806c7c8381a7 166 global_state_changed = true;
Jellehierck 6:5437cc97e1e6 167 }
Jellehierck 6:5437cc97e1e6 168
Jellehierck 15:421d3d9c563b 169 /*
Jellehierck 40:c6dffb676350 170 ------------------------------ MOTOR CONTROL ------------------------------
Jellehierck 40:c6dffb676350 171 */
Jellehierck 40:c6dffb676350 172 // Initialize encoder
Jellehierck 40:c6dffb676350 173 QEI encoder1(D9,D8,NC,64,QEI::X4_ENCODING); //Encoding motor 1
Jellehierck 40:c6dffb676350 174 QEI encoder2(D13,D12,NC,64,QEI::X4_ENCODING); //Encoding motor 2
Jellehierck 40:c6dffb676350 175
Jellehierck 40:c6dffb676350 176 volatile int counts1; // Encoder counts
Jellehierck 40:c6dffb676350 177 volatile int counts2;
Jellehierck 40:c6dffb676350 178 volatile int counts1af;
Jellehierck 40:c6dffb676350 179 volatile int counts2af;
Jellehierck 40:c6dffb676350 180 int counts1offset;
Jellehierck 40:c6dffb676350 181 int counts2offset;
Jellehierck 40:c6dffb676350 182 volatile int countsPrev1 = 0;
Jellehierck 40:c6dffb676350 183 volatile int countsPrev2 = 0;
Jellehierck 40:c6dffb676350 184 volatile int deltaCounts1;
Jellehierck 40:c6dffb676350 185 volatile int deltaCounts2;
Jellehierck 40:c6dffb676350 186
Jellehierck 40:c6dffb676350 187 // PWM period
Jellehierck 40:c6dffb676350 188 const float PWM_period = 1/(18*10e3);
Jellehierck 40:c6dffb676350 189
Jellehierck 40:c6dffb676350 190 // Degree to radian convertor
Jellehierck 40:c6dffb676350 191 const float deg2rad = 0.0174532; //Conversion factor degree to rad
Jellehierck 40:c6dffb676350 192 const float rad2deg = 57.29578; //Conversion factor rad to degree
Jellehierck 40:c6dffb676350 193
Jellehierck 40:c6dffb676350 194 // Motor angles in starting position
Jellehierck 40:c6dffb676350 195 float motor1angle=(-140.0-10.0)*deg2rad*5.5; //Measured angle motor 1
Jellehierck 40:c6dffb676350 196 float motor2angle=(-10.0)*deg2rad*2.75; //Measured angle motor 2
Jellehierck 40:c6dffb676350 197
Jellehierck 40:c6dffb676350 198 const float factorin = 6.23185/64; // Convert encoder counts to angle in rad
Jellehierck 40:c6dffb676350 199 const float gearratio = 131.25; // Gear ratio of gearbox
Jellehierck 40:c6dffb676350 200
Jellehierck 40:c6dffb676350 201 // GLobal variales
Jellehierck 40:c6dffb676350 202 float motor1offset; //Offset bij calibratie
Jellehierck 40:c6dffb676350 203 float motor2offset;
Jellehierck 40:c6dffb676350 204 float omega1; //velocity rad/s motor 1
Jellehierck 40:c6dffb676350 205 float omega2; //Velocity rad/s motor2
Jellehierck 40:c6dffb676350 206 bool motordir1;
Jellehierck 40:c6dffb676350 207 bool motordir2;
Jellehierck 40:c6dffb676350 208 float motor1ref=(-140.0-10.0)*deg2rad*5.5;
Jellehierck 40:c6dffb676350 209 float motor2ref=(-10.0)*deg2rad*2.75;
Jellehierck 40:c6dffb676350 210 double controlsignal1;
Jellehierck 40:c6dffb676350 211 double controlsignal2;
Jellehierck 40:c6dffb676350 212 double pi2= 6.283185;
Jellehierck 40:c6dffb676350 213 float motor1error; //motor 1 error
Jellehierck 40:c6dffb676350 214 float motor2error;
Jellehierck 40:c6dffb676350 215 float Kp=0.27;
Jellehierck 40:c6dffb676350 216 float Ki=0.35;
Jellehierck 40:c6dffb676350 217 float Kd=0.1;
Jellehierck 40:c6dffb676350 218 float u_p1;
Jellehierck 40:c6dffb676350 219 float u_p2;
Jellehierck 40:c6dffb676350 220 float u_i1;
Jellehierck 40:c6dffb676350 221 float u_i2;
Jellehierck 40:c6dffb676350 222
Jellehierck 40:c6dffb676350 223 //Windup control
Jellehierck 40:c6dffb676350 224 float ux1;
Jellehierck 40:c6dffb676350 225 float ux2;
Jellehierck 40:c6dffb676350 226 float up1; //Proportional contribution motor 1
Jellehierck 40:c6dffb676350 227 float up2; //Proportional contribution motor 2
Jellehierck 40:c6dffb676350 228 float ek1;
Jellehierck 40:c6dffb676350 229 float ek2;
Jellehierck 40:c6dffb676350 230 float ei1= 0.0; //Error integral motor 1
Jellehierck 40:c6dffb676350 231 float ei2=0.0; //Error integral motor 2
Jellehierck 40:c6dffb676350 232 float Ka= 1.0; //Integral windup gain
Jellehierck 40:c6dffb676350 233
Jellehierck 40:c6dffb676350 234 //RKI
Jellehierck 40:c6dffb676350 235 float Vx=0.0; //Desired linear velocity x direction
Jellehierck 40:c6dffb676350 236 float Vy=0.0; //Desired linear velocity y direction
Jellehierck 40:c6dffb676350 237 float q1=-10.0f*deg2rad; //Angle of first joint [rad]
Jellehierck 40:c6dffb676350 238 float q2=-140.0f*deg2rad; //Angle of second joint [rad]
Jellehierck 40:c6dffb676350 239 float q1dot; //Velocity of first joint [rad/s]
Jellehierck 40:c6dffb676350 240 float q2dot; //Velocity of second joint [rad/s]
Jellehierck 40:c6dffb676350 241 float l1=26.0; //Distance base-link [cm]
Jellehierck 40:c6dffb676350 242 float l2=62.0; //Distance link-endpoint [cm]
Jellehierck 40:c6dffb676350 243 float xe; //Endpoint x position [cm]
Jellehierck 40:c6dffb676350 244 float ye; //Endpoint y position [cm]
Jellehierck 40:c6dffb676350 245
Jellehierck 40:c6dffb676350 246 /*
Jellehierck 40:c6dffb676350 247 ------------------------------ MOTOR FUNCTIONS ------------------------------
Jellehierck 40:c6dffb676350 248 */
Jellehierck 40:c6dffb676350 249 void PID_controller()
Jellehierck 40:c6dffb676350 250 {
Jellehierck 40:c6dffb676350 251
Jellehierck 40:c6dffb676350 252 static float error_integral1=0;
Jellehierck 40:c6dffb676350 253 static float e_prev1=motor1error;
Jellehierck 40:c6dffb676350 254
Jellehierck 40:c6dffb676350 255 //Proportional part:
Jellehierck 40:c6dffb676350 256 u_p1=Kp*motor1error;
Jellehierck 40:c6dffb676350 257
Jellehierck 40:c6dffb676350 258 //Integral part
Jellehierck 40:c6dffb676350 259 error_integral1=error_integral1+ei1*Ts;
Jellehierck 40:c6dffb676350 260 u_i1=Ki*error_integral1;
Jellehierck 40:c6dffb676350 261
Jellehierck 40:c6dffb676350 262 //Derivative part
Jellehierck 40:c6dffb676350 263 float error_derivative1=(motor1error-e_prev1)/Ts;
Jellehierck 40:c6dffb676350 264 float u_d1=Kd*error_derivative1;
Jellehierck 40:c6dffb676350 265 e_prev1=motor1error;
Jellehierck 40:c6dffb676350 266
Jellehierck 40:c6dffb676350 267 // Sum and limit
Jellehierck 40:c6dffb676350 268 up1= u_p1+u_i1+u_d1;
Jellehierck 40:c6dffb676350 269 if (up1>1) {
Jellehierck 40:c6dffb676350 270 controlsignal1=1;
Jellehierck 40:c6dffb676350 271 } else if (up1<-1) {
Jellehierck 40:c6dffb676350 272 controlsignal1=-1;
Jellehierck 40:c6dffb676350 273 } else {
Jellehierck 40:c6dffb676350 274 controlsignal1=up1;
Jellehierck 40:c6dffb676350 275 }
Jellehierck 40:c6dffb676350 276
Jellehierck 40:c6dffb676350 277 // To prevent windup
Jellehierck 40:c6dffb676350 278 ux1= up1-controlsignal1;
Jellehierck 40:c6dffb676350 279 ek1= Ka*ux1;
Jellehierck 40:c6dffb676350 280 ei1= motor1error-ek1;
Jellehierck 40:c6dffb676350 281
Jellehierck 40:c6dffb676350 282 // Motor 2
Jellehierck 40:c6dffb676350 283
Jellehierck 40:c6dffb676350 284 static float error_integral2=0;
Jellehierck 40:c6dffb676350 285 static float e_prev2=motor2error;
Jellehierck 40:c6dffb676350 286
Jellehierck 40:c6dffb676350 287 //Proportional part:
Jellehierck 40:c6dffb676350 288 u_p2=Kp*motor2error;
Jellehierck 40:c6dffb676350 289
Jellehierck 40:c6dffb676350 290 //Integral part
Jellehierck 40:c6dffb676350 291 error_integral2=error_integral2+ei2*Ts;
Jellehierck 40:c6dffb676350 292 u_i2=Ki*error_integral2;
Jellehierck 40:c6dffb676350 293
Jellehierck 40:c6dffb676350 294 //Derivative part
Jellehierck 40:c6dffb676350 295 float error_derivative2=(motor2error-e_prev2)/Ts;
Jellehierck 40:c6dffb676350 296 float u_d2=Kd*error_derivative2;
Jellehierck 40:c6dffb676350 297 e_prev2=motor2error;
Jellehierck 40:c6dffb676350 298
Jellehierck 40:c6dffb676350 299 // Sum and limit
Jellehierck 40:c6dffb676350 300 up2= u_p2+u_i2+u_d2;
Jellehierck 40:c6dffb676350 301 if (up2>1.0f) {
Jellehierck 40:c6dffb676350 302 controlsignal2=1.0f;
Jellehierck 40:c6dffb676350 303 } else if (up2<-1) {
Jellehierck 40:c6dffb676350 304 controlsignal2=-1.0f;
Jellehierck 40:c6dffb676350 305 } else {
Jellehierck 40:c6dffb676350 306 controlsignal2=up2;
Jellehierck 40:c6dffb676350 307 }
Jellehierck 40:c6dffb676350 308
Jellehierck 40:c6dffb676350 309 // To prevent windup
Jellehierck 40:c6dffb676350 310 ux2= up2-controlsignal2;
Jellehierck 40:c6dffb676350 311 ek2= Ka*ux2;
Jellehierck 40:c6dffb676350 312 ei2= motor2error-ek2;
Jellehierck 40:c6dffb676350 313 }
Jellehierck 40:c6dffb676350 314
Jellehierck 40:c6dffb676350 315 void RKI()
Jellehierck 40:c6dffb676350 316 {
Jellehierck 40:c6dffb676350 317 q1dot=(l2*cos(q1+q2)*Vx+l2*sin(q1+q2)*Vy)/((-l1*sin(q1)-l2*sin(q1+q2))*l2*cos(q1+q2)+l2*sin(q1+q2)*(l1*cos(q1)+l2*cos(q1+q2)));
Jellehierck 40:c6dffb676350 318 q2dot=((-l1*cos(q1)-l2*cos(q1+q2))*Vx+(-l1*sin(q1)-l2*sin(q1+q2))*Vy)/((-l1*sin(q1)-l2*sin(q1+q2))*l2*cos(q1+q2)+l2*sin(q1+q2)*(l1*cos(q1)+l2*cos(q1+q2)));
Jellehierck 40:c6dffb676350 319 q1=q1+q1dot*Ts;
Jellehierck 40:c6dffb676350 320 q2=q2+q2dot*Ts;
Jellehierck 40:c6dffb676350 321
Jellehierck 40:c6dffb676350 322 xe=l1*cos(q1)+l2*cos(q1+q2);
Jellehierck 40:c6dffb676350 323 ye=l1*sin(q1)+l2*sin(q1+q2);
Jellehierck 40:c6dffb676350 324
Jellehierck 40:c6dffb676350 325
Jellehierck 40:c6dffb676350 326
Jellehierck 40:c6dffb676350 327 if (q1<-5.0f) {
Jellehierck 40:c6dffb676350 328 q1=-5.0;
Jellehierck 40:c6dffb676350 329 } else if (q1>65.0f*deg2rad) {
Jellehierck 40:c6dffb676350 330 q1=65.0f*deg2rad;
Jellehierck 40:c6dffb676350 331 } else {
Jellehierck 40:c6dffb676350 332 q1=q1;
Jellehierck 40:c6dffb676350 333 }
Jellehierck 40:c6dffb676350 334
Jellehierck 40:c6dffb676350 335 if (q2>-50.0*deg2rad) {
Jellehierck 40:c6dffb676350 336 q2=-50.0*deg2rad;
Jellehierck 40:c6dffb676350 337 } else if (q2<-138.0*deg2rad) {
Jellehierck 40:c6dffb676350 338 q2=-138.0*deg2rad;
Jellehierck 40:c6dffb676350 339 } else {
Jellehierck 40:c6dffb676350 340 q2=q2;
Jellehierck 40:c6dffb676350 341 }
Jellehierck 40:c6dffb676350 342
Jellehierck 40:c6dffb676350 343 motor1ref=5.5f*q1+5.5f*q2;
Jellehierck 40:c6dffb676350 344 motor2ref=2.75f*q1;
Jellehierck 40:c6dffb676350 345 }
Jellehierck 40:c6dffb676350 346
Jellehierck 40:c6dffb676350 347 void motorControl()
Jellehierck 40:c6dffb676350 348 {
Jellehierck 40:c6dffb676350 349 counts1= counts1af-counts1offset;
Jellehierck 40:c6dffb676350 350 motor1angle = (counts1 * factorin / gearratio)-((140.0*5.5*deg2rad)+(10.0*5.5*deg2rad)); // Angle of motor shaft in rad + correctie voor q1 en q2
Jellehierck 40:c6dffb676350 351 omega1 = deltaCounts1 / Ts * factorin / gearratio; // Angular velocity of motor shaft in rad/s
Jellehierck 40:c6dffb676350 352 motor1error=motor1ref-motor1angle;
Jellehierck 40:c6dffb676350 353 if (controlsignal1<0) {
Jellehierck 40:c6dffb676350 354 motordir1= 0;
Jellehierck 40:c6dffb676350 355 } else {
Jellehierck 40:c6dffb676350 356 motordir1= 1;
Jellehierck 40:c6dffb676350 357 }
Jellehierck 40:c6dffb676350 358
Jellehierck 40:c6dffb676350 359 motor1Power.write(abs(controlsignal1));
Jellehierck 40:c6dffb676350 360 motor1Direction= motordir1;
Jellehierck 40:c6dffb676350 361
Jellehierck 40:c6dffb676350 362 counts2= counts2af - counts2offset;
Jellehierck 40:c6dffb676350 363 motor2angle = (counts2 * factorin / gearratio)-(10.0*2.75*deg2rad); // Angle of motor shaft in rad + correctie voor q1
Jellehierck 40:c6dffb676350 364 omega2 = deltaCounts2 / Ts * factorin / gearratio; // Angular velocity of motor shaft in rad/s
Jellehierck 40:c6dffb676350 365 motor2error=motor2ref-motor2angle;
Jellehierck 40:c6dffb676350 366 if (controlsignal2<0) {
Jellehierck 40:c6dffb676350 367 motordir2= 0;
Jellehierck 40:c6dffb676350 368 } else {
Jellehierck 40:c6dffb676350 369 motordir2= 1;
Jellehierck 40:c6dffb676350 370 }
Jellehierck 40:c6dffb676350 371 if (motor_calibration_done == true) {
Jellehierck 40:c6dffb676350 372 motor2Power.write(abs(controlsignal2));
Jellehierck 40:c6dffb676350 373 }
Jellehierck 40:c6dffb676350 374 motor2Direction= motordir2;
Jellehierck 40:c6dffb676350 375 }
Jellehierck 40:c6dffb676350 376
Jellehierck 40:c6dffb676350 377 /*
Jellehierck 40:c6dffb676350 378 ------------------------------ MOTOR SUBSTATE FUNCTIONS ------------------------------
Jellehierck 40:c6dffb676350 379 */
Jellehierck 40:c6dffb676350 380
Jellehierck 40:c6dffb676350 381 void do_motor_wait()
Jellehierck 40:c6dffb676350 382 {
Jellehierck 40:c6dffb676350 383 // Entry function
Jellehierck 40:c6dffb676350 384 if ( motor_state_changed == true ) {
Jellehierck 40:c6dffb676350 385 motor_state_changed = false;
Jellehierck 40:c6dffb676350 386 // More functions
Jellehierck 40:c6dffb676350 387 }
Jellehierck 40:c6dffb676350 388
Jellehierck 40:c6dffb676350 389 PID_controller();
Jellehierck 40:c6dffb676350 390 motorControl();
Jellehierck 40:c6dffb676350 391
Jellehierck 40:c6dffb676350 392 // State transition guard
Jellehierck 40:c6dffb676350 393 if ( button2_pressed ) {
Jellehierck 40:c6dffb676350 394 button2_pressed = false;
Jellehierck 40:c6dffb676350 395 motor_curr_state = motor_encoderset; //Beginnen met calibratie
Jellehierck 40:c6dffb676350 396 motor_state_changed = true;
Jellehierck 40:c6dffb676350 397 // More functions
Jellehierck 40:c6dffb676350 398 }
Jellehierck 40:c6dffb676350 399
Jellehierck 40:c6dffb676350 400 }
Jellehierck 40:c6dffb676350 401
Jellehierck 40:c6dffb676350 402 void do_motor_Encoder_Set()
Jellehierck 40:c6dffb676350 403 {
Jellehierck 40:c6dffb676350 404 // Entry function
Jellehierck 40:c6dffb676350 405 if ( motor_state_changed == true ) {
Jellehierck 40:c6dffb676350 406 motor_state_changed = false;
Jellehierck 40:c6dffb676350 407 // More functions
Jellehierck 40:c6dffb676350 408 }
Jellehierck 40:c6dffb676350 409 motor1Power.write(0.0f);
Jellehierck 40:c6dffb676350 410 motor2Power.write(0.0f);
Jellehierck 40:c6dffb676350 411 counts1offset = counts1af ;
Jellehierck 40:c6dffb676350 412 counts2offset = counts2af;
Jellehierck 40:c6dffb676350 413
Jellehierck 40:c6dffb676350 414 // State transition guard
Jellehierck 40:c6dffb676350 415 if ( button2_pressed ) {
Jellehierck 40:c6dffb676350 416 button2_pressed = false;
Jellehierck 40:c6dffb676350 417 motor_calibration_done = true;
Jellehierck 40:c6dffb676350 418 motor_curr_state = motor_finish;
Jellehierck 40:c6dffb676350 419 motor_state_changed = true;
Jellehierck 40:c6dffb676350 420 }
Jellehierck 40:c6dffb676350 421 }
Jellehierck 40:c6dffb676350 422
Jellehierck 40:c6dffb676350 423 void do_motor_finish()
Jellehierck 40:c6dffb676350 424 {
Jellehierck 40:c6dffb676350 425 // Entry function
Jellehierck 40:c6dffb676350 426 if ( motor_state_changed == true ) {
Jellehierck 40:c6dffb676350 427 motor_state_changed = false;
Jellehierck 40:c6dffb676350 428 }
Jellehierck 40:c6dffb676350 429
Jellehierck 40:c6dffb676350 430 // Do stuff until end condition is true
Jellehierck 40:c6dffb676350 431 PID_controller();
Jellehierck 40:c6dffb676350 432 motorControl();
Jellehierck 40:c6dffb676350 433 RKI();
Jellehierck 40:c6dffb676350 434
Jellehierck 40:c6dffb676350 435 // State transition guard
Jellehierck 40:c6dffb676350 436 if ( button2_pressed ) {
Jellehierck 40:c6dffb676350 437 button2_pressed = false;
Jellehierck 40:c6dffb676350 438 motor_cal_done = true;
Jellehierck 40:c6dffb676350 439 motor_curr_state = motor_wait;
Jellehierck 40:c6dffb676350 440 motor_state_changed = true;
Jellehierck 40:c6dffb676350 441 }
Jellehierck 40:c6dffb676350 442
Jellehierck 40:c6dffb676350 443 }
Jellehierck 40:c6dffb676350 444
Jellehierck 40:c6dffb676350 445 /*
Jellehierck 38:8b597ab8344f 446 ------------------------------ EMG GLOBAL VARIABLES & CONSTANTS ------------------------------
Jellehierck 38:8b597ab8344f 447 */
Jellehierck 38:8b597ab8344f 448
Jellehierck 38:8b597ab8344f 449 // Set global constant values for EMG reading & analysis
Jellehierck 41:8e8141f355af 450 const double Tcal = 7.5f; // Calibration duration (s)
Jellehierck 38:8b597ab8344f 451
Jellehierck 38:8b597ab8344f 452 // Initialize variables for EMG reading & analysis
Jellehierck 38:8b597ab8344f 453 double emg1;
Jellehierck 38:8b597ab8344f 454 double emg1_env;
Jellehierck 38:8b597ab8344f 455 double emg1_MVC;
Jellehierck 38:8b597ab8344f 456 double emg1_rest;
Jellehierck 38:8b597ab8344f 457 double emg1_factor;//delete
Jellehierck 38:8b597ab8344f 458 double emg1_th;
Jellehierck 38:8b597ab8344f 459 double emg1_out;
Jellehierck 38:8b597ab8344f 460 double emg1_norm; //delete
Jellehierck 38:8b597ab8344f 461 vector<double> emg1_cal;
Jellehierck 38:8b597ab8344f 462 int emg1_cal_size; //delete
Jellehierck 41:8e8141f355af 463 double emg1_dir = 1.0;
Jellehierck 38:8b597ab8344f 464 double emg1_out_prev;
Jellehierck 38:8b597ab8344f 465 double emg1_dt; //delete
Jellehierck 38:8b597ab8344f 466 double emg1_dt_prev;
Jellehierck 38:8b597ab8344f 467 double emg1_dtdt; //delete
Jellehierck 38:8b597ab8344f 468
Jellehierck 38:8b597ab8344f 469 double emg2;
Jellehierck 38:8b597ab8344f 470 double emg2_env;
Jellehierck 38:8b597ab8344f 471 double emg2_MVC;
Jellehierck 38:8b597ab8344f 472 double emg2_rest;
Jellehierck 38:8b597ab8344f 473 double emg2_factor;//delete
Jellehierck 38:8b597ab8344f 474 double emg2_th;
Jellehierck 38:8b597ab8344f 475 double emg2_out;
Jellehierck 38:8b597ab8344f 476 double emg2_norm;//delete
Jellehierck 38:8b597ab8344f 477 vector<double> emg2_cal;
Jellehierck 38:8b597ab8344f 478 int emg2_cal_size;//delete
Jellehierck 41:8e8141f355af 479 double emg2_dir = 1.0;
Jellehierck 38:8b597ab8344f 480
Jellehierck 38:8b597ab8344f 481 double emg3;
Jellehierck 38:8b597ab8344f 482 double emg3_env;
Jellehierck 38:8b597ab8344f 483 double emg3_MVC;
Jellehierck 38:8b597ab8344f 484 double emg3_rest;
Jellehierck 38:8b597ab8344f 485 double emg3_factor;//delete
Jellehierck 38:8b597ab8344f 486 double emg3_th;
Jellehierck 38:8b597ab8344f 487 double emg3_out;
Jellehierck 38:8b597ab8344f 488 double emg3_norm;//delete
Jellehierck 38:8b597ab8344f 489 vector<double> emg3_cal;
Jellehierck 38:8b597ab8344f 490 int emg3_cal_size;//delete
Jellehierck 38:8b597ab8344f 491 int emg3_dir = 1;
Jellehierck 38:8b597ab8344f 492
Jellehierck 38:8b597ab8344f 493 /*
Jellehierck 38:8b597ab8344f 494 ------------------------------ EMG FILTERS ------------------------------
Jellehierck 38:8b597ab8344f 495 */
Jellehierck 38:8b597ab8344f 496
Jellehierck 38:8b597ab8344f 497 // Notch biquad filter coefficients (iirnotch Q factor 35 @50Hz) from MATLAB:
Jellehierck 38:8b597ab8344f 498 BiQuad bq1_notch( 0.995636295063941, -1.89829218816065, 0.995636295063941, 1, -1.89829218816065, 0.991272590127882); // b01 b11 b21 a01 a11 a21
Jellehierck 38:8b597ab8344f 499 BiQuad bq2_notch = bq1_notch;
Jellehierck 38:8b597ab8344f 500 BiQuad bq3_notch = bq1_notch;
Jellehierck 38:8b597ab8344f 501 BiQuadChain bqc1_notch;
Jellehierck 38:8b597ab8344f 502 BiQuadChain bqc2_notch;
Jellehierck 38:8b597ab8344f 503 BiQuadChain bqc3_notch;
Jellehierck 38:8b597ab8344f 504
Jellehierck 38:8b597ab8344f 505 // Highpass biquad filter coefficients (butter 4th order @10Hz cutoff) from MATLAB
Jellehierck 38:8b597ab8344f 506 BiQuad bq1_H1(0.922946103200875, -1.84589220640175, 0.922946103200875, 1, -1.88920703055163, 0.892769008131025); // b01 b11 b21 a01 a11 a21
Jellehierck 38:8b597ab8344f 507 BiQuad bq1_H2(1, -2, 1, 1, -1.95046575793011, 0.954143234875078); // b02 b12 b22 a02 a12 a22
Jellehierck 38:8b597ab8344f 508 BiQuad bq2_H1 = bq1_H1;
Jellehierck 38:8b597ab8344f 509 BiQuad bq2_H2 = bq1_H2;
Jellehierck 38:8b597ab8344f 510 BiQuad bq3_H1 = bq1_H1;
Jellehierck 38:8b597ab8344f 511 BiQuad bq3_H2 = bq1_H2;
Jellehierck 38:8b597ab8344f 512 BiQuadChain bqc1_high;
Jellehierck 38:8b597ab8344f 513 BiQuadChain bqc2_high;
Jellehierck 38:8b597ab8344f 514 BiQuadChain bqc3_high;
Jellehierck 38:8b597ab8344f 515
Jellehierck 38:8b597ab8344f 516 // Lowpass biquad filter coefficients (butter 4th order @5Hz cutoff) from MATLAB:
Jellehierck 38:8b597ab8344f 517 BiQuad bq1_L1(5.32116245737504e-08, 1.06423249147501e-07, 5.32116245737504e-08, 1, -1.94396715039462, 0.944882378004138); // b01 b11 b21 a01 a11 a21
Jellehierck 38:8b597ab8344f 518 BiQuad bq1_L2(1, 2, 1, 1, -1.97586467534468, 0.976794920438162); // b02 b12 b22 a02 a12 a22
Jellehierck 38:8b597ab8344f 519 BiQuad bq2_L1 = bq1_L1;
Jellehierck 38:8b597ab8344f 520 BiQuad bq2_L2 = bq1_L2;
Jellehierck 38:8b597ab8344f 521 BiQuad bq3_L1 = bq1_L1;
Jellehierck 38:8b597ab8344f 522 BiQuad bq3_L2 = bq1_L2;
Jellehierck 38:8b597ab8344f 523 BiQuadChain bqc1_low;
Jellehierck 38:8b597ab8344f 524 BiQuadChain bqc2_low;
Jellehierck 38:8b597ab8344f 525 BiQuadChain bqc3_low;
Jellehierck 38:8b597ab8344f 526
Jellehierck 38:8b597ab8344f 527 // Function to check filter stability
Jellehierck 38:8b597ab8344f 528 bool checkBQChainStable()
Jellehierck 38:8b597ab8344f 529 {
Jellehierck 38:8b597ab8344f 530 bool n_stable = bqc1_notch.stable(); // Check stability of all BQ Chains
Jellehierck 38:8b597ab8344f 531 bool hp_stable = bqc1_high.stable();
Jellehierck 38:8b597ab8344f 532 bool l_stable = bqc1_low.stable();
Jellehierck 38:8b597ab8344f 533
Jellehierck 38:8b597ab8344f 534 if (n_stable && hp_stable && l_stable) {
Jellehierck 38:8b597ab8344f 535 return true;
Jellehierck 38:8b597ab8344f 536 } else {
Jellehierck 38:8b597ab8344f 537 return false;
Jellehierck 38:8b597ab8344f 538 }
Jellehierck 38:8b597ab8344f 539 }
Jellehierck 38:8b597ab8344f 540 /*
Jellehierck 38:8b597ab8344f 541 ------------------------------ EMG SUBSTATE FUNCTIONS ------------------------------
Jellehierck 15:421d3d9c563b 542 */
Jellehierck 38:8b597ab8344f 543
Jellehierck 38:8b597ab8344f 544 // EMG Waiting state
Jellehierck 38:8b597ab8344f 545 void do_emg_wait()
Jellehierck 38:8b597ab8344f 546 {
Jellehierck 38:8b597ab8344f 547 // Entry function
Jellehierck 38:8b597ab8344f 548 if ( emg_state_changed == true ) {
Jellehierck 38:8b597ab8344f 549 emg_state_changed = false; // Disable entry functions
Jellehierck 38:8b597ab8344f 550
Jellehierck 38:8b597ab8344f 551 button1.fall( &button1Press ); // Change to state MVC calibration on button1 press
Jellehierck 38:8b597ab8344f 552 button2.fall( &button2Press ); // Change to state rest calibration on button2 press
Jellehierck 38:8b597ab8344f 553 }
Jellehierck 38:8b597ab8344f 554
Jellehierck 38:8b597ab8344f 555 // Do nothing until end condition is met
Jellehierck 38:8b597ab8344f 556
Jellehierck 38:8b597ab8344f 557 // State transition guard
Jellehierck 38:8b597ab8344f 558 if ( button1_pressed ) { // MVC calibration
Jellehierck 38:8b597ab8344f 559 button1_pressed = false; // Disable button pressed function until next button press
Jellehierck 38:8b597ab8344f 560 button1.fall( NULL ); // Disable interrupt during calibration
Jellehierck 38:8b597ab8344f 561 button2.fall( NULL ); // Disable interrupt during calibration
Jellehierck 38:8b597ab8344f 562 emg_curr_state = emg_cal_MVC; // Set next state
Jellehierck 38:8b597ab8344f 563 emg_state_changed = true; // Enable entry functions
Jellehierck 38:8b597ab8344f 564
Jellehierck 38:8b597ab8344f 565 } else if ( button2_pressed ) { // Rest calibration
Jellehierck 38:8b597ab8344f 566 button2_pressed = false; // Disable button pressed function until next button press
Jellehierck 38:8b597ab8344f 567 button1.fall( NULL ); // Disable interrupt during calibration
Jellehierck 38:8b597ab8344f 568 button2.fall( NULL ); // Disable interrupt during calibration
Jellehierck 38:8b597ab8344f 569 emg_curr_state = emg_cal_rest; // Set next state
Jellehierck 38:8b597ab8344f 570 emg_state_changed = true; // Enable entry functions
Jellehierck 38:8b597ab8344f 571
Jellehierck 38:8b597ab8344f 572 } else if ( emg_MVC_cal_done && emg_rest_cal_done ) { // Operation mode
Jellehierck 38:8b597ab8344f 573 button1.fall( NULL ); // Disable interrupt during operation
Jellehierck 38:8b597ab8344f 574 button2.fall( NULL ); // Disable interrupt during operation
Jellehierck 38:8b597ab8344f 575 emg_curr_state = emg_operation; // Set next state
Jellehierck 38:8b597ab8344f 576 emg_state_changed = true; // Enable entry functions
Jellehierck 38:8b597ab8344f 577 }
Jellehierck 38:8b597ab8344f 578 }
Jellehierck 38:8b597ab8344f 579
Jellehierck 38:8b597ab8344f 580 // EMG Calibration state
Jellehierck 38:8b597ab8344f 581 void do_emg_cal()
Jellehierck 38:8b597ab8344f 582 {
Jellehierck 38:8b597ab8344f 583 // Entry functions
Jellehierck 38:8b597ab8344f 584 if ( emg_state_changed == true ) {
Jellehierck 38:8b597ab8344f 585 emg_state_changed = false; // Disable entry functions
Jellehierck 38:8b597ab8344f 586 led_b = 0; // Turn on calibration led
Jellehierck 38:8b597ab8344f 587
Jellehierck 38:8b597ab8344f 588 timerCalibration.reset();
Jellehierck 38:8b597ab8344f 589 timerCalibration.start(); // Sets up timer to stop calibration after Tcal seconds
Jellehierck 38:8b597ab8344f 590 emg_sampleNow = true; // Enable signal sampling in sampleSignals()
Jellehierck 38:8b597ab8344f 591 emg_calibrateNow = true; // Enable calibration vector functionality in sampleSignals()
Jellehierck 38:8b597ab8344f 592
Jellehierck 38:8b597ab8344f 593 emg1_cal.reserve(Fs * Tcal); // Initialize vector lengths to prevent memory overflow
Jellehierck 38:8b597ab8344f 594 emg2_cal.reserve(Fs * Tcal); // Idem
Jellehierck 38:8b597ab8344f 595 emg3_cal.reserve(Fs * Tcal); // Idem
Jellehierck 38:8b597ab8344f 596 }
Jellehierck 38:8b597ab8344f 597
Jellehierck 38:8b597ab8344f 598 // Do stuff until end condition is met
Jellehierck 38:8b597ab8344f 599 // Set HIDScope outputs
Jellehierck 38:8b597ab8344f 600 scope.set(0, emg1 );
Jellehierck 38:8b597ab8344f 601 scope.set(1, emg1_env );
Jellehierck 41:8e8141f355af 602 scope.set(2, emg2 );
Jellehierck 41:8e8141f355af 603 scope.set(3, emg2_env );
Jellehierck 38:8b597ab8344f 604 //scope.set(2, emg2_env );
Jellehierck 38:8b597ab8344f 605 //scope.set(3, emg3_env );
Jellehierck 38:8b597ab8344f 606 scope.send();
Jellehierck 38:8b597ab8344f 607
Jellehierck 38:8b597ab8344f 608 // State transition guard
Jellehierck 38:8b597ab8344f 609 if ( timerCalibration.read() >= Tcal ) { // After interval Tcal the calibration step is finished
Jellehierck 38:8b597ab8344f 610 emg_sampleNow = false; // Disable signal sampling in sampleSignals()
Jellehierck 38:8b597ab8344f 611 emg_calibrateNow = false; // Disable calibration sampling
Jellehierck 38:8b597ab8344f 612 led_b = 1; // Turn off calibration led
Jellehierck 38:8b597ab8344f 613
Jellehierck 38:8b597ab8344f 614 // Extract EMG scale data from calibration
Jellehierck 38:8b597ab8344f 615 switch( emg_curr_state ) {
Jellehierck 39:f9042483b921 616 case emg_cal_MVC: // In case of MVC calibration
Jellehierck 38:8b597ab8344f 617 emg1_MVC = getMax(emg1_cal); // Store max value of MVC globally
Jellehierck 38:8b597ab8344f 618 emg2_MVC = getMax(emg2_cal);
Jellehierck 38:8b597ab8344f 619 emg3_MVC = getMax(emg3_cal);
Jellehierck 38:8b597ab8344f 620
Jellehierck 38:8b597ab8344f 621 emg_MVC_cal_done = true; // Set up transition to EMG operation mode
Jellehierck 38:8b597ab8344f 622 break;
Jellehierck 39:f9042483b921 623 case emg_cal_rest: // In case of rest calibration
Jellehierck 38:8b597ab8344f 624 emg1_rest = getMean(emg1_cal); // Store mean of EMG in rest globally
Jellehierck 38:8b597ab8344f 625 emg2_rest = getMean(emg2_cal);
Jellehierck 38:8b597ab8344f 626 emg3_rest = getMean(emg3_cal);
Jellehierck 38:8b597ab8344f 627 emg_rest_cal_done = true; // Set up transition to EMG operation mode
Jellehierck 38:8b597ab8344f 628 break;
Jellehierck 38:8b597ab8344f 629 }
Jellehierck 38:8b597ab8344f 630 vector<double>().swap(emg1_cal); // Empty vector to prevent memory overflow
Jellehierck 38:8b597ab8344f 631 vector<double>().swap(emg2_cal);
Jellehierck 38:8b597ab8344f 632 vector<double>().swap(emg3_cal);
Jellehierck 38:8b597ab8344f 633
Jellehierck 38:8b597ab8344f 634 emg_curr_state = emg_wait; // Set next substate
Jellehierck 38:8b597ab8344f 635 emg_state_changed = true; // Enable substate entry function
Jellehierck 38:8b597ab8344f 636 }
Jellehierck 38:8b597ab8344f 637 }
Jellehierck 38:8b597ab8344f 638
Jellehierck 38:8b597ab8344f 639 // EMG Operation state
Jellehierck 38:8b597ab8344f 640 void do_emg_operation()
Jellehierck 38:8b597ab8344f 641 {
Jellehierck 38:8b597ab8344f 642 // Entry function
Jellehierck 38:8b597ab8344f 643 if ( emg_state_changed == true ) {
Jellehierck 38:8b597ab8344f 644 emg_state_changed = false; // Disable entry functions
Jellehierck 40:c6dffb676350 645
Jellehierck 39:f9042483b921 646 // Compute scale factors for all EMG signals
Jellehierck 38:8b597ab8344f 647 double margin_percentage = 5; // Set up % margin for rest
Jellehierck 38:8b597ab8344f 648 emg1_factor = 1 / emg1_MVC; // Factor to normalize MVC
Jellehierck 38:8b597ab8344f 649 emg1_th = emg1_rest * emg1_factor + margin_percentage/100; // Set normalized rest threshold
Jellehierck 38:8b597ab8344f 650 emg2_factor = 1 / emg2_MVC; // Factor to normalize MVC
Jellehierck 38:8b597ab8344f 651 emg2_th = emg2_rest * emg2_factor + margin_percentage/100; // Set normalized rest threshold
Jellehierck 38:8b597ab8344f 652 emg3_factor = 1 / emg3_MVC; // Factor to normalize MVC
Jellehierck 38:8b597ab8344f 653 emg3_th = emg3_rest * emg3_factor + margin_percentage/100; // Set normalized rest threshold
Jellehierck 38:8b597ab8344f 654
Jellehierck 38:8b597ab8344f 655
Jellehierck 38:8b597ab8344f 656 // ------- TO DO: MAKE SURE THESE BUTTONS DO NOT BOUNCE (e.g. with button1.rise() ) ------
Jellehierck 38:8b597ab8344f 657 //button1.fall( &toggleEMG1Dir ); // Change to state MVC calibration on button1 press
Jellehierck 38:8b597ab8344f 658 //button2.fall( &toggleEMG2Dir ); // Change to state rest calibration on button2 press
Jellehierck 38:8b597ab8344f 659
Jellehierck 38:8b597ab8344f 660 }
Jellehierck 40:c6dffb676350 661
Jellehierck 39:f9042483b921 662 // This state only runs its entry functions ONCE and then exits the EMG substate machine
Jellehierck 38:8b597ab8344f 663
Jellehierck 38:8b597ab8344f 664 // State transition guard
Jellehierck 41:8e8141f355af 665 if ( true ) { // EMG substate machine is terminated directly after running this state once
Jellehierck 38:8b597ab8344f 666 emg_curr_state = emg_wait; // Set next state
Jellehierck 38:8b597ab8344f 667 emg_state_changed = true; // Enable entry function
Jellehierck 41:8e8141f355af 668 emg_cal_done = true; // Let the global substate machine know that EMG calibration is finished
Jellehierck 41:8e8141f355af 669
Jellehierck 41:8e8141f355af 670 // Enable buttons again
Jellehierck 41:8e8141f355af 671 button1.fall( &button1Press );
Jellehierck 41:8e8141f355af 672 button2.fall( &button2Press );
Jellehierck 38:8b597ab8344f 673 }
Jellehierck 38:8b597ab8344f 674 }
Jellehierck 38:8b597ab8344f 675
Jellehierck 38:8b597ab8344f 676 /*
Jellehierck 38:8b597ab8344f 677 ------------------------------ EMG SUBSTATE MACHINE ------------------------------
Jellehierck 38:8b597ab8344f 678 */
Jellehierck 38:8b597ab8344f 679
Jellehierck 38:8b597ab8344f 680 void emg_state_machine()
Jellehierck 38:8b597ab8344f 681 {
Jellehierck 38:8b597ab8344f 682 switch(emg_curr_state) {
Jellehierck 38:8b597ab8344f 683 case emg_wait:
Jellehierck 38:8b597ab8344f 684 do_emg_wait();
Jellehierck 38:8b597ab8344f 685 break;
Jellehierck 38:8b597ab8344f 686 case emg_cal_MVC:
Jellehierck 38:8b597ab8344f 687 do_emg_cal();
Jellehierck 38:8b597ab8344f 688 break;
Jellehierck 38:8b597ab8344f 689 case emg_cal_rest:
Jellehierck 38:8b597ab8344f 690 do_emg_cal();
Jellehierck 38:8b597ab8344f 691 break;
Jellehierck 38:8b597ab8344f 692 case emg_operation:
Jellehierck 38:8b597ab8344f 693 do_emg_operation();
Jellehierck 38:8b597ab8344f 694 break;
Jellehierck 38:8b597ab8344f 695 }
Jellehierck 38:8b597ab8344f 696 }
Jellehierck 7:7a088536f1c9 697
Jellehierck 15:421d3d9c563b 698 /*
Jellehierck 40:c6dffb676350 699 ------------------------------ MOTOR SUBSTATE MACHINE ------------------------------
Jellehierck 40:c6dffb676350 700 */
Jellehierck 40:c6dffb676350 701
Jellehierck 40:c6dffb676350 702 void motor_state_machine()
Jellehierck 40:c6dffb676350 703 {
Jellehierck 40:c6dffb676350 704 switch(motor_curr_state) {
Jellehierck 40:c6dffb676350 705 case motor_wait:
Jellehierck 40:c6dffb676350 706 do_motor_wait();
Jellehierck 40:c6dffb676350 707 break;
Jellehierck 40:c6dffb676350 708 case motor_encoderset:
Jellehierck 40:c6dffb676350 709 do_motor_Encoder_Set();
Jellehierck 40:c6dffb676350 710 break;
Jellehierck 40:c6dffb676350 711 case motor_finish:
Jellehierck 40:c6dffb676350 712 do_motor_finish();
Jellehierck 40:c6dffb676350 713 break;
Jellehierck 40:c6dffb676350 714 }
Jellehierck 40:c6dffb676350 715 }
Jellehierck 40:c6dffb676350 716
Jellehierck 40:c6dffb676350 717 /*
Jellehierck 37:806c7c8381a7 718 ------------------------------ GLOBAL STATE FUNCTIONS ------------------------------
Jellehierck 15:421d3d9c563b 719 */
Jellehierck 25:a1be4cf2ab0b 720 /* ALL STATES HAVE THE FOLLOWING FORM:
Jellehierck 25:a1be4cf2ab0b 721 void do_state_function() {
Jellehierck 25:a1be4cf2ab0b 722 // Entry function
Jellehierck 37:806c7c8381a7 723 if ( global_state_changed == true ) {
Jellehierck 37:806c7c8381a7 724 global_state_changed = false;
Jellehierck 25:a1be4cf2ab0b 725 // More functions
Jellehierck 25:a1be4cf2ab0b 726 }
Jellehierck 25:a1be4cf2ab0b 727
Jellehierck 25:a1be4cf2ab0b 728 // Do stuff until end condition is met
Jellehierck 25:a1be4cf2ab0b 729 doStuff();
Jellehierck 25:a1be4cf2ab0b 730
Jellehierck 25:a1be4cf2ab0b 731 // State transition guard
Jellehierck 25:a1be4cf2ab0b 732 if ( endCondition == true ) {
Jellehierck 37:806c7c8381a7 733 global_curr_state = next_state;
Jellehierck 37:806c7c8381a7 734 global_state_changed = true;
Jellehierck 25:a1be4cf2ab0b 735 // More functions
Jellehierck 25:a1be4cf2ab0b 736 }
Jellehierck 25:a1be4cf2ab0b 737 }
Jellehierck 25:a1be4cf2ab0b 738 */
Jellehierck 25:a1be4cf2ab0b 739
Jellehierck 37:806c7c8381a7 740 // FAILURE MODE
Jellehierck 37:806c7c8381a7 741 void do_global_failure()
Jellehierck 7:7a088536f1c9 742 {
Jellehierck 37:806c7c8381a7 743 // Entry function
Jellehierck 37:806c7c8381a7 744 if ( global_state_changed == true ) {
Jellehierck 37:806c7c8381a7 745 global_state_changed = false;
Jellehierck 25:a1be4cf2ab0b 746
Jellehierck 37:806c7c8381a7 747 failure_mode = true; // Set failure mode
Jellehierck 22:9079c6c0d898 748 }
Jellehierck 37:806c7c8381a7 749
Jellehierck 37:806c7c8381a7 750 // Do stuff until end condition is met
Jellehierck 40:c6dffb676350 751 motor1Power.write(0.0f);
Jellehierck 40:c6dffb676350 752 motor2Power.write(0.0f);
Jellehierck 40:c6dffb676350 753 Vx=0.0f;
Jellehierck 40:c6dffb676350 754 Vy=0.0f;
Jellehierck 37:806c7c8381a7 755
Jellehierck 37:806c7c8381a7 756 // State transition guard
Jellehierck 37:806c7c8381a7 757 if ( false ) { // Never move to other state
Jellehierck 37:806c7c8381a7 758 global_curr_state = global_wait;
Jellehierck 37:806c7c8381a7 759 global_state_changed = true;
Jellehierck 37:806c7c8381a7 760 }
Jellehierck 25:a1be4cf2ab0b 761 }
Jellehierck 25:a1be4cf2ab0b 762
Jellehierck 37:806c7c8381a7 763 // DEMO MODE
Jellehierck 37:806c7c8381a7 764 void do_global_demo()
Jellehierck 25:a1be4cf2ab0b 765 {
Jellehierck 25:a1be4cf2ab0b 766 // Entry function
Jellehierck 37:806c7c8381a7 767 if ( global_state_changed == true ) {
Jellehierck 37:806c7c8381a7 768 global_state_changed = false;
Jellehierck 37:806c7c8381a7 769 // More functions
Jellehierck 37:806c7c8381a7 770 }
Jellehierck 37:806c7c8381a7 771
Jellehierck 37:806c7c8381a7 772 // Do stuff until end condition is met
Jellehierck 37:806c7c8381a7 773 doStuff();
Jellehierck 35:e82834e62e44 774
Jellehierck 37:806c7c8381a7 775 // State transition guard
Jellehierck 37:806c7c8381a7 776 if ( switch2_pressed == true ) {
Jellehierck 37:806c7c8381a7 777 switch2_pressed = false;
Jellehierck 37:806c7c8381a7 778 global_curr_state = global_wait;
Jellehierck 37:806c7c8381a7 779 global_state_changed = true;
Jellehierck 37:806c7c8381a7 780 }
Jellehierck 37:806c7c8381a7 781 }
Jellehierck 37:806c7c8381a7 782
Jellehierck 37:806c7c8381a7 783 // WAIT MODE
Jellehierck 37:806c7c8381a7 784 void do_global_wait()
Jellehierck 37:806c7c8381a7 785 {
Jellehierck 37:806c7c8381a7 786 // Entry function
Jellehierck 37:806c7c8381a7 787 if ( global_state_changed == true ) {
Jellehierck 37:806c7c8381a7 788 global_state_changed = false;
Jellehierck 25:a1be4cf2ab0b 789 }
Jellehierck 25:a1be4cf2ab0b 790
Jellehierck 27:f18da01093c9 791 // Do nothing until end condition is met
Jellehierck 25:a1be4cf2ab0b 792
Jellehierck 37:806c7c8381a7 793 // State transition guard
Jellehierck 37:806c7c8381a7 794 if ( switch2_pressed == true ) { // DEMO MODE
Jellehierck 37:806c7c8381a7 795 switch2_pressed = false;
Jellehierck 37:806c7c8381a7 796 global_curr_state = global_demo;
Jellehierck 37:806c7c8381a7 797 global_state_changed = true;
Jellehierck 31:b5188b6d45db 798
Jellehierck 37:806c7c8381a7 799 } else if ( button1_pressed == true ) { // EMG CALIBRATION
Jellehierck 37:806c7c8381a7 800 button1_pressed = false;
Jellehierck 38:8b597ab8344f 801 global_curr_state = global_emg_cal;
Jellehierck 37:806c7c8381a7 802 global_state_changed = true;
Jellehierck 31:b5188b6d45db 803
Jellehierck 37:806c7c8381a7 804 } else if ( button2_pressed == true ) { // MOTOR CALIBRATION
Jellehierck 37:806c7c8381a7 805 button2_pressed = false;
Jellehierck 38:8b597ab8344f 806 global_curr_state = global_motor_cal;
Jellehierck 37:806c7c8381a7 807 global_state_changed = true;
Jellehierck 40:c6dffb676350 808
Jellehierck 39:f9042483b921 809 } else if ( emg_cal_done && motor_cal_done ) { // OPERATION MODE
Jellehierck 39:f9042483b921 810 global_curr_state = global_operation;
Jellehierck 39:f9042483b921 811 global_state_changed = true;
Jellehierck 25:a1be4cf2ab0b 812 }
Jellehierck 7:7a088536f1c9 813 }
Jellehierck 7:7a088536f1c9 814
Jellehierck 37:806c7c8381a7 815 // EMG CALIBRATION MODE
Jellehierck 38:8b597ab8344f 816 void do_global_emg_cal()
Jellehierck 21:e4569b47945e 817 {
Jellehierck 37:806c7c8381a7 818 // Entry function
Jellehierck 37:806c7c8381a7 819 if ( global_state_changed == true ) {
Jellehierck 37:806c7c8381a7 820 global_state_changed = false;
Jellehierck 22:9079c6c0d898 821 }
Jellehierck 7:7a088536f1c9 822
Jellehierck 39:f9042483b921 823 // Run EMG state machine until emg_cal_done flag is true
Jellehierck 39:f9042483b921 824 emg_state_machine();
Jellehierck 31:b5188b6d45db 825
Jellehierck 29:f51683a6cbbf 826 // State transition guard
Jellehierck 39:f9042483b921 827 if ( emg_cal_done == true ) { // WAIT MODE
Jellehierck 37:806c7c8381a7 828 global_curr_state = global_wait;
Jellehierck 37:806c7c8381a7 829 global_state_changed = true;
Jellehierck 25:a1be4cf2ab0b 830 }
Jellehierck 25:a1be4cf2ab0b 831 }
Jellehierck 23:8a0a0b959af1 832
Jellehierck 37:806c7c8381a7 833 // MOTOR CALIBRATION MODE
Jellehierck 38:8b597ab8344f 834 void do_global_motor_cal()
Jellehierck 26:7e81c7db6e7a 835 {
Jellehierck 25:a1be4cf2ab0b 836 // Entry function
Jellehierck 37:806c7c8381a7 837 if ( global_state_changed == true ) {
Jellehierck 37:806c7c8381a7 838 global_state_changed = false;
Jellehierck 25:a1be4cf2ab0b 839 }
Jellehierck 25:a1be4cf2ab0b 840
Jellehierck 25:a1be4cf2ab0b 841 // Do stuff until end condition is met
Jellehierck 40:c6dffb676350 842 motor_state_machine();
Jellehierck 28:59e8266f4633 843
Jellehierck 25:a1be4cf2ab0b 844 // State transition guard
Jellehierck 41:8e8141f355af 845 if ( motor_cal_done == true ) { // WAIT MODE
Jellehierck 37:806c7c8381a7 846 global_curr_state = global_wait;
Jellehierck 37:806c7c8381a7 847 global_state_changed = true;
Jellehierck 23:8a0a0b959af1 848 }
Jellehierck 23:8a0a0b959af1 849 }
Jellehierck 23:8a0a0b959af1 850
Jellehierck 37:806c7c8381a7 851 // OPERATION MODE
Jellehierck 37:806c7c8381a7 852 void do_global_operation()
Jellehierck 37:806c7c8381a7 853 {
Jellehierck 37:806c7c8381a7 854 // Entry function
Jellehierck 37:806c7c8381a7 855 if ( global_state_changed == true ) {
Jellehierck 37:806c7c8381a7 856 global_state_changed = false;
Jellehierck 40:c6dffb676350 857
Jellehierck 39:f9042483b921 858 emg_sampleNow = true; // Enable signal sampling in sampleSignals()
Jellehierck 39:f9042483b921 859 emg_calibrateNow = false; // Disable calibration functionality in sampleSignals()
Jellehierck 37:806c7c8381a7 860 }
Jellehierck 37:806c7c8381a7 861
Jellehierck 37:806c7c8381a7 862 // Do stuff until end condition is met
Jellehierck 39:f9042483b921 863 emg1_norm = emg1_env * emg1_factor; // Normalize current EMG signal with calibrated factor
Jellehierck 39:f9042483b921 864 emg2_norm = emg2_env * emg2_factor; // Idem
Jellehierck 39:f9042483b921 865 emg3_norm = emg3_env * emg3_factor; // Idem
Jellehierck 39:f9042483b921 866
Jellehierck 39:f9042483b921 867 emg1_out_prev = emg1_out; // Set previous emg_out signal
Jellehierck 39:f9042483b921 868 emg1_dt_prev = emg1_dt; // Set previous emg_out_dt signal
Jellehierck 41:8e8141f355af 869
Jellehierck 41:8e8141f355af 870 if (button1_pressed) {
Jellehierck 41:8e8141f355af 871 button1_pressed = false;
Jellehierck 41:8e8141f355af 872 emg1_dir = emg1_dir * -1.0;
Jellehierck 41:8e8141f355af 873 }
Jellehierck 41:8e8141f355af 874
Jellehierck 41:8e8141f355af 875 if (button2_pressed) {
Jellehierck 41:8e8141f355af 876 button2_pressed = false;
Jellehierck 41:8e8141f355af 877 emg2_dir = emg2_dir * -1.0;
Jellehierck 41:8e8141f355af 878 }
Jellehierck 39:f9042483b921 879
Jellehierck 39:f9042483b921 880 // Set normalized EMG output signal (CAN BE MOVED TO EXTERNAL FUNCTION BECAUSE IT IS REPEATED 3 TIMES)
Jellehierck 39:f9042483b921 881 if ( emg1_norm < emg1_th ) { // If below threshold, emg_out = 0 (ignored)
Jellehierck 39:f9042483b921 882 emg1_out = 0.0;
Jellehierck 39:f9042483b921 883 } else if ( emg1_norm > 1.0f ) { // If above MVC (e.g. due to filtering), emg_out = 1 (max value)
Jellehierck 39:f9042483b921 884 emg1_out = 1.0;
Jellehierck 39:f9042483b921 885 } else { // If in between threshold and MVC, scale EMG signal accordingly
Jellehierck 39:f9042483b921 886 // Inputs may be in range [emg_th, 1]
Jellehierck 39:f9042483b921 887 // Outputs are scaled to range [0, 1]
Jellehierck 39:f9042483b921 888 emg1_out = rescale(emg1_norm, 0, 1, emg1_th, 1);
Jellehierck 39:f9042483b921 889 }
Jellehierck 39:f9042483b921 890 emg1_dt = (emg1_out - emg1_out_prev) / Ts; // Calculate derivative of filtered normalized output signal
Jellehierck 39:f9042483b921 891 emg1_dtdt = (emg1_dt - emg1_dt_prev) / Ts; // Calculate acceleration of filtered normalized output signal
Jellehierck 41:8e8141f355af 892 Vx = emg1_out * 15.0f * emg1_dir;
Jellehierck 39:f9042483b921 893
Jellehierck 39:f9042483b921 894 // Idem for emg2
Jellehierck 39:f9042483b921 895 if ( emg2_norm < emg2_th ) {
Jellehierck 39:f9042483b921 896 emg2_out = 0.0;
Jellehierck 39:f9042483b921 897 } else if ( emg2_norm > 1.0f ) {
Jellehierck 39:f9042483b921 898 emg2_out = 1.0;
Jellehierck 39:f9042483b921 899 } else {
Jellehierck 39:f9042483b921 900 emg2_out = rescale(emg2_norm, 0, 1, emg2_th, 1);
Jellehierck 39:f9042483b921 901 }
Jellehierck 41:8e8141f355af 902 Vy = emg2_out * 15.0f * emg2_dir;
Jellehierck 39:f9042483b921 903
Jellehierck 39:f9042483b921 904 // Idem for emg3
Jellehierck 39:f9042483b921 905 if ( emg3_norm < emg3_th ) {
Jellehierck 39:f9042483b921 906 emg3_out = 0.0;
Jellehierck 39:f9042483b921 907 } else if ( emg3_norm > 1.0f ) {
Jellehierck 39:f9042483b921 908 emg3_out = 1.0;
Jellehierck 39:f9042483b921 909 } else {
Jellehierck 39:f9042483b921 910 emg3_out = rescale(emg3_norm, 0, 1, emg3_th, 1);
Jellehierck 39:f9042483b921 911 }
Jellehierck 40:c6dffb676350 912
Jellehierck 40:c6dffb676350 913 /*
Jellehierck 40:c6dffb676350 914 // For testing Vx and Vy
Jellehierck 40:c6dffb676350 915 static float t=0;
Jellehierck 40:c6dffb676350 916 Vy=10.0f*sin(1.0f*t);
Jellehierck 40:c6dffb676350 917 Vx=0.0f;
Jellehierck 40:c6dffb676350 918 t+=Ts;
Jellehierck 40:c6dffb676350 919 */
Jellehierck 40:c6dffb676350 920
Jellehierck 40:c6dffb676350 921 // Move motors
Jellehierck 40:c6dffb676350 922 PID_controller();
Jellehierck 40:c6dffb676350 923 motorControl();
Jellehierck 40:c6dffb676350 924 RKI();
Jellehierck 39:f9042483b921 925
Jellehierck 39:f9042483b921 926 // Set HIDScope outputs
Jellehierck 39:f9042483b921 927 scope.set(0, emg1 );
Jellehierck 41:8e8141f355af 928 scope.set(1, Vx );
Jellehierck 41:8e8141f355af 929 scope.set(2, emg2 );
Jellehierck 41:8e8141f355af 930 scope.set(3, Vy );
Jellehierck 39:f9042483b921 931 //scope.set(2, emg2_out );
Jellehierck 39:f9042483b921 932 //scope.set(3, emg3_out );
Jellehierck 39:f9042483b921 933 scope.send();
Jellehierck 39:f9042483b921 934
Jellehierck 39:f9042483b921 935 led_g = !led_g;
Jellehierck 37:806c7c8381a7 936
Jellehierck 37:806c7c8381a7 937 // State transition guard
Jellehierck 37:806c7c8381a7 938 if ( false ) { // Always stay in operation mode (can be changed)
Jellehierck 37:806c7c8381a7 939 global_curr_state = global_wait;
Jellehierck 37:806c7c8381a7 940 global_state_changed = true;
Jellehierck 37:806c7c8381a7 941 }
Jellehierck 37:806c7c8381a7 942 }
Jellehierck 23:8a0a0b959af1 943 /*
Jellehierck 37:806c7c8381a7 944 ------------------------------ GLOBAL STATE MACHINE ------------------------------
Jellehierck 23:8a0a0b959af1 945 */
Jellehierck 37:806c7c8381a7 946 void global_state_machine()
Jellehierck 23:8a0a0b959af1 947 {
Jellehierck 37:806c7c8381a7 948 switch(global_curr_state) {
Jellehierck 37:806c7c8381a7 949 case global_failure:
Jellehierck 37:806c7c8381a7 950 do_global_failure();
Jellehierck 23:8a0a0b959af1 951 break;
Jellehierck 37:806c7c8381a7 952 case global_wait:
Jellehierck 37:806c7c8381a7 953 do_global_wait();
Jellehierck 37:806c7c8381a7 954 break;
Jellehierck 38:8b597ab8344f 955 case global_emg_cal:
Jellehierck 38:8b597ab8344f 956 do_global_emg_cal();
Jellehierck 23:8a0a0b959af1 957 break;
Jellehierck 38:8b597ab8344f 958 case global_motor_cal:
Jellehierck 38:8b597ab8344f 959 do_global_motor_cal();
Jellehierck 23:8a0a0b959af1 960 break;
Jellehierck 37:806c7c8381a7 961 case global_operation:
Jellehierck 37:806c7c8381a7 962 do_global_operation();
Jellehierck 37:806c7c8381a7 963 break;
Jellehierck 37:806c7c8381a7 964 case global_demo:
Jellehierck 37:806c7c8381a7 965 do_global_demo();
Jellehierck 23:8a0a0b959af1 966 break;
Jellehierck 23:8a0a0b959af1 967 }
Jellehierck 23:8a0a0b959af1 968 }
Jellehierck 23:8a0a0b959af1 969
Jellehierck 38:8b597ab8344f 970 /*
Jellehierck 38:8b597ab8344f 971 ------------------------------ READ SAMPLES ------------------------------
Jellehierck 38:8b597ab8344f 972 */
Jellehierck 38:8b597ab8344f 973 void sampleSignals()
Jellehierck 38:8b597ab8344f 974 {
Jellehierck 38:8b597ab8344f 975 if (emg_sampleNow == true) { // This ticker only samples if the sample flag is true, to prevent unnecessary computations
Jellehierck 38:8b597ab8344f 976 // Read EMG inputs
Jellehierck 38:8b597ab8344f 977 emg1 = emg1_in.read();
Jellehierck 38:8b597ab8344f 978 emg2 = emg2_in.read();
Jellehierck 38:8b597ab8344f 979 emg3 = emg3_in.read();
Jellehierck 38:8b597ab8344f 980
Jellehierck 38:8b597ab8344f 981 double emg1_n = bqc1_notch.step( emg1 ); // Filter notch
Jellehierck 38:8b597ab8344f 982 double emg1_hp = bqc1_high.step( emg1_n ); // Filter highpass
Jellehierck 38:8b597ab8344f 983 double emg1_rectify = fabs( emg1_hp ); // Rectify
Jellehierck 38:8b597ab8344f 984 emg1_env = bqc1_low.step( emg1_rectify ); // Filter lowpass (completes envelope)
Jellehierck 38:8b597ab8344f 985
Jellehierck 38:8b597ab8344f 986 double emg2_n = bqc2_notch.step( emg2 ); // Filter notch
Jellehierck 38:8b597ab8344f 987 double emg2_hp = bqc2_high.step( emg2_n ); // Filter highpass
Jellehierck 38:8b597ab8344f 988 double emg2_rectify = fabs( emg2_hp ); // Rectify
Jellehierck 38:8b597ab8344f 989 emg2_env = bqc2_low.step( emg2_rectify ); // Filter lowpass (completes envelope)
Jellehierck 38:8b597ab8344f 990
Jellehierck 38:8b597ab8344f 991 double emg3_n = bqc3_notch.step( emg3 ); // Filter notch
Jellehierck 38:8b597ab8344f 992 double emg3_hp = bqc3_high.step( emg3_n ); // Filter highpass
Jellehierck 38:8b597ab8344f 993 double emg3_rectify = fabs( emg3_hp ); // Rectify
Jellehierck 38:8b597ab8344f 994 emg3_env = bqc3_low.step( emg3_rectify ); // Filter lowpass (completes envelope)
Jellehierck 38:8b597ab8344f 995
Jellehierck 38:8b597ab8344f 996 if (emg_calibrateNow == true) { // Only add values to EMG vectors if calibration flag is true
Jellehierck 38:8b597ab8344f 997 emg1_cal.push_back(emg1_env); // Add values to calibration vector
Jellehierck 38:8b597ab8344f 998 // emg1_cal_size = emg1_cal.size(); // Used for debugging
Jellehierck 38:8b597ab8344f 999 emg2_cal.push_back(emg2_env); // Add values to calibration vector
Jellehierck 38:8b597ab8344f 1000 // emg2_cal_size = emg1_cal.size(); // Used for debugging
Jellehierck 38:8b597ab8344f 1001 emg3_cal.push_back(emg3_env); // Add values to calibration vector
Jellehierck 38:8b597ab8344f 1002 // emg3_cal_size = emg1_cal.size(); // Used for debugging
Jellehierck 38:8b597ab8344f 1003 }
Jellehierck 38:8b597ab8344f 1004 }
Jellehierck 40:c6dffb676350 1005
Jellehierck 40:c6dffb676350 1006 counts1af = encoder1.getPulses();
Jellehierck 40:c6dffb676350 1007 deltaCounts1 = counts1af - countsPrev1;
Jellehierck 40:c6dffb676350 1008 countsPrev1 = counts1af;
Jellehierck 40:c6dffb676350 1009
Jellehierck 40:c6dffb676350 1010 counts2af = encoder2.getPulses();
Jellehierck 40:c6dffb676350 1011 deltaCounts2 = counts2af - countsPrev2;
Jellehierck 40:c6dffb676350 1012 countsPrev2 = counts2af;
Jellehierck 38:8b597ab8344f 1013 }
Jellehierck 37:806c7c8381a7 1014
Jellehierck 37:806c7c8381a7 1015 /*
Jellehierck 37:806c7c8381a7 1016 ------------------------------ GLOBAL PROGRAM LOOP ------------------------------
Jellehierck 37:806c7c8381a7 1017 */
Jellehierck 25:a1be4cf2ab0b 1018 void tickGlobalFunc()
Jellehierck 25:a1be4cf2ab0b 1019 {
Jellehierck 38:8b597ab8344f 1020 sampleSignals();
Jellehierck 37:806c7c8381a7 1021 global_state_machine();
Jellehierck 25:a1be4cf2ab0b 1022 // controller();
Jellehierck 25:a1be4cf2ab0b 1023 // outputToMotors();
Jellehierck 25:a1be4cf2ab0b 1024 }
Jellehierck 25:a1be4cf2ab0b 1025
Jellehierck 37:806c7c8381a7 1026 /*
Jellehierck 37:806c7c8381a7 1027 ------------------------------ MAIN FUNCTION ------------------------------
Jellehierck 37:806c7c8381a7 1028 */
Jellehierck 39:f9042483b921 1029 int main()
Jellehierck 23:8a0a0b959af1 1030 {
Jellehierck 23:8a0a0b959af1 1031 pc.baud(115200); // MODSERIAL rate
Jellehierck 23:8a0a0b959af1 1032 pc.printf("Starting\r\n");
Jellehierck 23:8a0a0b959af1 1033
Jellehierck 37:806c7c8381a7 1034 global_curr_state = global_wait; // Start off in EMG Wait state
Jellehierck 34:13fac02ef324 1035 tickGlobal.attach( &tickGlobalFunc, Ts ); // Start global ticker
Jellehierck 8:ea3de43c9e8b 1036
Jellehierck 38:8b597ab8344f 1037 // ---------- Attach filters ----------
Jellehierck 38:8b597ab8344f 1038 bqc1_notch.add( &bq1_notch );
Jellehierck 38:8b597ab8344f 1039 bqc1_high.add( &bq1_H1 ).add( &bq1_H2 );
Jellehierck 38:8b597ab8344f 1040 bqc1_low.add( &bq1_L1 ).add( &bq1_L2 );
Jellehierck 38:8b597ab8344f 1041
Jellehierck 38:8b597ab8344f 1042 bqc2_notch.add( &bq2_notch );
Jellehierck 38:8b597ab8344f 1043 bqc2_high.add( &bq2_H1 ).add( &bq2_H2 );
Jellehierck 38:8b597ab8344f 1044 bqc2_low.add( &bq2_L1 ).add( &bq2_L2 );
Jellehierck 38:8b597ab8344f 1045
Jellehierck 38:8b597ab8344f 1046 bqc3_notch.add( &bq3_notch );
Jellehierck 38:8b597ab8344f 1047 bqc3_high.add( &bq3_H1 ).add( &bq3_H2 );
Jellehierck 38:8b597ab8344f 1048 bqc3_low.add( &bq3_L1 ).add( &bq3_L2 );
Jellehierck 38:8b597ab8344f 1049
Jellehierck 38:8b597ab8344f 1050 // ---------- Attach buttons ----------
Jellehierck 37:806c7c8381a7 1051 button1.fall( &button1Press );
Jellehierck 37:806c7c8381a7 1052 button2.fall( &button2Press );
Jellehierck 37:806c7c8381a7 1053 switch2.fall( &switch2Press );
Jellehierck 37:806c7c8381a7 1054 switch3.fall( &switch3Press );
Jellehierck 38:8b597ab8344f 1055
Jellehierck 40:c6dffb676350 1056 // ---------- Attach PWM ----------
Jellehierck 40:c6dffb676350 1057 motor1Power.period(PWM_period);
Jellehierck 40:c6dffb676350 1058 motor2Power.period(PWM_period);
Jellehierck 40:c6dffb676350 1059
Jellehierck 38:8b597ab8344f 1060 // ---------- Turn OFF LEDs ----------
Jellehierck 38:8b597ab8344f 1061 led_b = 1;
Jellehierck 38:8b597ab8344f 1062 led_g = 1;
Jellehierck 38:8b597ab8344f 1063 led_r = 1;
Jellehierck 37:806c7c8381a7 1064
Jellehierck 23:8a0a0b959af1 1065 while(true) {
Jellehierck 40:c6dffb676350 1066 pc.printf("Global state: %i EMG substate: %i Motor substate: %i\r\n", global_curr_state, emg_curr_state, motor_curr_state);
Jellehierck 41:8e8141f355af 1067 pc.printf("EMG1 direction: %f EMG2 direction: %f \r\n", emg1_dir, emg2_dir);
Jellehierck 41:8e8141f355af 1068 pc.printf("Vx: %f Vy: %f \r\n", Vx, Vy);
Jellehierck 40:c6dffb676350 1069 pc.printf("q1: %f q2: %f \r\n",q1*rad2deg,q2*rad2deg);
Jellehierck 40:c6dffb676350 1070 pc.printf("Xe: %f Ye: %f\r\n",xe,ye);
Jellehierck 30:bac3b60d6283 1071 wait(0.5f);
Jellehierck 23:8a0a0b959af1 1072 }
Jellehierck 23:8a0a0b959af1 1073 }