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@48:31676da4be7a, 2019-10-31 (annotated)

Committer:
BasB
Date:
Thu Oct 31 18:26:11 2019 +0000
Revision:
48:31676da4be7a
Parent:
47:63b5ccd969e9
Child:
50:86bad994f08f
EnableServo in main functie gezet. Servo werkt nu altijd

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