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@52:fd35025574ca, 2019-10-31 (annotated)

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