Script 15-10-2019
Dependencies: Servoaansturing mbed QEI HIDScope biquadFilter MODSERIAL FastPWM
main.cpp.orig@19:1fd39a2afc30, 2019-10-24 (annotated)
- Committer:
- Renate
- Date:
- Thu Oct 24 09:25:27 2019 +0000
- Revision:
- 19:1fd39a2afc30
Versie na 21-10
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Renate | 19:1fd39a2afc30 | 1 | #include "mbed.h" |
Renate | 19:1fd39a2afc30 | 2 | #include "HIDScope.h" |
Renate | 19:1fd39a2afc30 | 3 | #include "QEI.h" |
Renate | 19:1fd39a2afc30 | 4 | #include "MODSERIAL.h" |
Renate | 19:1fd39a2afc30 | 5 | #include "BiQuad.h" |
Renate | 19:1fd39a2afc30 | 6 | #include "FastPWM.h" |
Renate | 19:1fd39a2afc30 | 7 | #include <math.h> |
Renate | 19:1fd39a2afc30 | 8 | #include "Servo.h" |
Renate | 19:1fd39a2afc30 | 9 | |
Renate | 19:1fd39a2afc30 | 10 | // Definieer objecten |
Renate | 19:1fd39a2afc30 | 11 | Serial pc(USBTX, USBRX); |
Renate | 19:1fd39a2afc30 | 12 | |
Renate | 19:1fd39a2afc30 | 13 | PwmOut motor1(D6); // Misschien moeten we hiervoor DigitalOut gebruiken, moet |
Renate | 19:1fd39a2afc30 | 14 | PwmOut motor2(D5); // samen kunnen gaan met de servo motor |
Renate | 19:1fd39a2afc30 | 15 | |
Renate | 19:1fd39a2afc30 | 16 | DigitalOut motor1_dir(D7); |
Renate | 19:1fd39a2afc30 | 17 | DigitalOut motor2_dir(D4); |
Renate | 19:1fd39a2afc30 | 18 | |
Renate | 19:1fd39a2afc30 | 19 | DigitalIn Power_button_pressed(D1); // Geen InterruptIn gebruiken! |
Renate | 19:1fd39a2afc30 | 20 | DigitalIn Emergency_button_pressed(D2); |
Renate | 19:1fd39a2afc30 | 21 | |
Renate | 19:1fd39a2afc30 | 22 | AnalogIn EMG_biceps_right_raw (A0); |
Renate | 19:1fd39a2afc30 | 23 | AnalogIn EMG_biceps_left_raw (A1); |
Renate | 19:1fd39a2afc30 | 24 | AnalogIn EMG_calf_raw (A2); |
Renate | 19:1fd39a2afc30 | 25 | |
Renate | 19:1fd39a2afc30 | 26 | Ticker loop_ticker; |
Renate | 19:1fd39a2afc30 | 27 | Ticker HIDScope_ticker; |
Renate | 19:1fd39a2afc30 | 28 | Ticker emgSampleTicker; |
Renate | 19:1fd39a2afc30 | 29 | |
Renate | 19:1fd39a2afc30 | 30 | HIDScope scope(3); |
Renate | 19:1fd39a2afc30 | 31 | |
Renate | 19:1fd39a2afc30 | 32 | BiQuadChain bqc; // Let op !!! Deze coëfficiënten moeten nog worden veranderd |
Renate | 19:1fd39a2afc30 | 33 | BiQuad bq1(0.0030, 0.0059, 0.0030, -1.8404,0.8522); //voor low-pass |
Renate | 19:1fd39a2afc30 | 34 | BiQuad bq2(0.9737, -1.9474, 0.9737, -1.9467, 0.9481); //voor high-pass |
Renate | 19:1fd39a2afc30 | 35 | BiQuad bq3(0.9912, -1.9823,0.9912, -1.9822, 0.9824); //lage piek eruit-> voor coëfficienten, zie matlab |
Renate | 19:1fd39a2afc30 | 36 | |
Renate | 19:1fd39a2afc30 | 37 | bool calib = false; // MOGELIJK GAAT HET HIER FOUT |
Renate | 19:1fd39a2afc30 | 38 | int i_calib = 0; |
Renate | 19:1fd39a2afc30 | 39 | |
Renate | 19:1fd39a2afc30 | 40 | void emgSampleFilter() // Deze functie wordt aangeroepen dmv een ticker. Het sampled |
Renate | 19:1fd39a2afc30 | 41 | // hierdoor het EMG signaal en het haalt er een filter overheen |
Renate | 19:1fd39a2afc30 | 42 | { |
Renate | 19:1fd39a2afc30 | 43 | float filtered_EMG_biceps_right=bqc.step(EMG_biceps_right_raw.read()); |
Renate | 19:1fd39a2afc30 | 44 | float filtered_EMG_biceps_left=bqc.step(EMG_biceps_left_raw.read()); |
Renate | 19:1fd39a2afc30 | 45 | float filtered_EMG_calf=bqc.step(EMG_calf_raw.read()); |
Renate | 19:1fd39a2afc30 | 46 | |
Renate | 19:1fd39a2afc30 | 47 | float filtered_EMG_biceps_right_total=filtered_EMG_biceps_right; |
Renate | 19:1fd39a2afc30 | 48 | float filtered_EMG_biceps_left_total=filtered_EMG_biceps_left; |
Renate | 19:1fd39a2afc30 | 49 | float filtered_EMG_calf_total=filtered_EMG_calf; |
Renate | 19:1fd39a2afc30 | 50 | |
Renate | 19:1fd39a2afc30 | 51 | if (calib) |
Renate | 19:1fd39a2afc30 | 52 | { |
Renate | 19:1fd39a2afc30 | 53 | if (i_calib < 500) |
Renate | 19:1fd39a2afc30 | 54 | { |
Renate | 19:1fd39a2afc30 | 55 | filtered_EMG_biceps_right_total=filtered_EMG_biceps_right+filtered_EMG_biceps_right_total; |
Renate | 19:1fd39a2afc30 | 56 | filtered_EMG_biceps_left_total=filtered_EMG_biceps_left+filtered_EMG_biceps_left_total; |
Renate | 19:1fd39a2afc30 | 57 | filtered_EMG_calf_total=filtered_EMG_calf+filtered_EMG_calf_total; |
Renate | 19:1fd39a2afc30 | 58 | i_calib++; |
Renate | 19:1fd39a2afc30 | 59 | } |
Renate | 19:1fd39a2afc30 | 60 | if (i_calib >= 500) |
Renate | 19:1fd39a2afc30 | 61 | { |
Renate | 19:1fd39a2afc30 | 62 | mean_EMG_biceps_right=filtered_EMG_biceps_right_total/500; |
Renate | 19:1fd39a2afc30 | 63 | mean_EMG_biceps_left=filtered_EMG_biceps_left_total/500; |
Renate | 19:1fd39a2afc30 | 64 | mean_EMG_calf=filtered_EMG_calf_total/500; |
Renate | 19:1fd39a2afc30 | 65 | calib = false; |
Renate | 19:1fd39a2afc30 | 66 | } |
Renate | 19:1fd39a2afc30 | 67 | } |
Renate | 19:1fd39a2afc30 | 68 | } |
Renate | 19:1fd39a2afc30 | 69 | |
Renate | 19:1fd39a2afc30 | 70 | void sendHIDScope() // Deze functie geeft de gefilterde EMG-signalen weer in de HIDScope |
Renate | 19:1fd39a2afc30 | 71 | // Wordt eveneens gerund dmv een ticker |
Renate | 19:1fd39a2afc30 | 72 | { |
Renate | 19:1fd39a2afc30 | 73 | /* Set the sampled emg values in channel 0 (the first channel) and 1 (the second channel) in the 'HIDScope' instance named 'scope' */ |
Renate | 19:1fd39a2afc30 | 74 | scope.set(0, filtered_EMG_biceps_right() ); // Werkt dit zo? Of nog met .read? |
Renate | 19:1fd39a2afc30 | 75 | scope.set(1, filtered_EMG_biceps_left() ); |
Renate | 19:1fd39a2afc30 | 76 | scope.set(2, filtered_EMG_calf() ); |
Renate | 19:1fd39a2afc30 | 77 | /* Repeat the step above if required for more channels of required (channel 0 up to 5 = 6 channels) |
Renate | 19:1fd39a2afc30 | 78 | * Ensure that enough channels are available (HIDScope scope( 2 )) |
Renate | 19:1fd39a2afc30 | 79 | * Finally, send all channels to the PC at once */ |
Renate | 19:1fd39a2afc30 | 80 | scope.send(); |
Renate | 19:1fd39a2afc30 | 81 | // Eventueel nog een ledje laten branden |
Renate | 19:1fd39a2afc30 | 82 | } |
Renate | 19:1fd39a2afc30 | 83 | |
Renate | 19:1fd39a2afc30 | 84 | // Emergency |
Renate | 19:1fd39a2afc30 | 85 | void emergency() |
Renate | 19:1fd39a2afc30 | 86 | { |
Renate | 19:1fd39a2afc30 | 87 | loop_ticker.detach(); |
Renate | 19:1fd39a2afc30 | 88 | motor1.write(0); |
Renate | 19:1fd39a2afc30 | 89 | motor2.write(0); |
Renate | 19:1fd39a2afc30 | 90 | pc.printf("Ik ga exploderen!!!\r\n"); |
Renate | 19:1fd39a2afc30 | 91 | // Alles moet uitgaan (evt. een rood LEDje laten branden), moet |
Renate | 19:1fd39a2afc30 | 92 | // opnieuw worden opgestart. Mogelijk kan dit door de ticker te |
Renate | 19:1fd39a2afc30 | 93 | // detachen |
Renate | 19:1fd39a2afc30 | 94 | } |
Renate | 19:1fd39a2afc30 | 95 | |
Renate | 19:1fd39a2afc30 | 96 | // Motoren uitzetten |
Renate | 19:1fd39a2afc30 | 97 | void motors_off() |
Renate | 19:1fd39a2afc30 | 98 | { |
Renate | 19:1fd39a2afc30 | 99 | motor1.write(0); |
Renate | 19:1fd39a2afc30 | 100 | motor2.write(0); |
Renate | 19:1fd39a2afc30 | 101 | pc.printf("Motoren uit functie\r\n"); |
Renate | 19:1fd39a2afc30 | 102 | } |
Renate | 19:1fd39a2afc30 | 103 | |
Renate | 19:1fd39a2afc30 | 104 | // Motoren aanzetten |
Renate | 19:1fd39a2afc30 | 105 | void motors_on() |
Renate | 19:1fd39a2afc30 | 106 | { |
Renate | 19:1fd39a2afc30 | 107 | motor1.write(0.9); |
Renate | 19:1fd39a2afc30 | 108 | motor1_dir.write(1); |
Renate | 19:1fd39a2afc30 | 109 | motor2.write(0.1); |
Renate | 19:1fd39a2afc30 | 110 | motor1_dir.write(1); |
Renate | 19:1fd39a2afc30 | 111 | pc.printf("Motoren aan functie\r\n"); |
Renate | 19:1fd39a2afc30 | 112 | } |
Renate | 19:1fd39a2afc30 | 113 | |
Renate | 19:1fd39a2afc30 | 114 | // EMG kalibreren |
Renate | 19:1fd39a2afc30 | 115 | void emg_calibration() |
Renate | 19:1fd39a2afc30 | 116 | { |
Renate | 19:1fd39a2afc30 | 117 | // Gedurende bijv. 5 seconden EMG meten, wanneer de spieren maximaal |
Renate | 19:1fd39a2afc30 | 118 | // worden aangespannen -> maximaal potentiaal verkrijgen. Een fractie |
Renate | 19:1fd39a2afc30 | 119 | // hiervan kan als drempel worden gebruikt voor beweging |
Renate | 19:1fd39a2afc30 | 120 | |
Renate | 19:1fd39a2afc30 | 121 | // *Tijd instellen* |
Renate | 19:1fd39a2afc30 | 122 | // Iets met DOUBLE_MAX? https://docs.microsoft.com/en-us/cpp/c-language/cpp-integer-limits?view=vs-2019 |
Renate | 19:1fd39a2afc30 | 123 | |
Renate | 19:1fd39a2afc30 | 124 | // Ledje van kleur laten veranderen |
Renate | 19:1fd39a2afc30 | 125 | |
Renate | 19:1fd39a2afc30 | 126 | // MOGELIJK NIET MEER NODIG??? |
Renate | 19:1fd39a2afc30 | 127 | |
Renate | 19:1fd39a2afc30 | 128 | } |
Renate | 19:1fd39a2afc30 | 129 | |
Renate | 19:1fd39a2afc30 | 130 | // Finite state machine programming (calibration servo motor?) |
Renate | 19:1fd39a2afc30 | 131 | enum states {Motors_off, Calib_motor, Calib_EMG, Homing, Operation_mode}; |
Renate | 19:1fd39a2afc30 | 132 | |
Renate | 19:1fd39a2afc30 | 133 | states currentState = Motors_off; |
Renate | 19:1fd39a2afc30 | 134 | bool stateChanged = true; // Make sure the initialization of first state is executed |
Renate | 19:1fd39a2afc30 | 135 | |
Renate | 19:1fd39a2afc30 | 136 | void ProcessStateMachine(void) |
Renate | 19:1fd39a2afc30 | 137 | { |
Renate | 19:1fd39a2afc30 | 138 | switch (currentState) |
Renate | 19:1fd39a2afc30 | 139 | { |
Renate | 19:1fd39a2afc30 | 140 | case Motors_off: |
Renate | 19:1fd39a2afc30 | 141 | |
Renate | 19:1fd39a2afc30 | 142 | if (stateChanged) |
Renate | 19:1fd39a2afc30 | 143 | { |
Renate | 19:1fd39a2afc30 | 144 | motors_off(); // functie waarbij motoren uitgaan |
Renate | 19:1fd39a2afc30 | 145 | stateChanged = false; |
Renate | 19:1fd39a2afc30 | 146 | pc.printf("Motors off state\r\n"); |
Renate | 19:1fd39a2afc30 | 147 | } |
Renate | 19:1fd39a2afc30 | 148 | if (Power_button_pressed.read() == false) // Normaal waarde 1 bij indrukken, nu nul -> false |
Renate | 19:1fd39a2afc30 | 149 | { |
Renate | 19:1fd39a2afc30 | 150 | motors_on(); |
Renate | 19:1fd39a2afc30 | 151 | currentState = Calib_motor; |
Renate | 19:1fd39a2afc30 | 152 | stateChanged = true; |
Renate | 19:1fd39a2afc30 | 153 | pc.printf("Moving to Calib_motor state\r\n"); |
Renate | 19:1fd39a2afc30 | 154 | } |
Renate | 19:1fd39a2afc30 | 155 | if (Emergency_button_pressed.read() == false) // Normaal waarde 1 bij indrukken, nu nul -> false |
Renate | 19:1fd39a2afc30 | 156 | { |
Renate | 19:1fd39a2afc30 | 157 | emergency(); |
Renate | 19:1fd39a2afc30 | 158 | } |
Renate | 19:1fd39a2afc30 | 159 | break; |
Renate | 19:1fd39a2afc30 | 160 | |
Renate | 19:1fd39a2afc30 | 161 | case Calib_motor: |
Renate | 19:1fd39a2afc30 | 162 | |
Renate | 19:1fd39a2afc30 | 163 | if (stateChanged) |
Renate | 19:1fd39a2afc30 | 164 | { |
Renate | 19:1fd39a2afc30 | 165 | // Hier wordt een kalibratie uitgevoerd, waarbij de motorhoeken worden bepaald |
Renate | 19:1fd39a2afc30 | 166 | currentState = Calib_EMG; |
Renate | 19:1fd39a2afc30 | 167 | stateChanged = true; |
Renate | 19:1fd39a2afc30 | 168 | pc.printf("Moving to Calib_EMG state\r\n"); |
Renate | 19:1fd39a2afc30 | 169 | } |
Renate | 19:1fd39a2afc30 | 170 | if (Emergency_button_pressed.read() == false) |
Renate | 19:1fd39a2afc30 | 171 | { |
Renate | 19:1fd39a2afc30 | 172 | emergency(); |
Renate | 19:1fd39a2afc30 | 173 | } |
Renate | 19:1fd39a2afc30 | 174 | break; |
Renate | 19:1fd39a2afc30 | 175 | |
Renate | 19:1fd39a2afc30 | 176 | case Calib_EMG: |
Renate | 19:1fd39a2afc30 | 177 | |
Renate | 19:1fd39a2afc30 | 178 | motors_off(); |
Renate | 19:1fd39a2afc30 | 179 | if (stateChanged) |
Renate | 19:1fd39a2afc30 | 180 | { |
Renate | 19:1fd39a2afc30 | 181 | // Hierbij wordt een een kalibratie uitgevoerd, waarbij de maximale EMG-amplitude waarde wordt bepaald |
Renate | 19:1fd39a2afc30 | 182 | calib = true; |
Renate | 19:1fd39a2afc30 | 183 | emgSampleFilter() // Gaat dit nu goed? -> moet sws worden toegevoegd bij relevante onderdelen? |
Renate | 19:1fd39a2afc30 | 184 | if (i_calib >= 500) // of wait(10);? |
Renate | 19:1fd39a2afc30 | 185 | { |
Renate | 19:1fd39a2afc30 | 186 | currentState = Homing; |
Renate | 19:1fd39a2afc30 | 187 | stateChanged = true; |
Renate | 19:1fd39a2afc30 | 188 | pc.printf("Moving to Homing state\r\n"); |
Renate | 19:1fd39a2afc30 | 189 | } |
Renate | 19:1fd39a2afc30 | 190 | if (Emergency_button_pressed.read() == false) |
Renate | 19:1fd39a2afc30 | 191 | { |
Renate | 19:1fd39a2afc30 | 192 | emergency(); |
Renate | 19:1fd39a2afc30 | 193 | } |
Renate | 19:1fd39a2afc30 | 194 | break; |
Renate | 19:1fd39a2afc30 | 195 | |
Renate | 19:1fd39a2afc30 | 196 | case Homing: |
Renate | 19:1fd39a2afc30 | 197 | |
Renate | 19:1fd39a2afc30 | 198 | motors_on(); |
Renate | 19:1fd39a2afc30 | 199 | if (stateChanged) |
Renate | 19:1fd39a2afc30 | 200 | { |
Renate | 19:1fd39a2afc30 | 201 | // Ervoor zorgen dat de motoren zo bewegen dat de robotarm |
Renate | 19:1fd39a2afc30 | 202 | // (inclusief de end-effector) in de juiste home positie wordt gezet |
Renate | 19:1fd39a2afc30 | 203 | currentState = Operation_mode; |
Renate | 19:1fd39a2afc30 | 204 | stateChanged = true; |
Renate | 19:1fd39a2afc30 | 205 | pc.printf("Moving to operation mode \r\n"); |
Renate | 19:1fd39a2afc30 | 206 | } |
Renate | 19:1fd39a2afc30 | 207 | if (Emergency_button_pressed.read() == false) |
Renate | 19:1fd39a2afc30 | 208 | { |
Renate | 19:1fd39a2afc30 | 209 | emergency(); |
Renate | 19:1fd39a2afc30 | 210 | } |
Renate | 19:1fd39a2afc30 | 211 | break; |
Renate | 19:1fd39a2afc30 | 212 | |
Renate | 19:1fd39a2afc30 | 213 | case Operation_mode: // Overgaan tot emergency wanneer referentie niet |
Renate | 19:1fd39a2afc30 | 214 | // overeenkomt met werkelijkheid |
Renate | 19:1fd39a2afc30 | 215 | |
Renate | 19:1fd39a2afc30 | 216 | if (stateChanged) |
Renate | 19:1fd39a2afc30 | 217 | |
Renate | 19:1fd39a2afc30 | 218 | // Hier moet een functie worden aangeroepen die ervoor zorgt dat |
Renate | 19:1fd39a2afc30 | 219 | // aan de hand van EMG-signalen de motoren kunnen worden aangestuurd, |
Renate | 19:1fd39a2afc30 | 220 | // zodat de robotarm kan bewegen |
Renate | 19:1fd39a2afc30 | 221 | |
Renate | 19:1fd39a2afc30 | 222 | if (Power_button_pressed.read() == false) // Normaal waarde 1 bij indrukken, nu nul -> false |
Renate | 19:1fd39a2afc30 | 223 | { |
Renate | 19:1fd39a2afc30 | 224 | motors_off(); |
Renate | 19:1fd39a2afc30 | 225 | currentState = Motors_off; |
Renate | 19:1fd39a2afc30 | 226 | stateChanged = true; |
Renate | 19:1fd39a2afc30 | 227 | pc.printf("Terug naar de state Motors_off\r\n"); |
Renate | 19:1fd39a2afc30 | 228 | } |
Renate | 19:1fd39a2afc30 | 229 | if (Emergency_button_pressed.read() == false) |
Renate | 19:1fd39a2afc30 | 230 | { |
Renate | 19:1fd39a2afc30 | 231 | emergency(); |
Renate | 19:1fd39a2afc30 | 232 | } |
Renate | 19:1fd39a2afc30 | 233 | // wait(5); |
Renate | 19:1fd39a2afc30 | 234 | else |
Renate | 19:1fd39a2afc30 | 235 | { |
Renate | 19:1fd39a2afc30 | 236 | currentState = Homing; |
Renate | 19:1fd39a2afc30 | 237 | stateChanged = true; |
Renate | 19:1fd39a2afc30 | 238 | pc.printf("Terug naar de state Homing\r\n"); |
Renate | 19:1fd39a2afc30 | 239 | } |
Renate | 19:1fd39a2afc30 | 240 | break; |
Renate | 19:1fd39a2afc30 | 241 | |
Renate | 19:1fd39a2afc30 | 242 | default: |
Renate | 19:1fd39a2afc30 | 243 | // Zelfde functie als die eerder is toegepast om motoren uit te schakelen -> safety! |
Renate | 19:1fd39a2afc30 | 244 | motors_off(); |
Renate | 19:1fd39a2afc30 | 245 | pc.printf("Unknown or uninplemented state reached!\r\n"); |
Renate | 19:1fd39a2afc30 | 246 | |
Renate | 19:1fd39a2afc30 | 247 | } |
Renate | 19:1fd39a2afc30 | 248 | } |
Renate | 19:1fd39a2afc30 | 249 | |
Renate | 19:1fd39a2afc30 | 250 | int main(void) |
Renate | 19:1fd39a2afc30 | 251 | { |
Renate | 19:1fd39a2afc30 | 252 | pc.printf("Opstarten\r\n"); |
Renate | 19:1fd39a2afc30 | 253 | bqc.add(&bq1).add(&bq2).add(&bq3); |
Renate | 19:1fd39a2afc30 | 254 | emgSampleTicker.attach(&emgSampleFilter, 0.01f); |
Renate | 19:1fd39a2afc30 | 255 | HIDScope_ticker.attach(&sendHIDScope, 0.01f); |
Renate | 19:1fd39a2afc30 | 256 | loop_ticker.attach(&ProcessStateMachine, 5.0f); |
Renate | 19:1fd39a2afc30 | 257 | while(true) |
Renate | 19:1fd39a2afc30 | 258 | { /* do nothing */} |
Renate | 19:1fd39a2afc30 | 259 | } |