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@70:c4a019e3830d, 2019-11-05 (annotated)

Committer:
Jellehierck
Date:
Tue Nov 05 16:49:03 2019 +0000
Revision:
70:c4a019e3830d
Parent:
69:bfefdfb04c29 
FINAL VERSION: added many comments to code.

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