Werkend met ledjes
Dependencies: mbed QEI HIDScope biquadFilter MODSERIAL FastPWM
main.cpp@21:bea7c8a08e1d, 2019-10-18 (annotated)
- Committer:
- joostbonekamp
- Date:
- Fri Oct 18 09:05:58 2019 +0000
- Revision:
- 21:bea7c8a08e1d
- Parent:
- 20:4424d447f3cd
- Child:
- 23:9eeac9d1ecbe
removed failure mode
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
joostbonekamp | 18:dddc8d9f7638 | 1 | /* |
joostbonekamp | 18:dddc8d9f7638 | 2 | To-do: |
joostbonekamp | 18:dddc8d9f7638 | 3 | Add reference generator |
joostbonekamp | 18:dddc8d9f7638 | 4 | fully implement schmitt trigger |
joostbonekamp | 18:dddc8d9f7638 | 5 | Homing |
joostbonekamp | 18:dddc8d9f7638 | 6 | Turning the magnet on/off |
joostbonekamp | 18:dddc8d9f7638 | 7 | Inverse kinematics |
joostbonekamp | 18:dddc8d9f7638 | 8 | Gravity compensation |
joostbonekamp | 18:dddc8d9f7638 | 9 | PID values |
joostbonekamp | 18:dddc8d9f7638 | 10 | General program layout |
joostbonekamp | 19:a37cae6964ca | 11 | better names for EMG input |
joostbonekamp | 18:dddc8d9f7638 | 12 | */ |
joostbonekamp | 18:dddc8d9f7638 | 13 | |
RobertoO | 0:67c50348f842 | 14 | #include "mbed.h" |
RobertoO | 1:b862262a9d14 | 15 | #include "MODSERIAL.h" |
joostbonekamp | 2:bbaa6fca2ab1 | 16 | #include "FastPWM.h" |
joostbonekamp | 2:bbaa6fca2ab1 | 17 | #include "QEI.h" |
joostbonekamp | 17:615c5d8b3710 | 18 | #include "HIDScope.h" |
joostbonekamp | 17:615c5d8b3710 | 19 | #include "BiQuad.h" |
hidde1104 | 13:51ae2da8da55 | 20 | #define PI 3.14159265 |
RobertoO | 0:67c50348f842 | 21 | |
joostbonekamp | 12:88cbc65f2563 | 22 | Serial pc(USBTX, USBRX); //connect to pc |
joostbonekamp | 20:4424d447f3cd | 23 | HIDScope scope(1); //HIDScope instance |
joostbonekamp | 12:88cbc65f2563 | 24 | DigitalOut motor1_direction(D4); //rotation motor 1 on shield (always D6) |
joostbonekamp | 12:88cbc65f2563 | 25 | FastPWM motor1_pwm(D5); //pwm 1 on shield (always D7) |
joostbonekamp | 12:88cbc65f2563 | 26 | DigitalOut motor2_direction(D7); //rotation motor 2 on shield (always D4) |
joostbonekamp | 12:88cbc65f2563 | 27 | FastPWM motor2_pwm(D6); //pwm 2 on shield (always D5) |
joostbonekamp | 5:aa8b5d5e632f | 28 | Ticker loop_ticker; //used in main() |
joostbonekamp | 20:4424d447f3cd | 29 | Ticker scope_ticker; |
joostbonekamp | 12:88cbc65f2563 | 30 | AnalogIn Pot1(A1); //pot 1 on biorobotics shield |
joostbonekamp | 12:88cbc65f2563 | 31 | AnalogIn Pot2(A0); //pot 2 on biorobotics shield |
joostbonekamp | 12:88cbc65f2563 | 32 | InterruptIn but1(D10); //debounced button on biorobotics shield |
joostbonekamp | 12:88cbc65f2563 | 33 | InterruptIn but2(D9); //debounced button on biorobotics shield |
joostbonekamp | 16:696e9cbcc823 | 34 | AnalogIn EMG1(A2); |
joostbonekamp | 16:696e9cbcc823 | 35 | AnalogIn EMG2(A3); |
joostbonekamp | 12:88cbc65f2563 | 36 | |
joostbonekamp | 17:615c5d8b3710 | 37 | void check_failure(); |
joostbonekamp | 10:b8c60fd468f1 | 38 | |
joostbonekamp | 17:615c5d8b3710 | 39 | QEI enc1 (D11, D12, NC, 8400, QEI::X4_ENCODING); //encoder 1 gebruiken |
joostbonekamp | 17:615c5d8b3710 | 40 | QEI enc2 (D1, D2, NC, 8400, QEI::X4_ENCODING); //encoder 2 gebruiken |
joostbonekamp | 17:615c5d8b3710 | 41 | |
joostbonekamp | 17:615c5d8b3710 | 42 | BiQuad bq1 (0.881889334678067, -1.76377866935613, 0.8818893346780671, -1.77069673005903, 0.797707797506027); |
joostbonekamp | 17:615c5d8b3710 | 43 | BiQuad bq2 (0.000198358203463849, 0.000396716406927699, 0.000198358203463849, -1.96262073248799, 0.963423352820821); |
joostbonekamp | 3:e3d12393adb1 | 44 | |
joostbonekamp | 5:aa8b5d5e632f | 45 | //variables |
joostbonekamp | 17:615c5d8b3710 | 46 | enum States {s_idle, s_cali_EMG, s_cali_enc, s_moving_magnet_off, s_moving_magnet_on, s_homing, s_failure}; |
joostbonekamp | 12:88cbc65f2563 | 47 | States state; //using the States enum |
joostbonekamp | 14:4cf17b10e504 | 48 | struct actuator_state { |
joostbonekamp | 12:88cbc65f2563 | 49 | float duty_cycle1; //pwm of 1st motor |
joostbonekamp | 12:88cbc65f2563 | 50 | float duty_cycle2; //pwm of 2nd motor |
joostbonekamp | 10:b8c60fd468f1 | 51 | int dir1; //direction of 1st motor |
joostbonekamp | 10:b8c60fd468f1 | 52 | int dir2; //direction of 2nd motor |
joostbonekamp | 12:88cbc65f2563 | 53 | bool magnet; //state of the magnet |
joostbonekamp | 17:615c5d8b3710 | 54 | } actuator; |
joostbonekamp | 12:88cbc65f2563 | 55 | |
joostbonekamp | 14:4cf17b10e504 | 56 | struct EMG_params { |
joostbonekamp | 16:696e9cbcc823 | 57 | float max; //params of the emg, tbd during calibration |
joostbonekamp | 17:615c5d8b3710 | 58 | float min; |
joostbonekamp | 16:696e9cbcc823 | 59 | } EMG_values; |
joostbonekamp | 5:aa8b5d5e632f | 60 | |
joostbonekamp | 18:dddc8d9f7638 | 61 | struct PID { |
joostbonekamp | 18:dddc8d9f7638 | 62 | float P; |
joostbonekamp | 18:dddc8d9f7638 | 63 | float I; |
joostbonekamp | 18:dddc8d9f7638 | 64 | float D; |
joostbonekamp | 18:dddc8d9f7638 | 65 | float I_counter; |
joostbonekamp | 19:a37cae6964ca | 66 | }; |
joostbonekamp | 19:a37cae6964ca | 67 | PID PID1; |
joostbonekamp | 19:a37cae6964ca | 68 | PID PID2; |
joostbonekamp | 18:dddc8d9f7638 | 69 | |
joostbonekamp | 18:dddc8d9f7638 | 70 | float dt = 0.001; |
joostbonekamp | 19:a37cae6964ca | 71 | float theta; |
joostbonekamp | 19:a37cae6964ca | 72 | float L; |
joostbonekamp | 20:4424d447f3cd | 73 | int enc1_zero = 0;//the zero position of the encoders, to be determined from the |
joostbonekamp | 20:4424d447f3cd | 74 | int enc2_zero = 0;//encoder calibration |
joostbonekamp | 16:696e9cbcc823 | 75 | int EMG1_filtered; |
joostbonekamp | 16:696e9cbcc823 | 76 | int EMG2_filtered; |
joostbonekamp | 16:696e9cbcc823 | 77 | int enc1_value; |
joostbonekamp | 16:696e9cbcc823 | 78 | int enc2_value; |
joostbonekamp | 18:dddc8d9f7638 | 79 | float error1 = 0.0; |
joostbonekamp | 18:dddc8d9f7638 | 80 | float error2 = 0.0; |
joostbonekamp | 19:a37cae6964ca | 81 | float last_error1 = 0.0; |
joostbonekamp | 19:a37cae6964ca | 82 | float last_error2 = 0.0; |
joostbonekamp | 19:a37cae6964ca | 83 | float action1; |
joostbonekamp | 19:a37cae6964ca | 84 | float action2; |
joostbonekamp | 12:88cbc65f2563 | 85 | bool state_changed = false; //used to see if the state is "starting" |
PatrickZieverink | 8:6f6a4dc12036 | 86 | volatile bool but1_pressed = false; |
PatrickZieverink | 8:6f6a4dc12036 | 87 | volatile bool but2_pressed = false; |
joostbonekamp | 16:696e9cbcc823 | 88 | volatile bool failure_occurred = false; |
joostbonekamp | 12:88cbc65f2563 | 89 | float pot_1; //used to keep track of the potentiometer values |
joostbonekamp | 10:b8c60fd468f1 | 90 | float pot_2; |
joostbonekamp | 16:696e9cbcc823 | 91 | bool EMG_has_been_calibrated; |
joostbonekamp | 17:615c5d8b3710 | 92 | bool button1_pressed; |
joostbonekamp | 17:615c5d8b3710 | 93 | bool button2_pressed; |
joostbonekamp | 19:a37cae6964ca | 94 | const int EMG_cali_amount = 1000; |
joostbonekamp | 19:a37cae6964ca | 95 | float past_EMG_values[EMG_cali_amount]; |
joostbonekamp | 19:a37cae6964ca | 96 | int past_EMG_count = 0; |
joostbonekamp | 12:88cbc65f2563 | 97 | |
joostbonekamp | 12:88cbc65f2563 | 98 | void do_nothing() |
joostbonekamp | 5:aa8b5d5e632f | 99 | |
PatrickZieverink | 9:6537eead1241 | 100 | /* |
joostbonekamp | 12:88cbc65f2563 | 101 | Idle state. Used in the beginning, before the calibration states. |
joostbonekamp | 12:88cbc65f2563 | 102 | */ |
joostbonekamp | 16:696e9cbcc823 | 103 | { |
joostbonekamp | 16:696e9cbcc823 | 104 | if (button1_pressed) { |
joostbonekamp | 16:696e9cbcc823 | 105 | state_changed = true; |
joostbonekamp | 17:615c5d8b3710 | 106 | state = s_cali_enc; |
joostbonekamp | 16:696e9cbcc823 | 107 | button1_pressed = false; |
joostbonekamp | 16:696e9cbcc823 | 108 | } |
joostbonekamp | 16:696e9cbcc823 | 109 | } |
joostbonekamp | 12:88cbc65f2563 | 110 | |
joostbonekamp | 12:88cbc65f2563 | 111 | void failure() |
joostbonekamp | 12:88cbc65f2563 | 112 | /* |
joostbonekamp | 12:88cbc65f2563 | 113 | Failure mode. This should execute when button 2 is pressed during operation. |
joostbonekamp | 12:88cbc65f2563 | 114 | */ |
joostbonekamp | 12:88cbc65f2563 | 115 | { |
joostbonekamp | 12:88cbc65f2563 | 116 | if (state_changed) { |
joostbonekamp | 12:88cbc65f2563 | 117 | pc.printf("Something went wrong!\r\n"); |
joostbonekamp | 12:88cbc65f2563 | 118 | state_changed = false; |
joostbonekamp | 12:88cbc65f2563 | 119 | } |
joostbonekamp | 12:88cbc65f2563 | 120 | } |
PatrickZieverink | 9:6537eead1241 | 121 | |
joostbonekamp | 12:88cbc65f2563 | 122 | void cali_EMG() |
joostbonekamp | 12:88cbc65f2563 | 123 | /* |
joostbonekamp | 16:696e9cbcc823 | 124 | Calibration of the EMG. Values determined during calibration should be |
joostbonekamp | 12:88cbc65f2563 | 125 | added to the EMG_params instance. |
joostbonekamp | 12:88cbc65f2563 | 126 | */ |
joostbonekamp | 12:88cbc65f2563 | 127 | { |
joostbonekamp | 12:88cbc65f2563 | 128 | if (state_changed) { |
joostbonekamp | 12:88cbc65f2563 | 129 | pc.printf("Started EMG calibration\r\n"); |
joostbonekamp | 12:88cbc65f2563 | 130 | state_changed = false; |
PatrickZieverink | 9:6537eead1241 | 131 | } |
joostbonekamp | 19:a37cae6964ca | 132 | if (past_EMG_count < EMG_cali_amount) { |
joostbonekamp | 19:a37cae6964ca | 133 | past_EMG_values[past_EMG_count] = EMG1_filtered; |
joostbonekamp | 19:a37cae6964ca | 134 | past_EMG_count++; |
joostbonekamp | 19:a37cae6964ca | 135 | } |
joostbonekamp | 19:a37cae6964ca | 136 | else { //calibration is has concluded |
joostbonekamp | 19:a37cae6964ca | 137 | float sum = 0.0; |
joostbonekamp | 19:a37cae6964ca | 138 | for(int i = 0; i<EMG_cali_amount; i++) { |
joostbonekamp | 19:a37cae6964ca | 139 | sum += past_EMG_values[i]; |
joostbonekamp | 19:a37cae6964ca | 140 | } |
joostbonekamp | 19:a37cae6964ca | 141 | float mean = sum/(float)EMG_cali_amount; |
joostbonekamp | 19:a37cae6964ca | 142 | EMG_values.min = mean; |
joostbonekamp | 19:a37cae6964ca | 143 | //calibration done, moving to cali_enc |
joostbonekamp | 19:a37cae6964ca | 144 | pc.printf("Calibration of the EMG is done, lower bound = %f", mean); |
joostbonekamp | 19:a37cae6964ca | 145 | EMG_has_been_calibrated = true; |
joostbonekamp | 19:a37cae6964ca | 146 | state_changed = true; |
joostbonekamp | 19:a37cae6964ca | 147 | state = s_cali_enc; |
joostbonekamp | 19:a37cae6964ca | 148 | } |
joostbonekamp | 12:88cbc65f2563 | 149 | } |
joostbonekamp | 16:696e9cbcc823 | 150 | |
joostbonekamp | 12:88cbc65f2563 | 151 | void cali_enc() |
joostbonekamp | 12:88cbc65f2563 | 152 | /* |
joostbonekamp | 14:4cf17b10e504 | 153 | Calibration of the encoder. The encoder should be moved to the lowest |
joostbonekamp | 19:a37cae6964ca | 154 | position for the linear stage and the horizontal postition for the |
joostbonekamp | 12:88cbc65f2563 | 155 | rotating stage. |
joostbonekamp | 12:88cbc65f2563 | 156 | */ |
joostbonekamp | 12:88cbc65f2563 | 157 | { |
joostbonekamp | 12:88cbc65f2563 | 158 | if (state_changed) { |
joostbonekamp | 12:88cbc65f2563 | 159 | pc.printf("Started encoder calibration\r\n"); |
joostbonekamp | 12:88cbc65f2563 | 160 | state_changed = false; |
PatrickZieverink | 9:6537eead1241 | 161 | } |
joostbonekamp | 16:696e9cbcc823 | 162 | if (button1_pressed) { |
joostbonekamp | 19:a37cae6964ca | 163 | pc.printf("Encoder has been calibrated"); |
joostbonekamp | 16:696e9cbcc823 | 164 | enc1_zero = enc1_value; |
joostbonekamp | 16:696e9cbcc823 | 165 | enc2_zero = enc2_value; |
joostbonekamp | 16:696e9cbcc823 | 166 | button1_pressed = false; |
joostbonekamp | 17:615c5d8b3710 | 167 | state = s_moving_magnet_off; |
joostbonekamp | 16:696e9cbcc823 | 168 | state_changed = true; |
joostbonekamp | 16:696e9cbcc823 | 169 | |
joostbonekamp | 16:696e9cbcc823 | 170 | } |
joostbonekamp | 12:88cbc65f2563 | 171 | } |
joostbonekamp | 16:696e9cbcc823 | 172 | |
joostbonekamp | 12:88cbc65f2563 | 173 | void moving_magnet_off() |
joostbonekamp | 12:88cbc65f2563 | 174 | /* |
joostbonekamp | 14:4cf17b10e504 | 175 | Moving with the magnet disabled. This is the part from the home position |
joostbonekamp | 12:88cbc65f2563 | 176 | towards the storage of chips. |
joostbonekamp | 12:88cbc65f2563 | 177 | */ |
joostbonekamp | 12:88cbc65f2563 | 178 | { |
joostbonekamp | 12:88cbc65f2563 | 179 | if (state_changed) { |
joostbonekamp | 12:88cbc65f2563 | 180 | pc.printf("Moving without magnet\r\n"); |
joostbonekamp | 12:88cbc65f2563 | 181 | state_changed = false; |
PatrickZieverink | 9:6537eead1241 | 182 | } |
PatrickZieverink | 9:6537eead1241 | 183 | } |
joostbonekamp | 16:696e9cbcc823 | 184 | |
joostbonekamp | 12:88cbc65f2563 | 185 | void moving_magnet_on() |
joostbonekamp | 12:88cbc65f2563 | 186 | /* |
joostbonekamp | 14:4cf17b10e504 | 187 | Moving with the magnet enabled. This is the part of the movement from the |
joostbonekamp | 12:88cbc65f2563 | 188 | chip holder to the top of the playing board. |
joostbonekamp | 12:88cbc65f2563 | 189 | */ |
joostbonekamp | 12:88cbc65f2563 | 190 | { |
joostbonekamp | 12:88cbc65f2563 | 191 | if (state_changed) { |
joostbonekamp | 12:88cbc65f2563 | 192 | pc.printf("Moving with magnet\r\n"); |
joostbonekamp | 12:88cbc65f2563 | 193 | state_changed = false; |
joostbonekamp | 12:88cbc65f2563 | 194 | } |
joostbonekamp | 12:88cbc65f2563 | 195 | return; |
PatrickZieverink | 9:6537eead1241 | 196 | } |
joostbonekamp | 12:88cbc65f2563 | 197 | void homing() |
joostbonekamp | 12:88cbc65f2563 | 198 | /* |
joostbonekamp | 14:4cf17b10e504 | 199 | Dropping the chip and moving towards the rest position. |
PatrickZieverink | 9:6537eead1241 | 200 | */ |
joostbonekamp | 12:88cbc65f2563 | 201 | { |
joostbonekamp | 12:88cbc65f2563 | 202 | if (state_changed) { |
joostbonekamp | 12:88cbc65f2563 | 203 | pc.printf("Started homing"); |
joostbonekamp | 12:88cbc65f2563 | 204 | state_changed = false; |
joostbonekamp | 12:88cbc65f2563 | 205 | } |
joostbonekamp | 12:88cbc65f2563 | 206 | return; |
joostbonekamp | 12:88cbc65f2563 | 207 | } |
PatrickZieverink | 9:6537eead1241 | 208 | |
joostbonekamp | 12:88cbc65f2563 | 209 | void measure_signals() |
joostbonekamp | 12:88cbc65f2563 | 210 | { |
joostbonekamp | 19:a37cae6964ca | 211 | //only one emg input, reference and plus |
joostbonekamp | 19:a37cae6964ca | 212 | float EMG1_raw = EMG1.read(); |
joostbonekamp | 19:a37cae6964ca | 213 | float EMG2_raw = EMG2.read(); |
joostbonekamp | 19:a37cae6964ca | 214 | float filter_value1 = fabs(bq2.step(fabs(bq1.step(EMG1_raw - EMG2_raw)))); |
joostbonekamp | 19:a37cae6964ca | 215 | |
joostbonekamp | 19:a37cae6964ca | 216 | if (filter_value1 > EMG_values.max) { |
joostbonekamp | 19:a37cae6964ca | 217 | EMG_values.max = filter_value1; |
joostbonekamp | 19:a37cae6964ca | 218 | } |
joostbonekamp | 19:a37cae6964ca | 219 | if (EMG_has_been_calibrated) { |
joostbonekamp | 19:a37cae6964ca | 220 | EMG1_filtered = (filter_value1-EMG_values.min)/(EMG_values.max-EMG_values.min); |
joostbonekamp | 19:a37cae6964ca | 221 | } |
joostbonekamp | 19:a37cae6964ca | 222 | else { |
joostbonekamp | 19:a37cae6964ca | 223 | EMG1_filtered = filter_value1; |
joostbonekamp | 19:a37cae6964ca | 224 | } |
joostbonekamp | 19:a37cae6964ca | 225 | |
joostbonekamp | 16:696e9cbcc823 | 226 | enc1_value = enc1.getPulses(); |
joostbonekamp | 16:696e9cbcc823 | 227 | enc2_value = enc2.getPulses(); |
joostbonekamp | 19:a37cae6964ca | 228 | enc1_value -= enc1_zero; |
joostbonekamp | 19:a37cae6964ca | 229 | enc2_value -= enc2_zero; |
joostbonekamp | 19:a37cae6964ca | 230 | theta = (float)(enc1_value)/(float)(8400*2*PI); |
joostbonekamp | 19:a37cae6964ca | 231 | L = (float)(enc2_value)/(float)(8400*2*PI); |
joostbonekamp | 19:a37cae6964ca | 232 | |
joostbonekamp | 19:a37cae6964ca | 233 | |
joostbonekamp | 12:88cbc65f2563 | 234 | } |
joostbonekamp | 10:b8c60fd468f1 | 235 | |
joostbonekamp | 17:615c5d8b3710 | 236 | void sample() |
joostbonekamp | 17:615c5d8b3710 | 237 | { |
joostbonekamp | 17:615c5d8b3710 | 238 | /* Set the sampled emg values in channel 0 (the first channel) and 1 (the second channel) in the 'HIDScope' instance named 'scope' */ |
joostbonekamp | 17:615c5d8b3710 | 239 | float emg0_value = EMG1.read(); |
joostbonekamp | 17:615c5d8b3710 | 240 | float emg1_value = EMG2.read(); |
joostbonekamp | 17:615c5d8b3710 | 241 | |
joostbonekamp | 17:615c5d8b3710 | 242 | //double filter_value = bqc.step(emg1_value); |
joostbonekamp | 17:615c5d8b3710 | 243 | float filter_value = fabs(bq2.step(fabs(bq1.step(emg0_value - emg1_value)))); |
joostbonekamp | 17:615c5d8b3710 | 244 | if (filter_value > EMG_values.max) { |
joostbonekamp | 17:615c5d8b3710 | 245 | EMG_values.max = filter_value; |
joostbonekamp | 17:615c5d8b3710 | 246 | } |
joostbonekamp | 17:615c5d8b3710 | 247 | if (EMG_values.min > filter_value) { |
joostbonekamp | 17:615c5d8b3710 | 248 | EMG_values.min = filter_value; |
joostbonekamp | 17:615c5d8b3710 | 249 | } |
joostbonekamp | 17:615c5d8b3710 | 250 | |
joostbonekamp | 17:615c5d8b3710 | 251 | filter_value = filter_value-EMG_values.min; |
joostbonekamp | 17:615c5d8b3710 | 252 | filter_value = filter_value/(EMG_values.max-EMG_values.min); |
joostbonekamp | 17:615c5d8b3710 | 253 | |
joostbonekamp | 17:615c5d8b3710 | 254 | scope.set(0, EMG1.read() ); |
joostbonekamp | 17:615c5d8b3710 | 255 | scope.set(1, EMG2.read() ); |
joostbonekamp | 17:615c5d8b3710 | 256 | scope.set(2, filter_value); |
joostbonekamp | 17:615c5d8b3710 | 257 | /* Repeat the step above if required for more channels of required (channel 0 up to 5 = 6 channels) |
joostbonekamp | 17:615c5d8b3710 | 258 | * Ensure that enough channels are available (HIDScope scope( 2 )) |
joostbonekamp | 17:615c5d8b3710 | 259 | * Finally, send all channels to the PC at once */ |
joostbonekamp | 17:615c5d8b3710 | 260 | scope.send(); |
joostbonekamp | 17:615c5d8b3710 | 261 | /* To indicate that the function is working, the LED is toggled */ |
joostbonekamp | 17:615c5d8b3710 | 262 | } |
joostbonekamp | 17:615c5d8b3710 | 263 | |
joostbonekamp | 17:615c5d8b3710 | 264 | void motor_controller() { |
joostbonekamp | 18:dddc8d9f7638 | 265 | float error1, error2; |
joostbonekamp | 18:dddc8d9f7638 | 266 | //P part of the controller |
joostbonekamp | 18:dddc8d9f7638 | 267 | float P_action1 = PID1.P * error1; |
joostbonekamp | 18:dddc8d9f7638 | 268 | float P_action2 = PID2.P * error2; |
joostbonekamp | 17:615c5d8b3710 | 269 | |
joostbonekamp | 18:dddc8d9f7638 | 270 | //I part |
joostbonekamp | 18:dddc8d9f7638 | 271 | PID1.I_counter += error1; |
joostbonekamp | 18:dddc8d9f7638 | 272 | PID2.I_counter += error2; |
joostbonekamp | 18:dddc8d9f7638 | 273 | float I_action1 = PID1.I_counter * PID1.I; |
joostbonekamp | 18:dddc8d9f7638 | 274 | float I_action2 = PID2.I_counter * PID2.I; |
joostbonekamp | 18:dddc8d9f7638 | 275 | |
joostbonekamp | 18:dddc8d9f7638 | 276 | //D part |
joostbonekamp | 18:dddc8d9f7638 | 277 | float velocity_estimate_1 = (error1-last_error1)/dt; //estimate of the time derivative of the error |
joostbonekamp | 18:dddc8d9f7638 | 278 | float velocity_estimate_2 = (error2-last_error2)/dt; |
joostbonekamp | 18:dddc8d9f7638 | 279 | float D_action1 = velocity_estimate_1 * PID1.D; |
joostbonekamp | 18:dddc8d9f7638 | 280 | float D_action2 = velocity_estimate_2 * PID2.D; |
joostbonekamp | 18:dddc8d9f7638 | 281 | |
joostbonekamp | 18:dddc8d9f7638 | 282 | action1 = P_action1 + I_action1 + D_action1; |
joostbonekamp | 18:dddc8d9f7638 | 283 | action2 = P_action2 + I_action2 + D_action2; |
joostbonekamp | 18:dddc8d9f7638 | 284 | |
joostbonekamp | 18:dddc8d9f7638 | 285 | last_error1 = error1; |
joostbonekamp | 18:dddc8d9f7638 | 286 | last_error2 = error2; |
joostbonekamp | 17:615c5d8b3710 | 287 | } |
joostbonekamp | 17:615c5d8b3710 | 288 | |
joostbonekamp | 15:9a1f34bc9958 | 289 | void output() |
joostbonekamp | 14:4cf17b10e504 | 290 | { |
joostbonekamp | 16:696e9cbcc823 | 291 | motor1_direction = actuator.dir1; |
joostbonekamp | 17:615c5d8b3710 | 292 | motor2_direction = actuator.dir2; |
joostbonekamp | 17:615c5d8b3710 | 293 | motor1_pwm.write(actuator.duty_cycle1); |
joostbonekamp | 17:615c5d8b3710 | 294 | motor2_pwm.write(actuator.duty_cycle2); |
joostbonekamp | 20:4424d447f3cd | 295 | |
joostbonekamp | 20:4424d447f3cd | 296 | scope.set(0, EMG1_filtered); |
joostbonekamp | 15:9a1f34bc9958 | 297 | } |
joostbonekamp | 14:4cf17b10e504 | 298 | |
joostbonekamp | 15:9a1f34bc9958 | 299 | void state_machine() |
joostbonekamp | 15:9a1f34bc9958 | 300 | { |
joostbonekamp | 16:696e9cbcc823 | 301 | check_failure(); //check for an error in the last loop before state machine |
joostbonekamp | 15:9a1f34bc9958 | 302 | //run current state |
joostbonekamp | 17:615c5d8b3710 | 303 | switch (state) { |
joostbonekamp | 17:615c5d8b3710 | 304 | case s_idle: |
joostbonekamp | 15:9a1f34bc9958 | 305 | do_nothing(); |
joostbonekamp | 15:9a1f34bc9958 | 306 | break; |
joostbonekamp | 17:615c5d8b3710 | 307 | case s_failure: |
joostbonekamp | 15:9a1f34bc9958 | 308 | failure(); |
joostbonekamp | 15:9a1f34bc9958 | 309 | break; |
joostbonekamp | 17:615c5d8b3710 | 310 | case s_cali_EMG: |
joostbonekamp | 15:9a1f34bc9958 | 311 | cali_EMG(); |
joostbonekamp | 15:9a1f34bc9958 | 312 | break; |
joostbonekamp | 17:615c5d8b3710 | 313 | case s_cali_enc: |
joostbonekamp | 17:615c5d8b3710 | 314 | cali_enc(); |
joostbonekamp | 15:9a1f34bc9958 | 315 | break; |
joostbonekamp | 17:615c5d8b3710 | 316 | case s_moving_magnet_on: |
joostbonekamp | 15:9a1f34bc9958 | 317 | moving_magnet_on(); |
joostbonekamp | 15:9a1f34bc9958 | 318 | break; |
joostbonekamp | 17:615c5d8b3710 | 319 | case s_moving_magnet_off: |
joostbonekamp | 15:9a1f34bc9958 | 320 | moving_magnet_off(); |
joostbonekamp | 15:9a1f34bc9958 | 321 | break; |
joostbonekamp | 17:615c5d8b3710 | 322 | case s_homing: |
joostbonekamp | 15:9a1f34bc9958 | 323 | homing(); |
joostbonekamp | 15:9a1f34bc9958 | 324 | break; |
joostbonekamp | 5:aa8b5d5e632f | 325 | } |
joostbonekamp | 5:aa8b5d5e632f | 326 | } |
joostbonekamp | 15:9a1f34bc9958 | 327 | |
joostbonekamp | 15:9a1f34bc9958 | 328 | void main_loop() |
joostbonekamp | 12:88cbc65f2563 | 329 | { |
joostbonekamp | 15:9a1f34bc9958 | 330 | measure_signals(); |
joostbonekamp | 15:9a1f34bc9958 | 331 | state_machine(); |
joostbonekamp | 15:9a1f34bc9958 | 332 | motor_controller(); |
joostbonekamp | 15:9a1f34bc9958 | 333 | output(); |
joostbonekamp | 15:9a1f34bc9958 | 334 | } |
joostbonekamp | 14:4cf17b10e504 | 335 | |
joostbonekamp | 14:4cf17b10e504 | 336 | //Helper functions, not directly called by the main_loop functions or |
joostbonekamp | 14:4cf17b10e504 | 337 | //state machines |
joostbonekamp | 16:696e9cbcc823 | 338 | void check_failure() |
joostbonekamp | 15:9a1f34bc9958 | 339 | { |
joostbonekamp | 21:bea7c8a08e1d | 340 | //state = s_failure; |
joostbonekamp | 21:bea7c8a08e1d | 341 | //state_changed = true; |
joostbonekamp | 16:696e9cbcc823 | 342 | } |
joostbonekamp | 16:696e9cbcc823 | 343 | |
joostbonekamp | 16:696e9cbcc823 | 344 | void but1_interrupt() |
joostbonekamp | 16:696e9cbcc823 | 345 | { |
joostbonekamp | 17:615c5d8b3710 | 346 | if(but2.read()) {//both buttons are pressed |
joostbonekamp | 16:696e9cbcc823 | 347 | failure_occurred = true; |
joostbonekamp | 16:696e9cbcc823 | 348 | } |
joostbonekamp | 15:9a1f34bc9958 | 349 | but1_pressed = true; |
joostbonekamp | 15:9a1f34bc9958 | 350 | pc.printf("Button 1 pressed \n\r"); |
joostbonekamp | 15:9a1f34bc9958 | 351 | } |
joostbonekamp | 14:4cf17b10e504 | 352 | |
joostbonekamp | 16:696e9cbcc823 | 353 | void but2_interrupt() |
joostbonekamp | 15:9a1f34bc9958 | 354 | { |
joostbonekamp | 17:615c5d8b3710 | 355 | if(but1.read()) {//both buttons are pressed |
joostbonekamp | 16:696e9cbcc823 | 356 | failure_occurred = true; |
joostbonekamp | 16:696e9cbcc823 | 357 | } |
joostbonekamp | 15:9a1f34bc9958 | 358 | but2_pressed = true; |
joostbonekamp | 15:9a1f34bc9958 | 359 | pc.printf("Button 2 pressed \n\r"); |
joostbonekamp | 15:9a1f34bc9958 | 360 | } |
joostbonekamp | 17:615c5d8b3710 | 361 | |
joostbonekamp | 16:696e9cbcc823 | 362 | int schmitt_trigger(float i) |
joostbonekamp | 16:696e9cbcc823 | 363 | { |
joostbonekamp | 17:615c5d8b3710 | 364 | int speed; |
joostbonekamp | 16:696e9cbcc823 | 365 | speed = -1; //default value, this means the state should not change |
joostbonekamp | 16:696e9cbcc823 | 366 | if (i > 0/14 && i < 2/14) {speed = 0;} |
joostbonekamp | 16:696e9cbcc823 | 367 | if (i > 3/14 && i < 5/14) {speed = 1;} |
joostbonekamp | 17:615c5d8b3710 | 368 | if (i > 6/14 && i < 8/14) {speed = 2;} |
joostbonekamp | 16:696e9cbcc823 | 369 | if (i > 9/14 && i < 11/14) {speed = 3;} |
joostbonekamp | 16:696e9cbcc823 | 370 | if (i > 12/14 && i < 14/14) {speed = 4;} |
joostbonekamp | 16:696e9cbcc823 | 371 | return speed; |
joostbonekamp | 16:696e9cbcc823 | 372 | } |
joostbonekamp | 14:4cf17b10e504 | 373 | |
joostbonekamp | 15:9a1f34bc9958 | 374 | int main() |
joostbonekamp | 15:9a1f34bc9958 | 375 | { |
joostbonekamp | 15:9a1f34bc9958 | 376 | pc.baud(115200); |
joostbonekamp | 15:9a1f34bc9958 | 377 | pc.printf("Executing main()... \r\n"); |
joostbonekamp | 17:615c5d8b3710 | 378 | state = s_idle; |
joostbonekamp | 12:88cbc65f2563 | 379 | |
joostbonekamp | 17:615c5d8b3710 | 380 | motor2_pwm.period(1/160000); // 1/frequency van waarop hij draait |
joostbonekamp | 17:615c5d8b3710 | 381 | motor1_pwm.period(1/160000); // 1/frequency van waarop hij draait |
joostbonekamp | 14:4cf17b10e504 | 382 | |
joostbonekamp | 15:9a1f34bc9958 | 383 | actuator.dir1 = 0; |
joostbonekamp | 15:9a1f34bc9958 | 384 | actuator.dir2 = 0; |
joostbonekamp | 15:9a1f34bc9958 | 385 | |
joostbonekamp | 15:9a1f34bc9958 | 386 | actuator.magnet = false; |
joostbonekamp | 14:4cf17b10e504 | 387 | |
joostbonekamp | 15:9a1f34bc9958 | 388 | but1.fall(&but1_interrupt); |
joostbonekamp | 15:9a1f34bc9958 | 389 | but2.fall(&but2_interrupt); |
joostbonekamp | 20:4424d447f3cd | 390 | scope_ticker.attach(&scope, &HIDScope::send, 0.02); |
joostbonekamp | 18:dddc8d9f7638 | 391 | loop_ticker.attach(&main_loop, dt); //main loop at 1kHz |
joostbonekamp | 15:9a1f34bc9958 | 392 | pc.printf("Main_loop is running\n\r"); |
joostbonekamp | 16:696e9cbcc823 | 393 | while (true) { |
joostbonekamp | 16:696e9cbcc823 | 394 | wait(0.1f); |
joostbonekamp | 16:696e9cbcc823 | 395 | } |
joostbonekamp | 17:615c5d8b3710 | 396 | } |