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@49:80970a03083b, 2019-10-31 (annotated)

Committer:
Jellehierck
Date:
Thu Oct 31 14:45:14 2019 +0000
Revision:
49:80970a03083b
Parent:
46:8a8fa8e602a1
Child:
50:86bad994f08f
Demo mode fixed hopefully, needs testing

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 42:2937ad8f1032 9 #include "FastPWM.h" // PWM control
Jellehierck 42:2937ad8f1032 10 #include "QEI.h" // Encoder reading
Jellehierck 42:2937ad8f1032 11 #include <Servo.h> // Servo control
IsaRobin 0:6972d0e91af1 12
Jellehierck 15:421d3d9c563b 13 /*
Jellehierck 37:806c7c8381a7 14 ------------------------------ DEFINE MBED CONNECTIONS ------------------------------
Jellehierck 15:421d3d9c563b 15 */
IsaRobin 0:6972d0e91af1 16
Jellehierck 38:8b597ab8344f 17 // PC connections
Jellehierck 40:c6dffb676350 18 HIDScope scope( 6 );
Jellehierck 15:421d3d9c563b 19 MODSERIAL pc(USBTX, USBRX);
IsaRobin 0:6972d0e91af1 20
Jellehierck 8:ea3de43c9e8b 21 // Buttons
Jellehierck 8:ea3de43c9e8b 22 InterruptIn button1(D11);
Jellehierck 8:ea3de43c9e8b 23 InterruptIn button2(D10);
Jellehierck 37:806c7c8381a7 24 InterruptIn switch2(SW2);
Jellehierck 37:806c7c8381a7 25 InterruptIn switch3(SW3);
Jellehierck 4:09a01d2db8f7 26
Jellehierck 38:8b597ab8344f 27 // LEDs
Jellehierck 38:8b597ab8344f 28 DigitalOut led_g(LED_GREEN);
Jellehierck 38:8b597ab8344f 29 DigitalOut led_r(LED_RED);
Jellehierck 38:8b597ab8344f 30 DigitalOut led_b(LED_BLUE);
Jellehierck 38:8b597ab8344f 31
Jellehierck 38:8b597ab8344f 32 // Analog EMG inputs
Jellehierck 40:c6dffb676350 33 AnalogIn emg1_in (A0); // Right biceps -> x axis
Jellehierck 40:c6dffb676350 34 AnalogIn emg2_in (A1); // Left biceps -> y axis
Jellehierck 40:c6dffb676350 35 AnalogIn emg3_in (A2); // Third muscle -> TBD
Jellehierck 45:d09040915cfe 36 AnalogIn emg4_in (A3); // Third muscle -> TBD
Jellehierck 45:d09040915cfe 37
Jellehierck 40:c6dffb676350 38
Jellehierck 40:c6dffb676350 39 // Encoder inputs
Jellehierck 40:c6dffb676350 40 DigitalIn encA1(D9);
Jellehierck 40:c6dffb676350 41 DigitalIn encB1(D8);
Jellehierck 40:c6dffb676350 42 DigitalIn encA2(D13);
Jellehierck 40:c6dffb676350 43 DigitalIn encB2(D12);
Jellehierck 40:c6dffb676350 44
Jellehierck 40:c6dffb676350 45 // Motor outputs
Jellehierck 42:2937ad8f1032 46 DigitalOut motor1Direction(D7);
Jellehierck 42:2937ad8f1032 47 FastPWM motor1Power(D6);
Jellehierck 40:c6dffb676350 48 DigitalOut motor2Direction(D4);
Jellehierck 40:c6dffb676350 49 FastPWM motor2Power(D5);
Jellehierck 42:2937ad8f1032 50
Jellehierck 42:2937ad8f1032 51 // Servo
Jellehierck 42:2937ad8f1032 52 Servo myServo(D3);
Jellehierck 38:8b597ab8344f 53
Jellehierck 15:421d3d9c563b 54 /*
Jellehierck 38:8b597ab8344f 55 ------------------------------ INITIALIZE TICKERS, TIMERS & TIMEOUTS ------------------------------
Jellehierck 38:8b597ab8344f 56 */
Jellehierck 38:8b597ab8344f 57 Ticker tickGlobal; // Set global ticker
Jellehierck 38:8b597ab8344f 58 Timer timerCalibration; // Set EMG Calibration timer
Jellehierck 38:8b597ab8344f 59
Jellehierck 38:8b597ab8344f 60 /*
Jellehierck 38:8b597ab8344f 61 ------------------------------ INITIALIZE GLOBAL VARIABLES ------------------------------
Jellehierck 15:421d3d9c563b 62 */
Jellehierck 15:421d3d9c563b 63
Jellehierck 37:806c7c8381a7 64 // State machine variables
Jellehierck 38:8b597ab8344f 65 enum GLOBAL_States { global_failure, global_wait, global_emg_cal, global_motor_cal, global_operation, global_demo }; // Define global states
Jellehierck 37:806c7c8381a7 66 GLOBAL_States global_curr_state = global_wait; // Initialize global state to waiting state
Jellehierck 37:806c7c8381a7 67 bool global_state_changed = true; // Enable entry functions
Jellehierck 42:2937ad8f1032 68 bool failure_mode = false; // Global failure mode flag (not used yet)
Jellehierck 42:2937ad8f1032 69 bool emg_cal_done = false; // Global EMG calibration flag
Jellehierck 42:2937ad8f1032 70 bool motor_cal_done = false; // Global motor calibration flag
Jellehierck 38:8b597ab8344f 71
Jellehierck 38:8b597ab8344f 72 // EMG Substate variables
Jellehierck 38:8b597ab8344f 73 enum EMG_States { emg_wait, emg_cal_MVC, emg_cal_rest, emg_operation }; // Define EMG substates
Jellehierck 38:8b597ab8344f 74 EMG_States emg_curr_state = emg_wait; // Initialize EMG substate variable
Jellehierck 42:2937ad8f1032 75 bool emg_state_changed = true; // Enable entry functions
Jellehierck 42:2937ad8f1032 76 bool emg_sampleNow = false; // Flag to enable EMG sampling and filtering in sampleSignals()
Jellehierck 42:2937ad8f1032 77 bool emg_calibrateNow = false; // Flag to enable EMG calibration in sampleSignals()
Jellehierck 42:2937ad8f1032 78 bool emg_MVC_cal_done = false; // Internal MVC calibration flag
Jellehierck 42:2937ad8f1032 79 bool emg_rest_cal_done = false; // Internal rest calibration flag
Jellehierck 8:ea3de43c9e8b 80
Jellehierck 40:c6dffb676350 81 // Motor Substate variables
Jellehierck 42:2937ad8f1032 82 enum Motor_States { motor_wait, motor_encoder_set, motor_finish, motor_movement }; // Define motor substates
Jellehierck 42:2937ad8f1032 83 Motor_States motor_curr_state = motor_wait; // Initialize motor substate variable
Jellehierck 42:2937ad8f1032 84 bool motor_state_changed = true; // Enable entry functions
Jellehierck 42:2937ad8f1032 85 bool motor_encoder_cal_done = false; // Internal encoder calibration flag
Jellehierck 40:c6dffb676350 86
Jellehierck 42:2937ad8f1032 87 // Operation Substate variables
Jellehierck 42:2937ad8f1032 88 enum Operation_States { operation_wait, operation_movement, operation_finish }; // Define operation substates
Jellehierck 42:2937ad8f1032 89 Operation_States operation_curr_state = operation_wait; // Initialize operation substate variable
Jellehierck 42:2937ad8f1032 90 bool operation_state_changed = true; // Enable entry functions
Jellehierck 42:2937ad8f1032 91 bool operation_showcard = false; // Internal flag to toggle servo position
Jellehierck 40:c6dffb676350 92
Jellehierck 43:1bd5417ded64 93 // Demo Substate variables
Jellehierck 43:1bd5417ded64 94 enum Demo_States { demo_wait, demo_motor_cal, demo_XY }; // Define demo substates
Jellehierck 43:1bd5417ded64 95 Demo_States demo_curr_state; // Initialize demo substate variable
Jellehierck 43:1bd5417ded64 96 bool demo_state_changed = true; // Enable entry functions
Jellehierck 43:1bd5417ded64 97
Jellehierck 37:806c7c8381a7 98 // Button press interrupts (to prevent bounce)
Jellehierck 37:806c7c8381a7 99 bool button1_pressed = false;
Jellehierck 37:806c7c8381a7 100 bool button2_pressed = false;
Jellehierck 37:806c7c8381a7 101 bool switch2_pressed = false;
Jellehierck 7:7a088536f1c9 102
Jellehierck 38:8b597ab8344f 103 // Global constants
Jellehierck 38:8b597ab8344f 104 const double Fs = 500.0;
Jellehierck 38:8b597ab8344f 105 const double Ts = 1/Fs;
Jellehierck 35:e82834e62e44 106
Jellehierck 35:e82834e62e44 107 /*
Jellehierck 37:806c7c8381a7 108 ------------------------------ HELPER FUNCTIONS ------------------------------
Jellehierck 37:806c7c8381a7 109 */
Jellehierck 38:8b597ab8344f 110 // Empty placeholder function, needs to be deleted at end of project
Jellehierck 38:8b597ab8344f 111 void doStuff() {}
Jellehierck 38:8b597ab8344f 112
Jellehierck 38:8b597ab8344f 113 // Return max value of vector
Jellehierck 38:8b597ab8344f 114 double getMax(const vector<double> &vect)
Jellehierck 38:8b597ab8344f 115 {
Jellehierck 38:8b597ab8344f 116 double curr_max = 0.0;
Jellehierck 38:8b597ab8344f 117 int vect_n = vect.size();
Jellehierck 38:8b597ab8344f 118 for (int i = 0; i < vect_n; i++) {
Jellehierck 38:8b597ab8344f 119 if (vect[i] > curr_max) {
Jellehierck 38:8b597ab8344f 120 curr_max = vect[i];
Jellehierck 38:8b597ab8344f 121 };
Jellehierck 38:8b597ab8344f 122 }
Jellehierck 38:8b597ab8344f 123 return curr_max;
Jellehierck 38:8b597ab8344f 124 }
Jellehierck 37:806c7c8381a7 125
Jellehierck 38:8b597ab8344f 126 // Return mean of vector
Jellehierck 38:8b597ab8344f 127 double getMean(const vector<double> &vect)
Jellehierck 38:8b597ab8344f 128 {
Jellehierck 38:8b597ab8344f 129 double sum = 0.0;
Jellehierck 38:8b597ab8344f 130 int vect_n = vect.size();
Jellehierck 38:8b597ab8344f 131 for ( int i = 0; i < vect_n; i++ ) {
Jellehierck 38:8b597ab8344f 132 sum += vect[i];
Jellehierck 38:8b597ab8344f 133 }
Jellehierck 38:8b597ab8344f 134 return sum/vect_n;
Jellehierck 38:8b597ab8344f 135 }
Jellehierck 37:806c7c8381a7 136
Jellehierck 38:8b597ab8344f 137 // Return standard deviation of vector
Jellehierck 38:8b597ab8344f 138 double getStdev(const vector<double> &vect, const double vect_mean)
Jellehierck 38:8b597ab8344f 139 {
Jellehierck 38:8b597ab8344f 140 double sum2 = 0.0;
Jellehierck 38:8b597ab8344f 141 int vect_n = vect.size();
Jellehierck 38:8b597ab8344f 142 for ( int i = 0; i < vect_n; i++ ) {
Jellehierck 38:8b597ab8344f 143 sum2 += pow( vect[i] - vect_mean, 2 );
Jellehierck 38:8b597ab8344f 144 }
Jellehierck 38:8b597ab8344f 145 double output = sqrt( sum2 / vect_n );
Jellehierck 38:8b597ab8344f 146 return output;
Jellehierck 38:8b597ab8344f 147 }
Jellehierck 38:8b597ab8344f 148
Jellehierck 38:8b597ab8344f 149 // Rescale double values to certain range
Jellehierck 38:8b597ab8344f 150 double rescale(double input, double out_min, double out_max, double in_min, double in_max)
Jellehierck 38:8b597ab8344f 151 {
Jellehierck 38:8b597ab8344f 152 double output = out_min + ((input-in_min)/(in_max-in_min))*(out_max-out_min); // Based on MATLAB rescale function
Jellehierck 38:8b597ab8344f 153 return output;
Jellehierck 38:8b597ab8344f 154 }
Jellehierck 37:806c7c8381a7 155
Jellehierck 37:806c7c8381a7 156 /*
Jellehierck 37:806c7c8381a7 157 ------------------------------ BUTTON FUNCTIONS ------------------------------
Jellehierck 35:e82834e62e44 158 */
Jellehierck 35:e82834e62e44 159
Jellehierck 25:a1be4cf2ab0b 160 // Handle button press
Jellehierck 25:a1be4cf2ab0b 161 void button1Press()
Jellehierck 25:a1be4cf2ab0b 162 {
Jellehierck 25:a1be4cf2ab0b 163 button1_pressed = true;
Jellehierck 25:a1be4cf2ab0b 164 }
Jellehierck 25:a1be4cf2ab0b 165
Jellehierck 25:a1be4cf2ab0b 166 // Handle button press
Jellehierck 25:a1be4cf2ab0b 167 void button2Press()
Jellehierck 25:a1be4cf2ab0b 168 {
Jellehierck 25:a1be4cf2ab0b 169 button2_pressed = true;
Jellehierck 25:a1be4cf2ab0b 170 }
Jellehierck 25:a1be4cf2ab0b 171
Jellehierck 37:806c7c8381a7 172 void switch2Press()
Jellehierck 6:5437cc97e1e6 173 {
Jellehierck 37:806c7c8381a7 174 switch2_pressed = true;
Jellehierck 35:e82834e62e44 175 }
Jellehierck 6:5437cc97e1e6 176
Jellehierck 37:806c7c8381a7 177 void switch3Press()
Jellehierck 35:e82834e62e44 178 {
Jellehierck 37:806c7c8381a7 179 global_curr_state = global_failure;
Jellehierck 37:806c7c8381a7 180 global_state_changed = true;
Jellehierck 6:5437cc97e1e6 181 }
Jellehierck 6:5437cc97e1e6 182
Jellehierck 15:421d3d9c563b 183 /*
Jellehierck 38:8b597ab8344f 184 ------------------------------ EMG GLOBAL VARIABLES & CONSTANTS ------------------------------
Jellehierck 38:8b597ab8344f 185 */
Jellehierck 38:8b597ab8344f 186
Jellehierck 38:8b597ab8344f 187 // Set global constant values for EMG reading & analysis
Jellehierck 41:8e8141f355af 188 const double Tcal = 7.5f; // Calibration duration (s)
Jellehierck 38:8b597ab8344f 189
Jellehierck 38:8b597ab8344f 190 // Initialize variables for EMG reading & analysis
Jellehierck 38:8b597ab8344f 191 double emg1;
Jellehierck 38:8b597ab8344f 192 double emg1_env;
Jellehierck 38:8b597ab8344f 193 double emg1_MVC;
Jellehierck 38:8b597ab8344f 194 double emg1_rest;
Jellehierck 38:8b597ab8344f 195 double emg1_factor;//delete
Jellehierck 38:8b597ab8344f 196 double emg1_th;
Jellehierck 38:8b597ab8344f 197 double emg1_out;
Jellehierck 38:8b597ab8344f 198 double emg1_norm; //delete
Jellehierck 38:8b597ab8344f 199 vector<double> emg1_cal;
Jellehierck 38:8b597ab8344f 200 int emg1_cal_size; //delete
Jellehierck 41:8e8141f355af 201 double emg1_dir = 1.0;
Jellehierck 38:8b597ab8344f 202
Jellehierck 38:8b597ab8344f 203 double emg2;
Jellehierck 38:8b597ab8344f 204 double emg2_env;
Jellehierck 38:8b597ab8344f 205 double emg2_MVC;
Jellehierck 38:8b597ab8344f 206 double emg2_rest;
Jellehierck 38:8b597ab8344f 207 double emg2_factor;//delete
Jellehierck 38:8b597ab8344f 208 double emg2_th;
Jellehierck 38:8b597ab8344f 209 double emg2_out;
Jellehierck 38:8b597ab8344f 210 double emg2_norm;//delete
Jellehierck 38:8b597ab8344f 211 vector<double> emg2_cal;
Jellehierck 38:8b597ab8344f 212 int emg2_cal_size;//delete
Jellehierck 41:8e8141f355af 213 double emg2_dir = 1.0;
Jellehierck 38:8b597ab8344f 214
Jellehierck 38:8b597ab8344f 215 double emg3;
Jellehierck 38:8b597ab8344f 216 double emg3_env;
Jellehierck 38:8b597ab8344f 217 double emg3_MVC;
Jellehierck 38:8b597ab8344f 218 double emg3_rest;
Jellehierck 38:8b597ab8344f 219 double emg3_factor;//delete
Jellehierck 38:8b597ab8344f 220 double emg3_th;
Jellehierck 38:8b597ab8344f 221 double emg3_out;
Jellehierck 38:8b597ab8344f 222 double emg3_norm;//delete
Jellehierck 38:8b597ab8344f 223 vector<double> emg3_cal;
Jellehierck 45:d09040915cfe 224
Jellehierck 45:d09040915cfe 225 double emg4;
Jellehierck 45:d09040915cfe 226 double emg4_env;
Jellehierck 45:d09040915cfe 227 double emg4_MVC;
Jellehierck 45:d09040915cfe 228 double emg4_rest;
Jellehierck 45:d09040915cfe 229 double emg4_factor;//delete
Jellehierck 45:d09040915cfe 230 double emg4_th;
Jellehierck 45:d09040915cfe 231 double emg4_out;
Jellehierck 45:d09040915cfe 232 double emg4_norm;//delete
Jellehierck 45:d09040915cfe 233 vector<double> emg4_cal;
Jellehierck 38:8b597ab8344f 234
Jellehierck 38:8b597ab8344f 235 /*
Jellehierck 38:8b597ab8344f 236 ------------------------------ EMG FILTERS ------------------------------
Jellehierck 38:8b597ab8344f 237 */
Jellehierck 38:8b597ab8344f 238
Jellehierck 38:8b597ab8344f 239 // Notch biquad filter coefficients (iirnotch Q factor 35 @50Hz) from MATLAB:
Jellehierck 38:8b597ab8344f 240 BiQuad bq1_notch( 0.995636295063941, -1.89829218816065, 0.995636295063941, 1, -1.89829218816065, 0.991272590127882); // b01 b11 b21 a01 a11 a21
Jellehierck 38:8b597ab8344f 241 BiQuad bq2_notch = bq1_notch;
Jellehierck 38:8b597ab8344f 242 BiQuad bq3_notch = bq1_notch;
Jellehierck 45:d09040915cfe 243 BiQuad bq4_notch = bq1_notch;
Jellehierck 38:8b597ab8344f 244 BiQuadChain bqc1_notch;
Jellehierck 38:8b597ab8344f 245 BiQuadChain bqc2_notch;
Jellehierck 38:8b597ab8344f 246 BiQuadChain bqc3_notch;
Jellehierck 45:d09040915cfe 247 BiQuadChain bqc4_notch;
Jellehierck 38:8b597ab8344f 248
Jellehierck 38:8b597ab8344f 249 // Highpass biquad filter coefficients (butter 4th order @10Hz cutoff) from MATLAB
Jellehierck 38:8b597ab8344f 250 BiQuad bq1_H1(0.922946103200875, -1.84589220640175, 0.922946103200875, 1, -1.88920703055163, 0.892769008131025); // b01 b11 b21 a01 a11 a21
Jellehierck 38:8b597ab8344f 251 BiQuad bq1_H2(1, -2, 1, 1, -1.95046575793011, 0.954143234875078); // b02 b12 b22 a02 a12 a22
Jellehierck 38:8b597ab8344f 252 BiQuad bq2_H1 = bq1_H1;
Jellehierck 38:8b597ab8344f 253 BiQuad bq2_H2 = bq1_H2;
Jellehierck 38:8b597ab8344f 254 BiQuad bq3_H1 = bq1_H1;
Jellehierck 38:8b597ab8344f 255 BiQuad bq3_H2 = bq1_H2;
Jellehierck 45:d09040915cfe 256 BiQuad bq4_H1 = bq1_H1;
Jellehierck 45:d09040915cfe 257 BiQuad bq4_H2 = bq1_H2;
Jellehierck 38:8b597ab8344f 258 BiQuadChain bqc1_high;
Jellehierck 38:8b597ab8344f 259 BiQuadChain bqc2_high;
Jellehierck 38:8b597ab8344f 260 BiQuadChain bqc3_high;
Jellehierck 45:d09040915cfe 261 BiQuadChain bqc4_high;
Jellehierck 38:8b597ab8344f 262
Jellehierck 38:8b597ab8344f 263 // Lowpass biquad filter coefficients (butter 4th order @5Hz cutoff) from MATLAB:
Jellehierck 38:8b597ab8344f 264 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 265 BiQuad bq1_L2(1, 2, 1, 1, -1.97586467534468, 0.976794920438162); // b02 b12 b22 a02 a12 a22
Jellehierck 38:8b597ab8344f 266 BiQuad bq2_L1 = bq1_L1;
Jellehierck 38:8b597ab8344f 267 BiQuad bq2_L2 = bq1_L2;
Jellehierck 38:8b597ab8344f 268 BiQuad bq3_L1 = bq1_L1;
Jellehierck 38:8b597ab8344f 269 BiQuad bq3_L2 = bq1_L2;
Jellehierck 45:d09040915cfe 270 BiQuad bq4_L1 = bq1_L1;
Jellehierck 45:d09040915cfe 271 BiQuad bq4_L2 = bq1_L2;
Jellehierck 38:8b597ab8344f 272 BiQuadChain bqc1_low;
Jellehierck 38:8b597ab8344f 273 BiQuadChain bqc2_low;
Jellehierck 38:8b597ab8344f 274 BiQuadChain bqc3_low;
Jellehierck 45:d09040915cfe 275 BiQuadChain bqc4_low;
Jellehierck 38:8b597ab8344f 276
Jellehierck 38:8b597ab8344f 277 // Function to check filter stability
Jellehierck 38:8b597ab8344f 278 bool checkBQChainStable()
Jellehierck 38:8b597ab8344f 279 {
Jellehierck 38:8b597ab8344f 280 bool n_stable = bqc1_notch.stable(); // Check stability of all BQ Chains
Jellehierck 38:8b597ab8344f 281 bool hp_stable = bqc1_high.stable();
Jellehierck 38:8b597ab8344f 282 bool l_stable = bqc1_low.stable();
Jellehierck 38:8b597ab8344f 283
Jellehierck 38:8b597ab8344f 284 if (n_stable && hp_stable && l_stable) {
Jellehierck 38:8b597ab8344f 285 return true;
Jellehierck 38:8b597ab8344f 286 } else {
Jellehierck 38:8b597ab8344f 287 return false;
Jellehierck 38:8b597ab8344f 288 }
Jellehierck 38:8b597ab8344f 289 }
Jellehierck 42:2937ad8f1032 290
Jellehierck 42:2937ad8f1032 291 /*
Jellehierck 42:2937ad8f1032 292 ------------------------------ EMG GLOBAL FUNCTIONS ------------------------------
Jellehierck 42:2937ad8f1032 293 */
Jellehierck 42:2937ad8f1032 294
Jellehierck 42:2937ad8f1032 295 void EMGOperationFunc()
Jellehierck 42:2937ad8f1032 296 {
Jellehierck 42:2937ad8f1032 297 emg1_norm = emg1_env * emg1_factor; // Normalize current EMG signal with calibrated factor
Jellehierck 42:2937ad8f1032 298 emg2_norm = emg2_env * emg2_factor; // Idem
Jellehierck 42:2937ad8f1032 299 emg3_norm = emg3_env * emg3_factor; // Idem
Jellehierck 45:d09040915cfe 300 emg4_norm = emg4_env * emg4_factor; // Idem
Jellehierck 42:2937ad8f1032 301
Jellehierck 42:2937ad8f1032 302
Jellehierck 42:2937ad8f1032 303 // Set normalized EMG output signal (CAN BE MOVED TO EXTERNAL FUNCTION BECAUSE IT IS REPEATED 3 TIMES)
Jellehierck 42:2937ad8f1032 304 if ( emg1_norm < emg1_th ) { // If below threshold, emg_out = 0 (ignored)
Jellehierck 42:2937ad8f1032 305 emg1_out = 0.0;
Jellehierck 42:2937ad8f1032 306 } else if ( emg1_norm > 1.0f ) { // If above MVC (e.g. due to filtering), emg_out = 1 (max value)
Jellehierck 42:2937ad8f1032 307 emg1_out = 1.0;
Jellehierck 42:2937ad8f1032 308 } else { // If in between threshold and MVC, scale EMG signal accordingly
Jellehierck 42:2937ad8f1032 309 // Inputs may be in range [emg_th, 1]
Jellehierck 42:2937ad8f1032 310 // Outputs are scaled to range [0, 1]
Jellehierck 42:2937ad8f1032 311 emg1_out = rescale(emg1_norm, 0, 1, emg1_th, 1);
Jellehierck 42:2937ad8f1032 312 }
Jellehierck 42:2937ad8f1032 313
Jellehierck 42:2937ad8f1032 314 // Idem for emg2
Jellehierck 42:2937ad8f1032 315 if ( emg2_norm < emg2_th ) {
Jellehierck 42:2937ad8f1032 316 emg2_out = 0.0;
Jellehierck 42:2937ad8f1032 317 } else if ( emg2_norm > 1.0f ) {
Jellehierck 42:2937ad8f1032 318 emg2_out = 1.0;
Jellehierck 42:2937ad8f1032 319 } else {
Jellehierck 42:2937ad8f1032 320 emg2_out = rescale(emg2_norm, 0, 1, emg2_th, 1);
Jellehierck 42:2937ad8f1032 321 }
Jellehierck 42:2937ad8f1032 322
Jellehierck 42:2937ad8f1032 323 // Idem for emg3
Jellehierck 42:2937ad8f1032 324 if ( emg3_norm < emg3_th ) {
Jellehierck 45:d09040915cfe 325 emg1_dir = 1.0;
Jellehierck 42:2937ad8f1032 326 } else {
Jellehierck 45:d09040915cfe 327 emg1_dir = -1.0;
Jellehierck 45:d09040915cfe 328 }
Jellehierck 45:d09040915cfe 329
Jellehierck 45:d09040915cfe 330 if ( emg4_norm < emg4_th ) {
Jellehierck 45:d09040915cfe 331 emg2_dir = 1.0;
Jellehierck 45:d09040915cfe 332 } else {
Jellehierck 45:d09040915cfe 333 emg2_dir = -1.0;
Jellehierck 42:2937ad8f1032 334 }
Jellehierck 42:2937ad8f1032 335 }
Jellehierck 38:8b597ab8344f 336 /*
Jellehierck 38:8b597ab8344f 337 ------------------------------ EMG SUBSTATE FUNCTIONS ------------------------------
Jellehierck 15:421d3d9c563b 338 */
Jellehierck 38:8b597ab8344f 339
Jellehierck 38:8b597ab8344f 340 // EMG Waiting state
Jellehierck 38:8b597ab8344f 341 void do_emg_wait()
Jellehierck 38:8b597ab8344f 342 {
Jellehierck 38:8b597ab8344f 343 // Entry function
Jellehierck 38:8b597ab8344f 344 if ( emg_state_changed == true ) {
Jellehierck 38:8b597ab8344f 345 emg_state_changed = false; // Disable entry functions
Jellehierck 38:8b597ab8344f 346
Jellehierck 38:8b597ab8344f 347 button1.fall( &button1Press ); // Change to state MVC calibration on button1 press
Jellehierck 38:8b597ab8344f 348 button2.fall( &button2Press ); // Change to state rest calibration on button2 press
Jellehierck 38:8b597ab8344f 349 }
Jellehierck 38:8b597ab8344f 350
Jellehierck 38:8b597ab8344f 351 // Do nothing until end condition is met
Jellehierck 38:8b597ab8344f 352
Jellehierck 38:8b597ab8344f 353 // State transition guard
Jellehierck 38:8b597ab8344f 354 if ( button1_pressed ) { // MVC calibration
Jellehierck 38:8b597ab8344f 355 button1_pressed = false; // Disable button pressed function until next button press
Jellehierck 38:8b597ab8344f 356 button1.fall( NULL ); // Disable interrupt during calibration
Jellehierck 38:8b597ab8344f 357 button2.fall( NULL ); // Disable interrupt during calibration
Jellehierck 38:8b597ab8344f 358 emg_curr_state = emg_cal_MVC; // Set next state
Jellehierck 38:8b597ab8344f 359 emg_state_changed = true; // Enable entry functions
Jellehierck 38:8b597ab8344f 360
Jellehierck 38:8b597ab8344f 361 } else if ( button2_pressed ) { // Rest calibration
Jellehierck 38:8b597ab8344f 362 button2_pressed = false; // Disable button pressed function until next button press
Jellehierck 38:8b597ab8344f 363 button1.fall( NULL ); // Disable interrupt during calibration
Jellehierck 38:8b597ab8344f 364 button2.fall( NULL ); // Disable interrupt during calibration
Jellehierck 38:8b597ab8344f 365 emg_curr_state = emg_cal_rest; // Set next state
Jellehierck 38:8b597ab8344f 366 emg_state_changed = true; // Enable entry functions
Jellehierck 38:8b597ab8344f 367
Jellehierck 38:8b597ab8344f 368 } else if ( emg_MVC_cal_done && emg_rest_cal_done ) { // Operation mode
Jellehierck 38:8b597ab8344f 369 button1.fall( NULL ); // Disable interrupt during operation
Jellehierck 38:8b597ab8344f 370 button2.fall( NULL ); // Disable interrupt during operation
Jellehierck 38:8b597ab8344f 371 emg_curr_state = emg_operation; // Set next state
Jellehierck 38:8b597ab8344f 372 emg_state_changed = true; // Enable entry functions
Jellehierck 38:8b597ab8344f 373 }
Jellehierck 38:8b597ab8344f 374 }
Jellehierck 38:8b597ab8344f 375
Jellehierck 38:8b597ab8344f 376 // EMG Calibration state
Jellehierck 38:8b597ab8344f 377 void do_emg_cal()
Jellehierck 38:8b597ab8344f 378 {
Jellehierck 38:8b597ab8344f 379 // Entry functions
Jellehierck 38:8b597ab8344f 380 if ( emg_state_changed == true ) {
Jellehierck 38:8b597ab8344f 381 emg_state_changed = false; // Disable entry functions
Jellehierck 38:8b597ab8344f 382 led_b = 0; // Turn on calibration led
Jellehierck 38:8b597ab8344f 383
Jellehierck 38:8b597ab8344f 384 timerCalibration.reset();
Jellehierck 38:8b597ab8344f 385 timerCalibration.start(); // Sets up timer to stop calibration after Tcal seconds
Jellehierck 38:8b597ab8344f 386 emg_sampleNow = true; // Enable signal sampling in sampleSignals()
Jellehierck 38:8b597ab8344f 387 emg_calibrateNow = true; // Enable calibration vector functionality in sampleSignals()
Jellehierck 38:8b597ab8344f 388
Jellehierck 38:8b597ab8344f 389 emg1_cal.reserve(Fs * Tcal); // Initialize vector lengths to prevent memory overflow
Jellehierck 38:8b597ab8344f 390 emg2_cal.reserve(Fs * Tcal); // Idem
Jellehierck 38:8b597ab8344f 391 emg3_cal.reserve(Fs * Tcal); // Idem
Jellehierck 45:d09040915cfe 392 emg4_cal.reserve(Fs * Tcal); // Idem
Jellehierck 38:8b597ab8344f 393 }
Jellehierck 38:8b597ab8344f 394
Jellehierck 38:8b597ab8344f 395 // Do stuff until end condition is met
Jellehierck 38:8b597ab8344f 396 // Set HIDScope outputs
Jellehierck 38:8b597ab8344f 397 scope.set(0, emg1 );
Jellehierck 45:d09040915cfe 398 scope.set(1, emg2 );
Jellehierck 45:d09040915cfe 399 scope.set(2, emg3 );
Jellehierck 45:d09040915cfe 400 scope.set(3, emg4 );
Jellehierck 38:8b597ab8344f 401 //scope.set(2, emg2_env );
Jellehierck 38:8b597ab8344f 402 //scope.set(3, emg3_env );
Jellehierck 38:8b597ab8344f 403 scope.send();
Jellehierck 38:8b597ab8344f 404
Jellehierck 38:8b597ab8344f 405 // State transition guard
Jellehierck 38:8b597ab8344f 406 if ( timerCalibration.read() >= Tcal ) { // After interval Tcal the calibration step is finished
Jellehierck 38:8b597ab8344f 407 emg_sampleNow = false; // Disable signal sampling in sampleSignals()
Jellehierck 38:8b597ab8344f 408 emg_calibrateNow = false; // Disable calibration sampling
Jellehierck 38:8b597ab8344f 409 led_b = 1; // Turn off calibration led
Jellehierck 38:8b597ab8344f 410
Jellehierck 38:8b597ab8344f 411 // Extract EMG scale data from calibration
Jellehierck 38:8b597ab8344f 412 switch( emg_curr_state ) {
Jellehierck 39:f9042483b921 413 case emg_cal_MVC: // In case of MVC calibration
Jellehierck 38:8b597ab8344f 414 emg1_MVC = getMax(emg1_cal); // Store max value of MVC globally
Jellehierck 38:8b597ab8344f 415 emg2_MVC = getMax(emg2_cal);
Jellehierck 38:8b597ab8344f 416 emg3_MVC = getMax(emg3_cal);
Jellehierck 45:d09040915cfe 417 emg4_MVC = getMax(emg4_cal);
Jellehierck 38:8b597ab8344f 418 emg_MVC_cal_done = true; // Set up transition to EMG operation mode
Jellehierck 38:8b597ab8344f 419 break;
Jellehierck 39:f9042483b921 420 case emg_cal_rest: // In case of rest calibration
Jellehierck 38:8b597ab8344f 421 emg1_rest = getMean(emg1_cal); // Store mean of EMG in rest globally
Jellehierck 38:8b597ab8344f 422 emg2_rest = getMean(emg2_cal);
Jellehierck 38:8b597ab8344f 423 emg3_rest = getMean(emg3_cal);
Jellehierck 45:d09040915cfe 424 emg4_rest = getMean(emg4_cal);
Jellehierck 38:8b597ab8344f 425 emg_rest_cal_done = true; // Set up transition to EMG operation mode
Jellehierck 38:8b597ab8344f 426 break;
Jellehierck 38:8b597ab8344f 427 }
Jellehierck 38:8b597ab8344f 428 vector<double>().swap(emg1_cal); // Empty vector to prevent memory overflow
Jellehierck 38:8b597ab8344f 429 vector<double>().swap(emg2_cal);
Jellehierck 38:8b597ab8344f 430 vector<double>().swap(emg3_cal);
Jellehierck 45:d09040915cfe 431 vector<double>().swap(emg4_cal);
Jellehierck 38:8b597ab8344f 432
Jellehierck 38:8b597ab8344f 433 emg_curr_state = emg_wait; // Set next substate
Jellehierck 38:8b597ab8344f 434 emg_state_changed = true; // Enable substate entry function
Jellehierck 38:8b597ab8344f 435 }
Jellehierck 38:8b597ab8344f 436 }
Jellehierck 38:8b597ab8344f 437
Jellehierck 38:8b597ab8344f 438 // EMG Operation state
Jellehierck 38:8b597ab8344f 439 void do_emg_operation()
Jellehierck 38:8b597ab8344f 440 {
Jellehierck 38:8b597ab8344f 441 // Entry function
Jellehierck 38:8b597ab8344f 442 if ( emg_state_changed == true ) {
Jellehierck 38:8b597ab8344f 443 emg_state_changed = false; // Disable entry functions
Jellehierck 40:c6dffb676350 444
Jellehierck 39:f9042483b921 445 // Compute scale factors for all EMG signals
Jellehierck 38:8b597ab8344f 446 double margin_percentage = 5; // Set up % margin for rest
Jellehierck 38:8b597ab8344f 447 emg1_factor = 1 / emg1_MVC; // Factor to normalize MVC
Jellehierck 38:8b597ab8344f 448 emg1_th = emg1_rest * emg1_factor + margin_percentage/100; // Set normalized rest threshold
Jellehierck 38:8b597ab8344f 449 emg2_factor = 1 / emg2_MVC; // Factor to normalize MVC
Jellehierck 38:8b597ab8344f 450 emg2_th = emg2_rest * emg2_factor + margin_percentage/100; // Set normalized rest threshold
Jellehierck 38:8b597ab8344f 451 emg3_factor = 1 / emg3_MVC; // Factor to normalize MVC
Jellehierck 38:8b597ab8344f 452 emg3_th = emg3_rest * emg3_factor + margin_percentage/100; // Set normalized rest threshold
Jellehierck 45:d09040915cfe 453 emg4_factor = 1 / emg4_MVC; // Factor to normalize MVC
Jellehierck 45:d09040915cfe 454 emg4_th = emg4_rest * emg4_factor + margin_percentage/100; // Set normalized rest threshold
Jellehierck 38:8b597ab8344f 455 }
Jellehierck 40:c6dffb676350 456
Jellehierck 39:f9042483b921 457 // This state only runs its entry functions ONCE and then exits the EMG substate machine
Jellehierck 38:8b597ab8344f 458
Jellehierck 38:8b597ab8344f 459 // State transition guard
Jellehierck 41:8e8141f355af 460 if ( true ) { // EMG substate machine is terminated directly after running this state once
Jellehierck 38:8b597ab8344f 461 emg_curr_state = emg_wait; // Set next state
Jellehierck 38:8b597ab8344f 462 emg_state_changed = true; // Enable entry function
Jellehierck 41:8e8141f355af 463 emg_cal_done = true; // Let the global substate machine know that EMG calibration is finished
Jellehierck 42:2937ad8f1032 464
Jellehierck 41:8e8141f355af 465 // Enable buttons again
Jellehierck 41:8e8141f355af 466 button1.fall( &button1Press );
Jellehierck 41:8e8141f355af 467 button2.fall( &button2Press );
Jellehierck 38:8b597ab8344f 468 }
Jellehierck 38:8b597ab8344f 469 }
Jellehierck 38:8b597ab8344f 470
Jellehierck 42:2937ad8f1032 471
Jellehierck 42:2937ad8f1032 472
Jellehierck 42:2937ad8f1032 473 /*
Jellehierck 42:2937ad8f1032 474 ------------------------------ MOTOR GLOBAL VARIABLES & CONSTANTS ------------------------------
Jellehierck 42:2937ad8f1032 475 */
Jellehierck 42:2937ad8f1032 476 // Initialize encoder
Jellehierck 42:2937ad8f1032 477 int encoder_res = 64;
Jellehierck 42:2937ad8f1032 478
Jellehierck 42:2937ad8f1032 479 QEI encoder1(D9, D8, NC, encoder_res, QEI::X4_ENCODING); //Encoder of motor 1
Jellehierck 42:2937ad8f1032 480 QEI encoder2(D13, D12, NC, encoder_res, QEI::X4_ENCODING); //Encoder of motor 2
Jellehierck 42:2937ad8f1032 481
Jellehierck 42:2937ad8f1032 482 // Initialize variables for encoder reading
Jellehierck 42:2937ad8f1032 483 volatile int counts1;
Jellehierck 42:2937ad8f1032 484 volatile int counts1af;
Jellehierck 42:2937ad8f1032 485 int counts1offset;
Jellehierck 42:2937ad8f1032 486 volatile int countsPrev1 = 0;
Jellehierck 42:2937ad8f1032 487 volatile int deltaCounts1;
Jellehierck 42:2937ad8f1032 488
Jellehierck 42:2937ad8f1032 489 volatile int counts2;
Jellehierck 42:2937ad8f1032 490 volatile int counts2af;
Jellehierck 42:2937ad8f1032 491 int counts2offset;
Jellehierck 42:2937ad8f1032 492 volatile int countsPrev2 = 0;
Jellehierck 42:2937ad8f1032 493 volatile int deltaCounts2;
Jellehierck 42:2937ad8f1032 494
Jellehierck 42:2937ad8f1032 495 // PWM period
Jellehierck 42:2937ad8f1032 496 const float PWM_period = 1/(18*10e3); // 18000 Hz
Jellehierck 42:2937ad8f1032 497
Jellehierck 42:2937ad8f1032 498 // Important constants
Jellehierck 42:2937ad8f1032 499 const double pi = 3.1415926535897; // pi
Jellehierck 42:2937ad8f1032 500 const double pi2 = pi * 2; // 2 pi
Jellehierck 42:2937ad8f1032 501 const double deg2rad = pi / 180; //Conversion factor degree to rad
Jellehierck 42:2937ad8f1032 502 const double rad2deg = 180 / pi; //Conversion factor rad to degree
Jellehierck 42:2937ad8f1032 503 const double gearratio1 = 110 / 20; // Teeth of large gear : teeth of driven gear
Jellehierck 42:2937ad8f1032 504 const double gearratio2 = 55 / 20; // Teeth of small gear : teeth of driven gear
Jellehierck 42:2937ad8f1032 505 const double encoder_factorin = pi2 / encoder_res; // Convert encoder counts to angle in rad
Jellehierck 42:2937ad8f1032 506 const float gearbox_ratio = 131.25; // Gear ratio of motor gearboxes
Jellehierck 42:2937ad8f1032 507
Jellehierck 42:2937ad8f1032 508 // Kinematics variables
Jellehierck 42:2937ad8f1032 509 const float l1 = 26.0; // Distance base-joint2 [cm]
Jellehierck 42:2937ad8f1032 510 const float l2 = 62.0; // Distance join2-endpoint [cm]
Jellehierck 42:2937ad8f1032 511
Jellehierck 42:2937ad8f1032 512 float q1 = -10.0 * deg2rad; // Angle of first joint [rad] (starts off in reference position)
Jellehierck 42:2937ad8f1032 513 float q1dot; // Velocity of first joint [rad/s]
Jellehierck 42:2937ad8f1032 514
Jellehierck 42:2937ad8f1032 515 float q2 = -140.0 * deg2rad;
Jellehierck 42:2937ad8f1032 516 float q2dot;
Jellehierck 42:2937ad8f1032 517
Jellehierck 42:2937ad8f1032 518 float Vx = 0.0; // Desired linear velocity x direction
Jellehierck 42:2937ad8f1032 519 float Vy = 0.0; // Desired linear velocity y direction
Jellehierck 42:2937ad8f1032 520
Jellehierck 42:2937ad8f1032 521 float xe; // Endpoint x position [cm]
Jellehierck 42:2937ad8f1032 522 float ye; // Endpoint y position [cm]
Jellehierck 42:2937ad8f1032 523
Jellehierck 42:2937ad8f1032 524 // Motor angles in starting position
Jellehierck 42:2937ad8f1032 525 const float motor1_init = q1 * gearratio1; // Measured angle motor 1 in initial (starting) position
Jellehierck 42:2937ad8f1032 526 float motor1_ref = motor1_init; // Expected motor angle
Jellehierck 42:2937ad8f1032 527 float motor1_angle = motor1_init; // Actual motor angle
Jellehierck 42:2937ad8f1032 528
Jellehierck 42:2937ad8f1032 529 const float motor2_init = q2 * gearratio2; // Measured angle motor 2 in initial (starting) position
Jellehierck 42:2937ad8f1032 530 float motor2_ref = motor2_init;
Jellehierck 42:2937ad8f1032 531 float motor2_angle = motor2_init;
Jellehierck 42:2937ad8f1032 532
Jellehierck 42:2937ad8f1032 533 // Initialize variables for motor control
Jellehierck 42:2937ad8f1032 534 float motor1offset; // Offset during calibration
Jellehierck 42:2937ad8f1032 535 float omega1; //velocity (rad/s)
Jellehierck 42:2937ad8f1032 536 bool motordir1; // Toggle of motor direction
Jellehierck 42:2937ad8f1032 537 double controlsignal1; // ??
Jellehierck 42:2937ad8f1032 538 float motor1_error; // Error between encoder and reference
Jellehierck 42:2937ad8f1032 539
Jellehierck 42:2937ad8f1032 540 float motor2offset;
Jellehierck 42:2937ad8f1032 541 float omega2;
Jellehierck 42:2937ad8f1032 542 bool motordir2;
Jellehierck 42:2937ad8f1032 543 double controlsignal2;
Jellehierck 42:2937ad8f1032 544 float motor2error;
Jellehierck 42:2937ad8f1032 545
Jellehierck 42:2937ad8f1032 546 // Initialize variables for PID controller
Jellehierck 42:2937ad8f1032 547 float Kp = 0.27; // Proportional gain
Jellehierck 42:2937ad8f1032 548 float Ki = 0.35; // Integral gain
Jellehierck 42:2937ad8f1032 549 float Kd = 0.1; // Derivative gain
Jellehierck 42:2937ad8f1032 550 float Ka = 1.0;
Jellehierck 42:2937ad8f1032 551
Jellehierck 42:2937ad8f1032 552 float u_p1; //
Jellehierck 42:2937ad8f1032 553 float u_i1; //
Jellehierck 42:2937ad8f1032 554 float ux1; //
Jellehierck 42:2937ad8f1032 555 float up1; // Proportional contribution
Jellehierck 42:2937ad8f1032 556 float ek1; //
Jellehierck 42:2937ad8f1032 557 float ei1 = 0.0; // Integral error (starts at 0)
Jellehierck 42:2937ad8f1032 558
Jellehierck 42:2937ad8f1032 559 float u_p2;
Jellehierck 42:2937ad8f1032 560 float u_i2;
Jellehierck 42:2937ad8f1032 561 float ux2;
Jellehierck 42:2937ad8f1032 562 float up2;
Jellehierck 42:2937ad8f1032 563 float ek2;
Jellehierck 42:2937ad8f1032 564 float ei2 = 0.0;
Jellehierck 42:2937ad8f1032 565
Jellehierck 42:2937ad8f1032 566 /*
Jellehierck 42:2937ad8f1032 567 ------------------------------ MOTOR FUNCTIONS ------------------------------
Jellehierck 42:2937ad8f1032 568 */
Jellehierck 42:2937ad8f1032 569 void PID_controller()
Jellehierck 42:2937ad8f1032 570 {
Jellehierck 42:2937ad8f1032 571 // Motor 1
Jellehierck 42:2937ad8f1032 572 static float error_integral1 = 0;
Jellehierck 42:2937ad8f1032 573 static float e_prev1 = motor1_error;
Jellehierck 42:2937ad8f1032 574
Jellehierck 42:2937ad8f1032 575 //Proportional part
Jellehierck 42:2937ad8f1032 576 u_p1 = Kp * motor1_error;
Jellehierck 42:2937ad8f1032 577
Jellehierck 42:2937ad8f1032 578 //Integral part
Jellehierck 42:2937ad8f1032 579 error_integral1 = error_integral1 + ei1 * Ts;
Jellehierck 42:2937ad8f1032 580 u_i1 = Ki * error_integral1;
Jellehierck 42:2937ad8f1032 581
Jellehierck 42:2937ad8f1032 582 //Derivative part
Jellehierck 42:2937ad8f1032 583 float error_derivative1 = (motor1_error - e_prev1) / Ts;
Jellehierck 42:2937ad8f1032 584 float u_d1 = Kd * error_derivative1;
Jellehierck 42:2937ad8f1032 585 e_prev1 = motor1_error;
Jellehierck 42:2937ad8f1032 586
Jellehierck 42:2937ad8f1032 587 // Sum and limit
Jellehierck 42:2937ad8f1032 588 up1 = u_p1 + u_i1 + u_d1;
Jellehierck 42:2937ad8f1032 589 if ( up1 > 1 ) {
Jellehierck 42:2937ad8f1032 590 controlsignal1 = 1;
Jellehierck 42:2937ad8f1032 591 } else if ( up1 < -1 ) {
Jellehierck 42:2937ad8f1032 592 controlsignal1 = -1;
Jellehierck 42:2937ad8f1032 593 } else {
Jellehierck 42:2937ad8f1032 594 controlsignal1 = up1;
Jellehierck 42:2937ad8f1032 595 }
Jellehierck 42:2937ad8f1032 596
Jellehierck 42:2937ad8f1032 597 // To prevent windup
Jellehierck 42:2937ad8f1032 598 ux1 = up1 - controlsignal1;
Jellehierck 42:2937ad8f1032 599 ek1 = Ka * ux1;
Jellehierck 42:2937ad8f1032 600 ei1 = motor1_error - ek1;
Jellehierck 42:2937ad8f1032 601
Jellehierck 42:2937ad8f1032 602 // Motor 2
Jellehierck 42:2937ad8f1032 603 static float error_integral2 = 0;
Jellehierck 42:2937ad8f1032 604 static float e_prev2 = motor2error;
Jellehierck 42:2937ad8f1032 605
Jellehierck 42:2937ad8f1032 606 //Proportional part:
Jellehierck 42:2937ad8f1032 607 u_p2 = Kp * motor2error;
Jellehierck 42:2937ad8f1032 608
Jellehierck 42:2937ad8f1032 609 //Integral part
Jellehierck 42:2937ad8f1032 610 error_integral2 = error_integral2 + ei2 * Ts;
Jellehierck 42:2937ad8f1032 611 u_i2 = Ki * error_integral2;
Jellehierck 42:2937ad8f1032 612
Jellehierck 42:2937ad8f1032 613 //Derivative part
Jellehierck 42:2937ad8f1032 614 float error_derivative2 = (motor2error - e_prev2) / Ts;
Jellehierck 42:2937ad8f1032 615 float u_d2 = Kd * error_derivative2;
Jellehierck 42:2937ad8f1032 616 e_prev2 = motor2error;
Jellehierck 42:2937ad8f1032 617
Jellehierck 42:2937ad8f1032 618 // Sum and limit
Jellehierck 42:2937ad8f1032 619 up2 = u_p2 + u_i2 + u_d2;
Jellehierck 42:2937ad8f1032 620 if ( up2 > 1.0f ) {
Jellehierck 42:2937ad8f1032 621 controlsignal2 = 1.0f;
Jellehierck 42:2937ad8f1032 622 } else if ( up2 < -1 ) {
Jellehierck 42:2937ad8f1032 623 controlsignal2 = -1.0f;
Jellehierck 42:2937ad8f1032 624 } else {
Jellehierck 42:2937ad8f1032 625 controlsignal2 = up2;
Jellehierck 42:2937ad8f1032 626 }
Jellehierck 42:2937ad8f1032 627
Jellehierck 42:2937ad8f1032 628 // To prevent windup
Jellehierck 42:2937ad8f1032 629 ux2 = up2 - controlsignal2;
Jellehierck 42:2937ad8f1032 630 ek2 = Ka * ux2;
Jellehierck 42:2937ad8f1032 631 ei2 = motor2error - ek2;
Jellehierck 42:2937ad8f1032 632 }
Jellehierck 42:2937ad8f1032 633
Jellehierck 42:2937ad8f1032 634 void RKI()
Jellehierck 42:2937ad8f1032 635 {
Jellehierck 42:2937ad8f1032 636 // Derived function for angular velocity of joint angles
Jellehierck 42:2937ad8f1032 637 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 42:2937ad8f1032 638 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 42:2937ad8f1032 639 q1 = q1 + q1dot * Ts;
Jellehierck 42:2937ad8f1032 640 q2 = q2 + q2dot * Ts;
Jellehierck 42:2937ad8f1032 641
Jellehierck 42:2937ad8f1032 642 xe = l1 * cos(q1) + l2 * cos(q1+q2);
Jellehierck 42:2937ad8f1032 643 ye = l1 * sin(q1) + l2 * sin(q1+q2);
Jellehierck 42:2937ad8f1032 644
Jellehierck 42:2937ad8f1032 645 if ( q1 < -5.0f ) {
Jellehierck 42:2937ad8f1032 646 q1 = -5.0;
Jellehierck 42:2937ad8f1032 647 } else if ( q1 > 65.0f*deg2rad ) {
Jellehierck 42:2937ad8f1032 648 q1 = 65.0f * deg2rad;
Jellehierck 42:2937ad8f1032 649 } else {
Jellehierck 42:2937ad8f1032 650 q1 = q1;
Jellehierck 42:2937ad8f1032 651 }
Jellehierck 42:2937ad8f1032 652
Jellehierck 42:2937ad8f1032 653 if ( q2 > -50.0*deg2rad ) {
Jellehierck 42:2937ad8f1032 654 q2 = -50.0 * deg2rad;
Jellehierck 42:2937ad8f1032 655 } else if ( q2 < -138.0*deg2rad ) {
Jellehierck 42:2937ad8f1032 656 q2 = -138.0 * deg2rad;
Jellehierck 42:2937ad8f1032 657 } else {
Jellehierck 42:2937ad8f1032 658 q2 = q2;
Jellehierck 42:2937ad8f1032 659 }
Jellehierck 42:2937ad8f1032 660
Jellehierck 42:2937ad8f1032 661 motor1_ref = 5.5f * q1 + 5.5f * q2;
Jellehierck 42:2937ad8f1032 662 motor2_ref = 2.75f * q1;
Jellehierck 42:2937ad8f1032 663 }
Jellehierck 42:2937ad8f1032 664
Jellehierck 42:2937ad8f1032 665 void motorControl()
Jellehierck 42:2937ad8f1032 666 {
Jellehierck 42:2937ad8f1032 667 counts1 = counts1af - counts1offset;
Jellehierck 42:2937ad8f1032 668 motor1_angle = (counts1 * encoder_factorin / gearbox_ratio) + (motor1_init + motor2_init); // Angle of motor shaft in rad + correctie voor q1 en q2
Jellehierck 42:2937ad8f1032 669 omega1 = deltaCounts1 / Ts * encoder_factorin / gearbox_ratio; // Angular velocity of motor shaft in rad/s
Jellehierck 42:2937ad8f1032 670 motor1_error = motor1_ref - motor1_angle;
Jellehierck 42:2937ad8f1032 671 if ( controlsignal1 < 0 ) {
Jellehierck 42:2937ad8f1032 672 motordir1 = 0;
Jellehierck 42:2937ad8f1032 673 } else {
Jellehierck 42:2937ad8f1032 674 motordir1 = 1;
Jellehierck 42:2937ad8f1032 675 }
Jellehierck 42:2937ad8f1032 676
Jellehierck 42:2937ad8f1032 677 motor1Power.write(abs(controlsignal1));
Jellehierck 42:2937ad8f1032 678 motor1Direction = motordir1;
Jellehierck 42:2937ad8f1032 679
Jellehierck 42:2937ad8f1032 680 counts2 = counts2af - counts2offset;
Jellehierck 42:2937ad8f1032 681 motor2_angle = (counts2 * encoder_factorin / gearbox_ratio) + motor1_init; // Angle of motor shaft in rad + correctie voor q1
Jellehierck 42:2937ad8f1032 682 omega2 = deltaCounts2 / Ts * encoder_factorin / gearbox_ratio; // Angular velocity of motor shaft in rad/s
Jellehierck 42:2937ad8f1032 683 motor2error = motor2_ref-motor2_angle;
Jellehierck 42:2937ad8f1032 684 if ( controlsignal2 < 0 ) {
Jellehierck 42:2937ad8f1032 685 motordir2 = 0;
Jellehierck 42:2937ad8f1032 686 } else {
Jellehierck 42:2937ad8f1032 687 motordir2 = 1;
Jellehierck 42:2937ad8f1032 688 }
Jellehierck 42:2937ad8f1032 689 if (motor_encoder_cal_done == true) {
Jellehierck 42:2937ad8f1032 690 motor2Power.write(abs(controlsignal2));
Jellehierck 42:2937ad8f1032 691 }
Jellehierck 42:2937ad8f1032 692 motor2Direction = motordir2;
Jellehierck 42:2937ad8f1032 693 }
Jellehierck 42:2937ad8f1032 694
Jellehierck 42:2937ad8f1032 695 void motorKillPower()
Jellehierck 42:2937ad8f1032 696 {
Jellehierck 42:2937ad8f1032 697 motor1Power.write(0.0f);
Jellehierck 42:2937ad8f1032 698 motor2Power.write(0.0f);
Jellehierck 42:2937ad8f1032 699 Vx=0.0f;
Jellehierck 42:2937ad8f1032 700 Vy=0.0f;
Jellehierck 42:2937ad8f1032 701 }
Jellehierck 42:2937ad8f1032 702
Jellehierck 42:2937ad8f1032 703 /*
Jellehierck 42:2937ad8f1032 704 ------------------------------ MOTOR SUBSTATE FUNCTIONS ------------------------------
Jellehierck 42:2937ad8f1032 705 */
Jellehierck 42:2937ad8f1032 706
Jellehierck 42:2937ad8f1032 707 void do_motor_wait()
Jellehierck 42:2937ad8f1032 708 {
Jellehierck 42:2937ad8f1032 709 // Entry function
Jellehierck 42:2937ad8f1032 710 if ( motor_state_changed == true ) {
Jellehierck 42:2937ad8f1032 711 motor_state_changed = false;
Jellehierck 42:2937ad8f1032 712 }
Jellehierck 42:2937ad8f1032 713
Jellehierck 42:2937ad8f1032 714 PID_controller();
Jellehierck 42:2937ad8f1032 715 motorControl();
Jellehierck 42:2937ad8f1032 716
Jellehierck 42:2937ad8f1032 717 // State transition guard
Jellehierck 42:2937ad8f1032 718 if ( button2_pressed ) {
Jellehierck 42:2937ad8f1032 719 button2_pressed = false;
Jellehierck 42:2937ad8f1032 720 motor_curr_state = motor_encoder_set; //Beginnen met calibratie
Jellehierck 42:2937ad8f1032 721 motor_state_changed = true;
Jellehierck 42:2937ad8f1032 722 }
Jellehierck 42:2937ad8f1032 723 }
Jellehierck 42:2937ad8f1032 724
Jellehierck 42:2937ad8f1032 725 void do_motor_encoder_set()
Jellehierck 42:2937ad8f1032 726 {
Jellehierck 42:2937ad8f1032 727 // Entry function
Jellehierck 42:2937ad8f1032 728 if ( motor_state_changed == true ) {
Jellehierck 42:2937ad8f1032 729 motor_state_changed = false;
Jellehierck 42:2937ad8f1032 730 // More functions
Jellehierck 42:2937ad8f1032 731 }
Jellehierck 42:2937ad8f1032 732 motor1Power.write(0.0f);
Jellehierck 42:2937ad8f1032 733 motor2Power.write(0.0f);
Jellehierck 42:2937ad8f1032 734 counts1offset = counts1af ;
Jellehierck 42:2937ad8f1032 735 counts2offset = counts2af;
Jellehierck 42:2937ad8f1032 736
Jellehierck 42:2937ad8f1032 737 // State transition guard
Jellehierck 42:2937ad8f1032 738 if ( button2_pressed ) {
Jellehierck 42:2937ad8f1032 739 button2_pressed = false;
Jellehierck 42:2937ad8f1032 740 motor_encoder_cal_done = true;
Jellehierck 42:2937ad8f1032 741 motor_curr_state = motor_finish;
Jellehierck 42:2937ad8f1032 742 motor_state_changed = true;
Jellehierck 42:2937ad8f1032 743 }
Jellehierck 42:2937ad8f1032 744 }
Jellehierck 42:2937ad8f1032 745
Jellehierck 42:2937ad8f1032 746 void do_motor_finish()
Jellehierck 42:2937ad8f1032 747 {
Jellehierck 42:2937ad8f1032 748 // Entry function
Jellehierck 42:2937ad8f1032 749 if ( motor_state_changed == true ) {
Jellehierck 42:2937ad8f1032 750 motor_state_changed = false;
Jellehierck 42:2937ad8f1032 751 }
Jellehierck 42:2937ad8f1032 752
Jellehierck 42:2937ad8f1032 753 // Do stuff until end condition is true
Jellehierck 42:2937ad8f1032 754 PID_controller();
Jellehierck 42:2937ad8f1032 755 motorControl();
Jellehierck 42:2937ad8f1032 756 RKI();
Jellehierck 42:2937ad8f1032 757
Jellehierck 42:2937ad8f1032 758 // State transition guard
Jellehierck 42:2937ad8f1032 759 if ( button2_pressed ) {
Jellehierck 42:2937ad8f1032 760 button2_pressed = false;
Jellehierck 42:2937ad8f1032 761 motor_cal_done = true;
Jellehierck 42:2937ad8f1032 762 motor_curr_state = motor_wait;
Jellehierck 42:2937ad8f1032 763 motor_state_changed = true;
Jellehierck 42:2937ad8f1032 764 }
Jellehierck 42:2937ad8f1032 765 }
Jellehierck 42:2937ad8f1032 766
Jellehierck 42:2937ad8f1032 767 /*
Jellehierck 42:2937ad8f1032 768 ------------------------------ OPERATION GLOBAL VARIABLES & CONSTANTS ------------------------------
Jellehierck 42:2937ad8f1032 769 */
Jellehierck 42:2937ad8f1032 770
Jellehierck 42:2937ad8f1032 771 /*
Jellehierck 42:2937ad8f1032 772 ------------------------------ OPERATION GLOBAL FUNCTIONS ------------------------------
Jellehierck 42:2937ad8f1032 773 */
Jellehierck 42:2937ad8f1032 774 void toggleServo()
Jellehierck 42:2937ad8f1032 775 {
Jellehierck 42:2937ad8f1032 776 if ( operation_showcard == true ) {
Jellehierck 42:2937ad8f1032 777 myServo.SetPosition(2000);
Jellehierck 42:2937ad8f1032 778 operation_showcard = !operation_showcard;
Jellehierck 42:2937ad8f1032 779 }
Jellehierck 42:2937ad8f1032 780
Jellehierck 42:2937ad8f1032 781 else {
Jellehierck 42:2937ad8f1032 782 myServo.SetPosition(1000);
Jellehierck 42:2937ad8f1032 783 operation_showcard = !operation_showcard;
Jellehierck 42:2937ad8f1032 784 }
Jellehierck 42:2937ad8f1032 785 }
Jellehierck 42:2937ad8f1032 786
Jellehierck 42:2937ad8f1032 787
Jellehierck 42:2937ad8f1032 788 /*
Jellehierck 42:2937ad8f1032 789 ------------------------------ OPERATION SUBSTATE FUNCTIONS ------------------------------
Jellehierck 42:2937ad8f1032 790 */
Jellehierck 42:2937ad8f1032 791 void do_operation_wait()
Jellehierck 42:2937ad8f1032 792 {
Jellehierck 42:2937ad8f1032 793 // Entry function
Jellehierck 42:2937ad8f1032 794 if ( operation_state_changed == true ) {
Jellehierck 42:2937ad8f1032 795 operation_state_changed = false;
Jellehierck 42:2937ad8f1032 796 emg_sampleNow = false; // Disable signal sampling in sampleSignals()
Jellehierck 42:2937ad8f1032 797 emg_calibrateNow = false; // Disable calibration functionality in sampleSignals()
Jellehierck 42:2937ad8f1032 798 }
Jellehierck 42:2937ad8f1032 799
Jellehierck 42:2937ad8f1032 800 // Do stuff until end condition is met
Jellehierck 43:1bd5417ded64 801 EMGOperationFunc();
Jellehierck 44:342af9b3c1a0 802
Jellehierck 43:1bd5417ded64 803 Vx = emg1_out * 15.0f * emg1_dir;
Jellehierck 43:1bd5417ded64 804 Vy = emg2_out * 15.0f * emg2_dir;
Jellehierck 44:342af9b3c1a0 805
Jellehierck 43:1bd5417ded64 806 PID_controller();
Jellehierck 43:1bd5417ded64 807 motorControl();
Jellehierck 43:1bd5417ded64 808 RKI();
Jellehierck 44:342af9b3c1a0 809
Jellehierck 42:2937ad8f1032 810 motorKillPower(); // Disables motor outputs
Jellehierck 44:342af9b3c1a0 811
Jellehierck 42:2937ad8f1032 812 if ( switch2_pressed == true) {
Jellehierck 42:2937ad8f1032 813 switch2_pressed = false;
Jellehierck 42:2937ad8f1032 814 toggleServo();
Jellehierck 42:2937ad8f1032 815 }
Jellehierck 42:2937ad8f1032 816
Jellehierck 42:2937ad8f1032 817 // State transition guard
Jellehierck 45:d09040915cfe 818 if ( button1_pressed == true ) {
Jellehierck 45:d09040915cfe 819 button1_pressed = false;
Jellehierck 42:2937ad8f1032 820 operation_curr_state = operation_movement;
Jellehierck 42:2937ad8f1032 821 operation_state_changed = true;
Jellehierck 45:d09040915cfe 822 } else if ( button2_pressed == true ) {
Jellehierck 45:d09040915cfe 823 button2_pressed = false;
Jellehierck 45:d09040915cfe 824 //operation_curr_state = operation_finish; // To move to finished operation mode (not used yet)
Jellehierck 45:d09040915cfe 825 operation_curr_state = operation_wait; // TEMPORARY
Jellehierck 45:d09040915cfe 826 operation_state_changed = true;
Jellehierck 45:d09040915cfe 827 global_curr_state = global_wait; // TEMPORARY move directly to global wait state
Jellehierck 45:d09040915cfe 828 global_state_changed = true; // TEMPORARY move directly to global wait state
Jellehierck 42:2937ad8f1032 829 }
Jellehierck 42:2937ad8f1032 830 }
Jellehierck 42:2937ad8f1032 831
Jellehierck 42:2937ad8f1032 832 void do_operation_movement()
Jellehierck 42:2937ad8f1032 833 {
Jellehierck 42:2937ad8f1032 834 // Entry function
Jellehierck 42:2937ad8f1032 835 if ( operation_state_changed == true ) {
Jellehierck 42:2937ad8f1032 836 operation_state_changed = false;
Jellehierck 42:2937ad8f1032 837 emg_sampleNow = true; // Enable signal sampling in sampleSignals()
Jellehierck 42:2937ad8f1032 838 emg_calibrateNow = false; // Disable calibration functionality in sampleSignals()
Jellehierck 42:2937ad8f1032 839 }
Jellehierck 42:2937ad8f1032 840
Jellehierck 42:2937ad8f1032 841 // Do stuff until end condition is met
Jellehierck 43:1bd5417ded64 842 EMGOperationFunc();
Jellehierck 44:342af9b3c1a0 843
Jellehierck 43:1bd5417ded64 844 Vx = emg1_out * 15.0f * emg1_dir;
Jellehierck 43:1bd5417ded64 845 Vy = emg2_out * 15.0f * emg2_dir;
Jellehierck 44:342af9b3c1a0 846
Jellehierck 43:1bd5417ded64 847 PID_controller();
Jellehierck 43:1bd5417ded64 848 motorControl();
Jellehierck 43:1bd5417ded64 849 RKI();
Jellehierck 44:342af9b3c1a0 850
Jellehierck 42:2937ad8f1032 851 if ( switch2_pressed == true) {
Jellehierck 42:2937ad8f1032 852 switch2_pressed = false;
Jellehierck 42:2937ad8f1032 853 toggleServo();
Jellehierck 42:2937ad8f1032 854 }
Jellehierck 42:2937ad8f1032 855
Jellehierck 42:2937ad8f1032 856 // State transition guard
Jellehierck 45:d09040915cfe 857 if ( button1_pressed == true ) {
Jellehierck 45:d09040915cfe 858 button1_pressed = false;
Jellehierck 42:2937ad8f1032 859 operation_curr_state = operation_wait;
Jellehierck 42:2937ad8f1032 860 operation_state_changed = true;
Jellehierck 45:d09040915cfe 861 } else if ( button2_pressed == true ) {
Jellehierck 45:d09040915cfe 862 button2_pressed = false;
Jellehierck 45:d09040915cfe 863 //operation_curr_state = operation_finish; // To move to finished operation mode (not used yet)
Jellehierck 45:d09040915cfe 864 operation_curr_state = operation_wait; // TEMPORARY
Jellehierck 45:d09040915cfe 865 operation_state_changed = true;
Jellehierck 45:d09040915cfe 866 global_curr_state = global_wait; // TEMPORARY move directly to global wait state
Jellehierck 45:d09040915cfe 867 global_state_changed = true; // TEMPORARY move directly to global wait state
Jellehierck 42:2937ad8f1032 868 }
Jellehierck 42:2937ad8f1032 869 }
Jellehierck 42:2937ad8f1032 870
Jellehierck 45:d09040915cfe 871 void do_operation_finish() // NOT USED YET
Jellehierck 42:2937ad8f1032 872 {
Jellehierck 42:2937ad8f1032 873 // Entry function
Jellehierck 42:2937ad8f1032 874 if ( operation_state_changed == true ) {
Jellehierck 42:2937ad8f1032 875 operation_state_changed = false;
Jellehierck 42:2937ad8f1032 876 emg_sampleNow = false; // Enable signal sampling in sampleSignals()
Jellehierck 42:2937ad8f1032 877 emg_calibrateNow = false; // Disable calibration functionality in sampleSignals()
Jellehierck 42:2937ad8f1032 878 }
Jellehierck 42:2937ad8f1032 879
Jellehierck 42:2937ad8f1032 880 // Do stuff until end condition is met
Jellehierck 43:1bd5417ded64 881 EMGOperationFunc();
Jellehierck 44:342af9b3c1a0 882
Jellehierck 43:1bd5417ded64 883 Vx = emg1_out * 15.0f * emg1_dir;
Jellehierck 43:1bd5417ded64 884 Vy = emg2_out * 15.0f * emg2_dir;
Jellehierck 44:342af9b3c1a0 885
Jellehierck 43:1bd5417ded64 886 PID_controller();
Jellehierck 43:1bd5417ded64 887 motorControl();
Jellehierck 43:1bd5417ded64 888 RKI();
Jellehierck 44:342af9b3c1a0 889
Jellehierck 42:2937ad8f1032 890 // Function to move to starting position
Jellehierck 42:2937ad8f1032 891
Jellehierck 42:2937ad8f1032 892 // State transition guard
Jellehierck 45:d09040915cfe 893 if ( button2_pressed == true ) {
Jellehierck 45:d09040915cfe 894 button2_pressed = false;
Jellehierck 42:2937ad8f1032 895 operation_curr_state = operation_wait;
Jellehierck 42:2937ad8f1032 896 operation_state_changed = true;
Jellehierck 44:342af9b3c1a0 897
Jellehierck 42:2937ad8f1032 898 global_curr_state = global_wait;
Jellehierck 42:2937ad8f1032 899 global_state_changed = true;
Jellehierck 42:2937ad8f1032 900 }
Jellehierck 42:2937ad8f1032 901 }
Jellehierck 42:2937ad8f1032 902
Jellehierck 38:8b597ab8344f 903 /*
Jellehierck 38:8b597ab8344f 904 ------------------------------ EMG SUBSTATE MACHINE ------------------------------
Jellehierck 38:8b597ab8344f 905 */
Jellehierck 38:8b597ab8344f 906
Jellehierck 38:8b597ab8344f 907 void emg_state_machine()
Jellehierck 38:8b597ab8344f 908 {
Jellehierck 38:8b597ab8344f 909 switch(emg_curr_state) {
Jellehierck 38:8b597ab8344f 910 case emg_wait:
Jellehierck 38:8b597ab8344f 911 do_emg_wait();
Jellehierck 38:8b597ab8344f 912 break;
Jellehierck 38:8b597ab8344f 913 case emg_cal_MVC:
Jellehierck 38:8b597ab8344f 914 do_emg_cal();
Jellehierck 38:8b597ab8344f 915 break;
Jellehierck 38:8b597ab8344f 916 case emg_cal_rest:
Jellehierck 38:8b597ab8344f 917 do_emg_cal();
Jellehierck 38:8b597ab8344f 918 break;
Jellehierck 38:8b597ab8344f 919 case emg_operation:
Jellehierck 38:8b597ab8344f 920 do_emg_operation();
Jellehierck 38:8b597ab8344f 921 break;
Jellehierck 38:8b597ab8344f 922 }
Jellehierck 38:8b597ab8344f 923 }
Jellehierck 7:7a088536f1c9 924
Jellehierck 15:421d3d9c563b 925 /*
Jellehierck 40:c6dffb676350 926 ------------------------------ MOTOR SUBSTATE MACHINE ------------------------------
Jellehierck 40:c6dffb676350 927 */
Jellehierck 40:c6dffb676350 928
Jellehierck 40:c6dffb676350 929 void motor_state_machine()
Jellehierck 40:c6dffb676350 930 {
Jellehierck 40:c6dffb676350 931 switch(motor_curr_state) {
Jellehierck 40:c6dffb676350 932 case motor_wait:
Jellehierck 40:c6dffb676350 933 do_motor_wait();
Jellehierck 40:c6dffb676350 934 break;
Jellehierck 42:2937ad8f1032 935 case motor_encoder_set:
Jellehierck 42:2937ad8f1032 936 do_motor_encoder_set();
Jellehierck 40:c6dffb676350 937 break;
Jellehierck 40:c6dffb676350 938 case motor_finish:
Jellehierck 40:c6dffb676350 939 do_motor_finish();
Jellehierck 40:c6dffb676350 940 break;
Jellehierck 40:c6dffb676350 941 }
Jellehierck 40:c6dffb676350 942 }
Jellehierck 40:c6dffb676350 943
Jellehierck 40:c6dffb676350 944 /*
Jellehierck 42:2937ad8f1032 945 ------------------------------ OPERATION SUBSTATE MACHINE ------------------------------
Jellehierck 42:2937ad8f1032 946 */
Jellehierck 42:2937ad8f1032 947
Jellehierck 42:2937ad8f1032 948 void operation_state_machine()
Jellehierck 42:2937ad8f1032 949 {
Jellehierck 42:2937ad8f1032 950 switch(operation_curr_state) {
Jellehierck 42:2937ad8f1032 951 case operation_wait:
Jellehierck 42:2937ad8f1032 952 do_operation_wait();
Jellehierck 42:2937ad8f1032 953 break;
Jellehierck 42:2937ad8f1032 954 case operation_movement:
Jellehierck 42:2937ad8f1032 955 do_operation_movement();
Jellehierck 42:2937ad8f1032 956 break;
Jellehierck 42:2937ad8f1032 957 case operation_finish:
Jellehierck 42:2937ad8f1032 958 do_operation_finish();
Jellehierck 42:2937ad8f1032 959 break;
Jellehierck 42:2937ad8f1032 960 }
Jellehierck 42:2937ad8f1032 961 }
Jellehierck 42:2937ad8f1032 962
Jellehierck 42:2937ad8f1032 963 /*
Jellehierck 43:1bd5417ded64 964 ------------------------------ DEMO SUBSTATE FUNCTIONS ------------------------------
Jellehierck 43:1bd5417ded64 965 */
Jellehierck 44:342af9b3c1a0 966 void do_demo_wait()
Jellehierck 44:342af9b3c1a0 967 {
Jellehierck 43:1bd5417ded64 968 // Entry function
Jellehierck 43:1bd5417ded64 969 if ( demo_state_changed == true ) {
Jellehierck 43:1bd5417ded64 970 demo_state_changed = false;
Jellehierck 43:1bd5417ded64 971 }
Jellehierck 43:1bd5417ded64 972
Jellehierck 43:1bd5417ded64 973 // Do nothing until end condition is met
Jellehierck 44:342af9b3c1a0 974
Jellehierck 43:1bd5417ded64 975 // State transition guard
Jellehierck 44:342af9b3c1a0 976 if ( button1_pressed == true ) { // demo_XY
Jellehierck 43:1bd5417ded64 977 button1_pressed = false;
Jellehierck 43:1bd5417ded64 978 demo_curr_state = demo_XY;
Jellehierck 43:1bd5417ded64 979 demo_state_changed = true;
Jellehierck 43:1bd5417ded64 980 // More functions
Jellehierck 44:342af9b3c1a0 981 } else if (button2_pressed == true) { // Return to global wait
Jellehierck 43:1bd5417ded64 982 button2_pressed = false;
Jellehierck 43:1bd5417ded64 983 demo_curr_state = demo_wait;
Jellehierck 43:1bd5417ded64 984 demo_state_changed = true;
Jellehierck 43:1bd5417ded64 985 motor_cal_done = false; // Disables motor calibration again (robot is probably not in reference position)
Jellehierck 43:1bd5417ded64 986 global_curr_state = global_wait;
Jellehierck 43:1bd5417ded64 987 global_state_changed = true;
Jellehierck 43:1bd5417ded64 988 }
Jellehierck 43:1bd5417ded64 989 }
Jellehierck 43:1bd5417ded64 990
Jellehierck 44:342af9b3c1a0 991 void do_demo_motor_cal()
Jellehierck 44:342af9b3c1a0 992 {
Jellehierck 43:1bd5417ded64 993 // Entry function
Jellehierck 43:1bd5417ded64 994 if ( demo_state_changed == true ) {
Jellehierck 43:1bd5417ded64 995 demo_state_changed = false;
Jellehierck 43:1bd5417ded64 996 }
Jellehierck 43:1bd5417ded64 997
Jellehierck 43:1bd5417ded64 998 // Do stuff until end condition is met
Jellehierck 43:1bd5417ded64 999 motor_state_machine();
Jellehierck 43:1bd5417ded64 1000
Jellehierck 43:1bd5417ded64 1001 // State transition guard
Jellehierck 44:342af9b3c1a0 1002 if ( motor_cal_done == true ) { // demo_wait
Jellehierck 43:1bd5417ded64 1003 demo_curr_state = demo_wait;
Jellehierck 43:1bd5417ded64 1004 demo_state_changed = true;
Jellehierck 43:1bd5417ded64 1005 }
Jellehierck 43:1bd5417ded64 1006 }
Jellehierck 43:1bd5417ded64 1007
Jellehierck 44:342af9b3c1a0 1008 void do_demo_XY()
Jellehierck 44:342af9b3c1a0 1009 {
Jellehierck 43:1bd5417ded64 1010 // Entry function
Jellehierck 43:1bd5417ded64 1011 if ( demo_state_changed == true ) {
Jellehierck 43:1bd5417ded64 1012 demo_state_changed = false;
Jellehierck 43:1bd5417ded64 1013 }
Jellehierck 43:1bd5417ded64 1014
Jellehierck 43:1bd5417ded64 1015 // Do stuff until end condition is met
Jellehierck 43:1bd5417ded64 1016 static float t = 0;
Jellehierck 43:1bd5417ded64 1017 Vy = 10.0f * sin(1.0f*t);
Jellehierck 43:1bd5417ded64 1018 Vx = 0.0f;
Jellehierck 43:1bd5417ded64 1019 t += Ts;
Jellehierck 44:342af9b3c1a0 1020
Jellehierck 43:1bd5417ded64 1021 PID_controller();
Jellehierck 43:1bd5417ded64 1022 motorControl();
Jellehierck 43:1bd5417ded64 1023 RKI();
Jellehierck 43:1bd5417ded64 1024
Jellehierck 43:1bd5417ded64 1025 // State transition guard
Jellehierck 44:342af9b3c1a0 1026 if ( button1_pressed == true ) { // demo_wait
Jellehierck 44:342af9b3c1a0 1027 button1_pressed = false;
Jellehierck 43:1bd5417ded64 1028 demo_curr_state = demo_wait;
Jellehierck 43:1bd5417ded64 1029 demo_state_changed = true;
Jellehierck 43:1bd5417ded64 1030 }
Jellehierck 43:1bd5417ded64 1031 }
Jellehierck 43:1bd5417ded64 1032
Jellehierck 43:1bd5417ded64 1033 /*
Jellehierck 43:1bd5417ded64 1034 ------------------------------ DEMO SUBSTATE MACHINE ------------------------------
Jellehierck 43:1bd5417ded64 1035 */
Jellehierck 43:1bd5417ded64 1036
Jellehierck 43:1bd5417ded64 1037 void demo_state_machine()
Jellehierck 43:1bd5417ded64 1038 {
Jellehierck 43:1bd5417ded64 1039 switch(demo_curr_state) {
Jellehierck 43:1bd5417ded64 1040 case demo_wait:
Jellehierck 43:1bd5417ded64 1041 do_demo_wait();
Jellehierck 43:1bd5417ded64 1042 break;
Jellehierck 43:1bd5417ded64 1043 case demo_motor_cal:
Jellehierck 43:1bd5417ded64 1044 do_demo_motor_cal();
Jellehierck 43:1bd5417ded64 1045 break;
Jellehierck 43:1bd5417ded64 1046 case demo_XY:
Jellehierck 43:1bd5417ded64 1047 do_demo_XY();
Jellehierck 43:1bd5417ded64 1048 break;
Jellehierck 43:1bd5417ded64 1049 }
Jellehierck 43:1bd5417ded64 1050 }
Jellehierck 43:1bd5417ded64 1051
Jellehierck 43:1bd5417ded64 1052 /*
Jellehierck 37:806c7c8381a7 1053 ------------------------------ GLOBAL STATE FUNCTIONS ------------------------------
Jellehierck 15:421d3d9c563b 1054 */
Jellehierck 25:a1be4cf2ab0b 1055 /* ALL STATES HAVE THE FOLLOWING FORM:
Jellehierck 25:a1be4cf2ab0b 1056 void do_state_function() {
Jellehierck 25:a1be4cf2ab0b 1057 // Entry function
Jellehierck 37:806c7c8381a7 1058 if ( global_state_changed == true ) {
Jellehierck 37:806c7c8381a7 1059 global_state_changed = false;
Jellehierck 25:a1be4cf2ab0b 1060 // More functions
Jellehierck 25:a1be4cf2ab0b 1061 }
Jellehierck 25:a1be4cf2ab0b 1062
Jellehierck 25:a1be4cf2ab0b 1063 // Do stuff until end condition is met
Jellehierck 25:a1be4cf2ab0b 1064 doStuff();
Jellehierck 25:a1be4cf2ab0b 1065
Jellehierck 25:a1be4cf2ab0b 1066 // State transition guard
Jellehierck 25:a1be4cf2ab0b 1067 if ( endCondition == true ) {
Jellehierck 37:806c7c8381a7 1068 global_curr_state = next_state;
Jellehierck 37:806c7c8381a7 1069 global_state_changed = true;
Jellehierck 25:a1be4cf2ab0b 1070 // More functions
Jellehierck 25:a1be4cf2ab0b 1071 }
Jellehierck 25:a1be4cf2ab0b 1072 }
Jellehierck 25:a1be4cf2ab0b 1073 */
Jellehierck 25:a1be4cf2ab0b 1074
Jellehierck 37:806c7c8381a7 1075 // FAILURE MODE
Jellehierck 37:806c7c8381a7 1076 void do_global_failure()
Jellehierck 7:7a088536f1c9 1077 {
Jellehierck 37:806c7c8381a7 1078 // Entry function
Jellehierck 37:806c7c8381a7 1079 if ( global_state_changed == true ) {
Jellehierck 37:806c7c8381a7 1080 global_state_changed = false;
Jellehierck 25:a1be4cf2ab0b 1081
Jellehierck 37:806c7c8381a7 1082 failure_mode = true; // Set failure mode
Jellehierck 22:9079c6c0d898 1083 }
Jellehierck 37:806c7c8381a7 1084
Jellehierck 37:806c7c8381a7 1085 // Do stuff until end condition is met
Jellehierck 42:2937ad8f1032 1086 motorKillPower();
Jellehierck 37:806c7c8381a7 1087
Jellehierck 37:806c7c8381a7 1088 // State transition guard
Jellehierck 37:806c7c8381a7 1089 if ( false ) { // Never move to other state
Jellehierck 37:806c7c8381a7 1090 global_curr_state = global_wait;
Jellehierck 37:806c7c8381a7 1091 global_state_changed = true;
Jellehierck 37:806c7c8381a7 1092 }
Jellehierck 25:a1be4cf2ab0b 1093 }
Jellehierck 25:a1be4cf2ab0b 1094
Jellehierck 37:806c7c8381a7 1095 // DEMO MODE
Jellehierck 37:806c7c8381a7 1096 void do_global_demo()
Jellehierck 25:a1be4cf2ab0b 1097 {
Jellehierck 25:a1be4cf2ab0b 1098 // Entry function
Jellehierck 37:806c7c8381a7 1099 if ( global_state_changed == true ) {
Jellehierck 37:806c7c8381a7 1100 global_state_changed = false;
Jellehierck 49:80970a03083b 1101
Jellehierck 49:80970a03083b 1102 if ( motor_cal_done == true ) {
Jellehierck 49:80970a03083b 1103 demo_curr_state = demo_wait;
Jellehierck 49:80970a03083b 1104 } else if (motor_cal_done == false ) {
Jellehierck 49:80970a03083b 1105 demo_curr_state = demo_motor_cal;
Jellehierck 49:80970a03083b 1106 }
Jellehierck 49:80970a03083b 1107 demo_state_changed = true;
Jellehierck 37:806c7c8381a7 1108 }
Jellehierck 37:806c7c8381a7 1109
Jellehierck 37:806c7c8381a7 1110 // Do stuff until end condition is met
Jellehierck 46:8a8fa8e602a1 1111 demo_state_machine();
Jellehierck 46:8a8fa8e602a1 1112
Jellehierck 46:8a8fa8e602a1 1113 scope.set(0, emg1 );
Jellehierck 46:8a8fa8e602a1 1114 scope.set(1, Vx );
Jellehierck 46:8a8fa8e602a1 1115 scope.set(2, emg2 );
Jellehierck 46:8a8fa8e602a1 1116 scope.set(3, Vy );
Jellehierck 35:e82834e62e44 1117
Jellehierck 37:806c7c8381a7 1118 // State transition guard
Jellehierck 37:806c7c8381a7 1119 if ( switch2_pressed == true ) {
Jellehierck 37:806c7c8381a7 1120 switch2_pressed = false;
Jellehierck 37:806c7c8381a7 1121 global_curr_state = global_wait;
Jellehierck 37:806c7c8381a7 1122 global_state_changed = true;
Jellehierck 37:806c7c8381a7 1123 }
Jellehierck 37:806c7c8381a7 1124 }
Jellehierck 37:806c7c8381a7 1125
Jellehierck 37:806c7c8381a7 1126 // WAIT MODE
Jellehierck 37:806c7c8381a7 1127 void do_global_wait()
Jellehierck 37:806c7c8381a7 1128 {
Jellehierck 37:806c7c8381a7 1129 // Entry function
Jellehierck 37:806c7c8381a7 1130 if ( global_state_changed == true ) {
Jellehierck 37:806c7c8381a7 1131 global_state_changed = false;
Jellehierck 25:a1be4cf2ab0b 1132 }
Jellehierck 25:a1be4cf2ab0b 1133
Jellehierck 27:f18da01093c9 1134 // Do nothing until end condition is met
Jellehierck 25:a1be4cf2ab0b 1135
Jellehierck 37:806c7c8381a7 1136 // State transition guard
Jellehierck 37:806c7c8381a7 1137 if ( switch2_pressed == true ) { // DEMO MODE
Jellehierck 37:806c7c8381a7 1138 switch2_pressed = false;
Jellehierck 37:806c7c8381a7 1139 global_curr_state = global_demo;
Jellehierck 37:806c7c8381a7 1140 global_state_changed = true;
Jellehierck 31:b5188b6d45db 1141
Jellehierck 37:806c7c8381a7 1142 } else if ( button1_pressed == true ) { // EMG CALIBRATION
Jellehierck 37:806c7c8381a7 1143 button1_pressed = false;
Jellehierck 38:8b597ab8344f 1144 global_curr_state = global_emg_cal;
Jellehierck 37:806c7c8381a7 1145 global_state_changed = true;
Jellehierck 31:b5188b6d45db 1146
Jellehierck 37:806c7c8381a7 1147 } else if ( button2_pressed == true ) { // MOTOR CALIBRATION
Jellehierck 37:806c7c8381a7 1148 button2_pressed = false;
Jellehierck 38:8b597ab8344f 1149 global_curr_state = global_motor_cal;
Jellehierck 37:806c7c8381a7 1150 global_state_changed = true;
Jellehierck 42:2937ad8f1032 1151
Jellehierck 39:f9042483b921 1152 } else if ( emg_cal_done && motor_cal_done ) { // OPERATION MODE
Jellehierck 39:f9042483b921 1153 global_curr_state = global_operation;
Jellehierck 39:f9042483b921 1154 global_state_changed = true;
Jellehierck 25:a1be4cf2ab0b 1155 }
Jellehierck 7:7a088536f1c9 1156 }
Jellehierck 7:7a088536f1c9 1157
Jellehierck 37:806c7c8381a7 1158 // EMG CALIBRATION MODE
Jellehierck 38:8b597ab8344f 1159 void do_global_emg_cal()
Jellehierck 21:e4569b47945e 1160 {
Jellehierck 37:806c7c8381a7 1161 // Entry function
Jellehierck 37:806c7c8381a7 1162 if ( global_state_changed == true ) {
Jellehierck 37:806c7c8381a7 1163 global_state_changed = false;
Jellehierck 22:9079c6c0d898 1164 }
Jellehierck 7:7a088536f1c9 1165
Jellehierck 39:f9042483b921 1166 // Run EMG state machine until emg_cal_done flag is true
Jellehierck 39:f9042483b921 1167 emg_state_machine();
Jellehierck 31:b5188b6d45db 1168
Jellehierck 29:f51683a6cbbf 1169 // State transition guard
Jellehierck 39:f9042483b921 1170 if ( emg_cal_done == true ) { // WAIT MODE
Jellehierck 37:806c7c8381a7 1171 global_curr_state = global_wait;
Jellehierck 37:806c7c8381a7 1172 global_state_changed = true;
Jellehierck 25:a1be4cf2ab0b 1173 }
Jellehierck 25:a1be4cf2ab0b 1174 }
Jellehierck 23:8a0a0b959af1 1175
Jellehierck 37:806c7c8381a7 1176 // MOTOR CALIBRATION MODE
Jellehierck 38:8b597ab8344f 1177 void do_global_motor_cal()
Jellehierck 26:7e81c7db6e7a 1178 {
Jellehierck 25:a1be4cf2ab0b 1179 // Entry function
Jellehierck 37:806c7c8381a7 1180 if ( global_state_changed == true ) {
Jellehierck 37:806c7c8381a7 1181 global_state_changed = false;
Jellehierck 25:a1be4cf2ab0b 1182 }
Jellehierck 25:a1be4cf2ab0b 1183
Jellehierck 25:a1be4cf2ab0b 1184 // Do stuff until end condition is met
Jellehierck 40:c6dffb676350 1185 motor_state_machine();
Jellehierck 28:59e8266f4633 1186
Jellehierck 25:a1be4cf2ab0b 1187 // State transition guard
Jellehierck 41:8e8141f355af 1188 if ( motor_cal_done == true ) { // WAIT MODE
Jellehierck 37:806c7c8381a7 1189 global_curr_state = global_wait;
Jellehierck 37:806c7c8381a7 1190 global_state_changed = true;
Jellehierck 23:8a0a0b959af1 1191 }
Jellehierck 23:8a0a0b959af1 1192 }
Jellehierck 23:8a0a0b959af1 1193
Jellehierck 37:806c7c8381a7 1194 // OPERATION MODE
Jellehierck 37:806c7c8381a7 1195 void do_global_operation()
Jellehierck 37:806c7c8381a7 1196 {
Jellehierck 37:806c7c8381a7 1197 // Entry function
Jellehierck 37:806c7c8381a7 1198 if ( global_state_changed == true ) {
Jellehierck 37:806c7c8381a7 1199 global_state_changed = false;
Jellehierck 40:c6dffb676350 1200
Jellehierck 39:f9042483b921 1201 emg_sampleNow = true; // Enable signal sampling in sampleSignals()
Jellehierck 39:f9042483b921 1202 emg_calibrateNow = false; // Disable calibration functionality in sampleSignals()
Jellehierck 37:806c7c8381a7 1203 }
Jellehierck 37:806c7c8381a7 1204
Jellehierck 37:806c7c8381a7 1205 // Do stuff until end condition is met
Jellehierck 43:1bd5417ded64 1206 operation_state_machine();
Jellehierck 42:2937ad8f1032 1207
Jellehierck 39:f9042483b921 1208 // Set HIDScope outputs
Jellehierck 39:f9042483b921 1209 scope.set(0, emg1 );
Jellehierck 41:8e8141f355af 1210 scope.set(1, Vx );
Jellehierck 41:8e8141f355af 1211 scope.set(2, emg2 );
Jellehierck 41:8e8141f355af 1212 scope.set(3, Vy );
Jellehierck 39:f9042483b921 1213 scope.send();
Jellehierck 39:f9042483b921 1214
Jellehierck 39:f9042483b921 1215 led_g = !led_g;
Jellehierck 37:806c7c8381a7 1216
Jellehierck 37:806c7c8381a7 1217 // State transition guard
Jellehierck 37:806c7c8381a7 1218 if ( false ) { // Always stay in operation mode (can be changed)
Jellehierck 37:806c7c8381a7 1219 global_curr_state = global_wait;
Jellehierck 37:806c7c8381a7 1220 global_state_changed = true;
Jellehierck 37:806c7c8381a7 1221 }
Jellehierck 37:806c7c8381a7 1222 }
Jellehierck 23:8a0a0b959af1 1223 /*
Jellehierck 37:806c7c8381a7 1224 ------------------------------ GLOBAL STATE MACHINE ------------------------------
Jellehierck 23:8a0a0b959af1 1225 */
Jellehierck 37:806c7c8381a7 1226 void global_state_machine()
Jellehierck 23:8a0a0b959af1 1227 {
Jellehierck 37:806c7c8381a7 1228 switch(global_curr_state) {
Jellehierck 37:806c7c8381a7 1229 case global_failure:
Jellehierck 37:806c7c8381a7 1230 do_global_failure();
Jellehierck 23:8a0a0b959af1 1231 break;
Jellehierck 37:806c7c8381a7 1232 case global_wait:
Jellehierck 37:806c7c8381a7 1233 do_global_wait();
Jellehierck 37:806c7c8381a7 1234 break;
Jellehierck 38:8b597ab8344f 1235 case global_emg_cal:
Jellehierck 38:8b597ab8344f 1236 do_global_emg_cal();
Jellehierck 23:8a0a0b959af1 1237 break;
Jellehierck 38:8b597ab8344f 1238 case global_motor_cal:
Jellehierck 38:8b597ab8344f 1239 do_global_motor_cal();
Jellehierck 23:8a0a0b959af1 1240 break;
Jellehierck 37:806c7c8381a7 1241 case global_operation:
Jellehierck 37:806c7c8381a7 1242 do_global_operation();
Jellehierck 37:806c7c8381a7 1243 break;
Jellehierck 37:806c7c8381a7 1244 case global_demo:
Jellehierck 37:806c7c8381a7 1245 do_global_demo();
Jellehierck 23:8a0a0b959af1 1246 break;
Jellehierck 23:8a0a0b959af1 1247 }
Jellehierck 23:8a0a0b959af1 1248 }
Jellehierck 23:8a0a0b959af1 1249
Jellehierck 38:8b597ab8344f 1250 /*
Jellehierck 38:8b597ab8344f 1251 ------------------------------ READ SAMPLES ------------------------------
Jellehierck 38:8b597ab8344f 1252 */
Jellehierck 38:8b597ab8344f 1253 void sampleSignals()
Jellehierck 38:8b597ab8344f 1254 {
Jellehierck 38:8b597ab8344f 1255 if (emg_sampleNow == true) { // This ticker only samples if the sample flag is true, to prevent unnecessary computations
Jellehierck 38:8b597ab8344f 1256 // Read EMG inputs
Jellehierck 38:8b597ab8344f 1257 emg1 = emg1_in.read();
Jellehierck 38:8b597ab8344f 1258 emg2 = emg2_in.read();
Jellehierck 38:8b597ab8344f 1259 emg3 = emg3_in.read();
Jellehierck 45:d09040915cfe 1260 emg4 = emg4_in.read();
Jellehierck 38:8b597ab8344f 1261
Jellehierck 38:8b597ab8344f 1262 double emg1_n = bqc1_notch.step( emg1 ); // Filter notch
Jellehierck 38:8b597ab8344f 1263 double emg1_hp = bqc1_high.step( emg1_n ); // Filter highpass
Jellehierck 38:8b597ab8344f 1264 double emg1_rectify = fabs( emg1_hp ); // Rectify
Jellehierck 38:8b597ab8344f 1265 emg1_env = bqc1_low.step( emg1_rectify ); // Filter lowpass (completes envelope)
Jellehierck 38:8b597ab8344f 1266
Jellehierck 38:8b597ab8344f 1267 double emg2_n = bqc2_notch.step( emg2 ); // Filter notch
Jellehierck 38:8b597ab8344f 1268 double emg2_hp = bqc2_high.step( emg2_n ); // Filter highpass
Jellehierck 38:8b597ab8344f 1269 double emg2_rectify = fabs( emg2_hp ); // Rectify
Jellehierck 38:8b597ab8344f 1270 emg2_env = bqc2_low.step( emg2_rectify ); // Filter lowpass (completes envelope)
Jellehierck 38:8b597ab8344f 1271
Jellehierck 38:8b597ab8344f 1272 double emg3_n = bqc3_notch.step( emg3 ); // Filter notch
Jellehierck 38:8b597ab8344f 1273 double emg3_hp = bqc3_high.step( emg3_n ); // Filter highpass
Jellehierck 38:8b597ab8344f 1274 double emg3_rectify = fabs( emg3_hp ); // Rectify
Jellehierck 38:8b597ab8344f 1275 emg3_env = bqc3_low.step( emg3_rectify ); // Filter lowpass (completes envelope)
Jellehierck 45:d09040915cfe 1276
Jellehierck 45:d09040915cfe 1277 double emg4_n = bqc4_notch.step( emg4 ); // Filter notch
Jellehierck 45:d09040915cfe 1278 double emg4_hp = bqc4_high.step( emg4_n ); // Filter highpass
Jellehierck 45:d09040915cfe 1279 double emg4_rectify = fabs( emg4_hp ); // Rectify
Jellehierck 45:d09040915cfe 1280 emg4_env = bqc4_low.step( emg4_rectify ); // Filter lowpass (completes envelope)
Jellehierck 38:8b597ab8344f 1281
Jellehierck 38:8b597ab8344f 1282 if (emg_calibrateNow == true) { // Only add values to EMG vectors if calibration flag is true
Jellehierck 38:8b597ab8344f 1283 emg1_cal.push_back(emg1_env); // Add values to calibration vector
Jellehierck 38:8b597ab8344f 1284 emg2_cal.push_back(emg2_env); // Add values to calibration vector
Jellehierck 38:8b597ab8344f 1285 emg3_cal.push_back(emg3_env); // Add values to calibration vector
Jellehierck 45:d09040915cfe 1286 emg4_cal.push_back(emg4_env); // Add values to calibration vector
Jellehierck 38:8b597ab8344f 1287 }
Jellehierck 38:8b597ab8344f 1288 }
Jellehierck 40:c6dffb676350 1289
Jellehierck 40:c6dffb676350 1290 counts1af = encoder1.getPulses();
Jellehierck 40:c6dffb676350 1291 deltaCounts1 = counts1af - countsPrev1;
Jellehierck 40:c6dffb676350 1292 countsPrev1 = counts1af;
Jellehierck 40:c6dffb676350 1293
Jellehierck 40:c6dffb676350 1294 counts2af = encoder2.getPulses();
Jellehierck 40:c6dffb676350 1295 deltaCounts2 = counts2af - countsPrev2;
Jellehierck 40:c6dffb676350 1296 countsPrev2 = counts2af;
Jellehierck 38:8b597ab8344f 1297 }
Jellehierck 37:806c7c8381a7 1298
Jellehierck 37:806c7c8381a7 1299 /*
Jellehierck 37:806c7c8381a7 1300 ------------------------------ GLOBAL PROGRAM LOOP ------------------------------
Jellehierck 37:806c7c8381a7 1301 */
Jellehierck 25:a1be4cf2ab0b 1302 void tickGlobalFunc()
Jellehierck 25:a1be4cf2ab0b 1303 {
Jellehierck 38:8b597ab8344f 1304 sampleSignals();
Jellehierck 37:806c7c8381a7 1305 global_state_machine();
Jellehierck 25:a1be4cf2ab0b 1306 // controller();
Jellehierck 25:a1be4cf2ab0b 1307 // outputToMotors();
Jellehierck 25:a1be4cf2ab0b 1308 }
Jellehierck 25:a1be4cf2ab0b 1309
Jellehierck 37:806c7c8381a7 1310 /*
Jellehierck 37:806c7c8381a7 1311 ------------------------------ MAIN FUNCTION ------------------------------
Jellehierck 37:806c7c8381a7 1312 */
Jellehierck 39:f9042483b921 1313 int main()
Jellehierck 23:8a0a0b959af1 1314 {
Jellehierck 23:8a0a0b959af1 1315 pc.baud(115200); // MODSERIAL rate
Jellehierck 23:8a0a0b959af1 1316 pc.printf("Starting\r\n");
Jellehierck 23:8a0a0b959af1 1317
Jellehierck 37:806c7c8381a7 1318 global_curr_state = global_wait; // Start off in EMG Wait state
Jellehierck 34:13fac02ef324 1319 tickGlobal.attach( &tickGlobalFunc, Ts ); // Start global ticker
Jellehierck 8:ea3de43c9e8b 1320
Jellehierck 38:8b597ab8344f 1321 // ---------- Attach filters ----------
Jellehierck 38:8b597ab8344f 1322 bqc1_notch.add( &bq1_notch );
Jellehierck 38:8b597ab8344f 1323 bqc1_high.add( &bq1_H1 ).add( &bq1_H2 );
Jellehierck 38:8b597ab8344f 1324 bqc1_low.add( &bq1_L1 ).add( &bq1_L2 );
Jellehierck 38:8b597ab8344f 1325
Jellehierck 38:8b597ab8344f 1326 bqc2_notch.add( &bq2_notch );
Jellehierck 38:8b597ab8344f 1327 bqc2_high.add( &bq2_H1 ).add( &bq2_H2 );
Jellehierck 38:8b597ab8344f 1328 bqc2_low.add( &bq2_L1 ).add( &bq2_L2 );
Jellehierck 38:8b597ab8344f 1329
Jellehierck 38:8b597ab8344f 1330 bqc3_notch.add( &bq3_notch );
Jellehierck 38:8b597ab8344f 1331 bqc3_high.add( &bq3_H1 ).add( &bq3_H2 );
Jellehierck 38:8b597ab8344f 1332 bqc3_low.add( &bq3_L1 ).add( &bq3_L2 );
Jellehierck 45:d09040915cfe 1333
Jellehierck 45:d09040915cfe 1334 bqc4_notch.add( &bq4_notch );
Jellehierck 45:d09040915cfe 1335 bqc4_high.add( &bq4_H1 ).add( &bq4_H2 );
Jellehierck 45:d09040915cfe 1336 bqc4_low.add( &bq4_L1 ).add( &bq4_L2 );
Jellehierck 38:8b597ab8344f 1337
Jellehierck 38:8b597ab8344f 1338 // ---------- Attach buttons ----------
Jellehierck 37:806c7c8381a7 1339 button1.fall( &button1Press );
Jellehierck 37:806c7c8381a7 1340 button2.fall( &button2Press );
Jellehierck 37:806c7c8381a7 1341 switch2.fall( &switch2Press );
Jellehierck 37:806c7c8381a7 1342 switch3.fall( &switch3Press );
Jellehierck 42:2937ad8f1032 1343
Jellehierck 40:c6dffb676350 1344 // ---------- Attach PWM ----------
Jellehierck 40:c6dffb676350 1345 motor1Power.period(PWM_period);
Jellehierck 40:c6dffb676350 1346 motor2Power.period(PWM_period);
Jellehierck 40:c6dffb676350 1347
Jellehierck 38:8b597ab8344f 1348 // ---------- Turn OFF LEDs ----------
Jellehierck 38:8b597ab8344f 1349 led_b = 1;
Jellehierck 38:8b597ab8344f 1350 led_g = 1;
Jellehierck 38:8b597ab8344f 1351 led_r = 1;
Jellehierck 37:806c7c8381a7 1352
Jellehierck 23:8a0a0b959af1 1353 while(true) {
Jellehierck 45:d09040915cfe 1354 pc.printf("Global state: %i EMG state: %i Motor state: %i Operation state: %i Demo state: %i\r\n", global_curr_state, emg_curr_state, motor_curr_state, operation_curr_state, demo_curr_state);
Jellehierck 41:8e8141f355af 1355 pc.printf("EMG1 direction: %f EMG2 direction: %f \r\n", emg1_dir, emg2_dir);
Jellehierck 45:d09040915cfe 1356 //pc.printf("q1: %f q2: %f \r\n",q1*rad2deg,q2*rad2deg);
Jellehierck 45:d09040915cfe 1357 //pc.printf("Xe: %f Ye: %f\r\n",xe,ye);
Jellehierck 30:bac3b60d6283 1358 wait(0.5f);
Jellehierck 23:8a0a0b959af1 1359 }
Jellehierck 23:8a0a0b959af1 1360 }