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