Script 15-10-2019
Dependencies: Servoaansturing mbed QEI HIDScope biquadFilter MODSERIAL FastPWM
main.cpp@26:7ae60739b310, 2019-10-29 (annotated)
- Committer:
- Renate
- Date:
- Tue Oct 29 12:36:03 2019 +0000
- Revision:
- 26:7ae60739b310
- Parent:
- 23:4572750a5c59
- Child:
- 27:7305319c11e8
Aanpassing op script 28-10;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
RobertoO | 0:67c50348f842 | 1 | #include "mbed.h" |
Rosalie | 3:6ee0b20c23b0 | 2 | #include "HIDScope.h" |
Rosalie | 3:6ee0b20c23b0 | 3 | #include "QEI.h" |
RobertoO | 1:b862262a9d14 | 4 | #include "MODSERIAL.h" |
Rosalie | 3:6ee0b20c23b0 | 5 | #include "BiQuad.h" |
Rosalie | 3:6ee0b20c23b0 | 6 | #include "FastPWM.h" |
Renate | 21:456acc79726c | 7 | #define M_PI 3.14159265358979323846 /* pi */ |
WiesjeRoskamp | 2:aee655d11b6d | 8 | #include <math.h> |
Rosalie | 5:9f1260408ef2 | 9 | #include "Servo.h" |
Renate | 21:456acc79726c | 10 | #include <cmath> |
RobertoO | 0:67c50348f842 | 11 | |
WiesjeRoskamp | 2:aee655d11b6d | 12 | Serial pc(USBTX, USBRX); |
Rosalie | 3:6ee0b20c23b0 | 13 | |
Renate | 23:4572750a5c59 | 14 | // TICKERS |
Renate | 23:4572750a5c59 | 15 | Ticker loop_ticker; |
Renate | 15:ad065ab92d11 | 16 | |
Renate | 23:4572750a5c59 | 17 | // BENODIGD VOOR PROCESS STATE MACHINE |
Renate | 23:4572750a5c59 | 18 | enum states {Motors_off, Calib_motor, Calib_EMG, Homing, Operation_mode}; |
Renate | 23:4572750a5c59 | 19 | states currentState = Motors_off; |
Renate | 23:4572750a5c59 | 20 | bool stateChanged = true; // Make sure the initialization of first state is executed |
Renate | 23:4572750a5c59 | 21 | |
Renate | 23:4572750a5c59 | 22 | // INPUTS |
Renate | 14:54343b9fd708 | 23 | DigitalIn Power_button_pressed(D1); // Geen InterruptIn gebruiken! |
Renate | 9:4de589636f50 | 24 | DigitalIn Emergency_button_pressed(D2); |
Renate | 22:8585d41a670b | 25 | DigitalIn Motor_calib_button_pressed(SW2); |
WiesjeRoskamp | 2:aee655d11b6d | 26 | |
Renate | 15:ad065ab92d11 | 27 | AnalogIn EMG_biceps_right_raw (A0); |
Renate | 15:ad065ab92d11 | 28 | AnalogIn EMG_biceps_left_raw (A1); |
Renate | 19:1fd39a2afc30 | 29 | AnalogIn EMG_calf_raw (A2); |
Renate | 15:ad065ab92d11 | 30 | |
Renate | 23:4572750a5c59 | 31 | QEI Encoder1(D12, D13, NC, 8400, QEI::X4_ENCODING); //Checken of die D12, D9 etc wel kloppen, 8400= gear ratio x 64 |
Renate | 23:4572750a5c59 | 32 | QEI Encoder2(D9, D10, NC, 8400, QEI::X4_ENCODING); |
Renate | 21:456acc79726c | 33 | |
Renate | 23:4572750a5c59 | 34 | // OUTPUTS |
Renate | 23:4572750a5c59 | 35 | PwmOut motor1(D6); // Misschien moeten we hiervoor DigitalOut gebruiken, moet |
Renate | 23:4572750a5c59 | 36 | PwmOut motor2(D5); // samen kunnen gaan met de servo motor |
Renate | 21:456acc79726c | 37 | |
Renate | 23:4572750a5c59 | 38 | DigitalOut motor1_dir(D7); |
Renate | 23:4572750a5c59 | 39 | DigitalOut motor2_dir(D4); |
Renate | 23:4572750a5c59 | 40 | |
Renate | 23:4572750a5c59 | 41 | // VARIABELEN VOOR ENCODER, MOTORHOEK ETC. |
Renate | 21:456acc79726c | 42 | int counts1; |
Renate | 21:456acc79726c | 43 | int counts2; |
Renate | 21:456acc79726c | 44 | const int CPR = 64; // Counts per revolution |
Renate | 21:456acc79726c | 45 | const int full_degrees = 360; |
Renate | 21:456acc79726c | 46 | const int half_degrees = 180; |
Renate | 21:456acc79726c | 47 | double theta_h_1_deg; |
Renate | 21:456acc79726c | 48 | double theta_h_2_deg; |
Renate | 21:456acc79726c | 49 | double theta_h_1_rad; |
Renate | 21:456acc79726c | 50 | double theta_h_2_rad; |
Renate | 21:456acc79726c | 51 | |
Renate | 23:4572750a5c59 | 52 | // DEFINITIES VOOR FILTERS |
Renate | 20:a6a5bdd7d118 | 53 | |
Renate | 21:456acc79726c | 54 | // BICEPS-RECHTS |
Renate | 21:456acc79726c | 55 | // Definities voor eerste BiQuadChain (High-pass en Notch) |
Renate | 21:456acc79726c | 56 | BiQuadChain bqcbr; |
Renate | 21:456acc79726c | 57 | BiQuad bqbr1(0.8006, -1.6012, 0.8006, -1.5610, 0.6414); // High-pass |
Renate | 21:456acc79726c | 58 | BiQuad bqbr2(1, -1.6180, 1, -1.6019, 0.9801); // Notch |
Renate | 21:456acc79726c | 59 | // Na het nemen van de absolute waarde moet de tweede BiQuadChain worden toegepast. |
Renate | 21:456acc79726c | 60 | // Definieer (twee Low-pass -> vierde orde verkrijgen): |
Renate | 21:456acc79726c | 61 | BiQuadChain bqcbr2; |
Renate | 21:456acc79726c | 62 | BiQuad bqbr3(1.5515e-4, 3.1030e-4, 1.5515e-4, -1.9645, 0.9651); // Low-pass |
Renate | 21:456acc79726c | 63 | BiQuad bqbr4(1.5515e-4, 3.1030e-4, 1.5515e-4, -1.9645, 0.9651); // Low-pass |
Renate | 20:a6a5bdd7d118 | 64 | |
Renate | 21:456acc79726c | 65 | // BICEPS-LINKS |
Renate | 21:456acc79726c | 66 | // Definities voor eerste BiQuadChain (High-pass en Notch) |
Renate | 21:456acc79726c | 67 | BiQuadChain bqcbl; |
Renate | 21:456acc79726c | 68 | BiQuad bqbl1(0.8006, -1.6012, 0.8006, -1.5610, 0.6414); // High-pass |
Renate | 21:456acc79726c | 69 | BiQuad bqbl2(1, -1.6180, 1, -1.6019, 0.9801); // Notch |
Renate | 20:a6a5bdd7d118 | 70 | // Na het nemen van de absolute waarde moet de tweede BiQuadChain worden toegepast. |
Renate | 21:456acc79726c | 71 | // Definieer (twee Low-pass -> vierde orde verkrijgen): |
Renate | 21:456acc79726c | 72 | BiQuadChain bqcbl2; |
Renate | 21:456acc79726c | 73 | BiQuad bqbl3(1.5515e-4, 3.1030e-4, 1.5515e-4, -1.9645, 0.9651); // Low-pass |
Renate | 21:456acc79726c | 74 | BiQuad bqbl4(1.5515e-4, 3.1030e-4, 1.5515e-4, -1.9645, 0.9651); // Low-pass |
Renate | 21:456acc79726c | 75 | |
Renate | 21:456acc79726c | 76 | // KUIT |
Renate | 21:456acc79726c | 77 | // Definities voor eerste BiQuadChain (High-pass en Notch) |
Renate | 21:456acc79726c | 78 | BiQuadChain bqck; |
Renate | 21:456acc79726c | 79 | BiQuad bqk1(0.8006, -1.6012, 0.8006, -1.5610, 0.6414); // High-pass |
Renate | 21:456acc79726c | 80 | BiQuad bqk2(1, -1.6180, 1, -1.6019, 0.9801); // Notch |
Renate | 21:456acc79726c | 81 | // Na het nemen van de absolute waarde moet de tweede BiQuadChain worden toegepast. |
Renate | 21:456acc79726c | 82 | // Definieer (twee Low-pass -> vierde orde verkrijgen): |
Renate | 21:456acc79726c | 83 | BiQuadChain bqck2; |
Renate | 21:456acc79726c | 84 | BiQuad bqk3(1.5515e-4, 3.1030e-4, 1.5515e-4, -1.9645, 0.9651); // Low-pass |
Renate | 21:456acc79726c | 85 | BiQuad bqk4(1.5515e-4, 3.1030e-4, 1.5515e-4, -1.9645, 0.9651); // Low-pass |
Renate | 20:a6a5bdd7d118 | 86 | |
Renate | 23:4572750a5c59 | 87 | // VARIABELEN VOOR EMG + FILTEREN |
Renate | 23:4572750a5c59 | 88 | double filtered_EMG_biceps_right; |
Renate | 23:4572750a5c59 | 89 | double filtered_EMG_biceps_left; |
Renate | 23:4572750a5c59 | 90 | double filtered_EMG_calf; |
Renate | 23:4572750a5c59 | 91 | |
Renate | 23:4572750a5c59 | 92 | double filtered_EMG_biceps_right_1; |
Renate | 23:4572750a5c59 | 93 | double filtered_EMG_biceps_left_1; |
Renate | 23:4572750a5c59 | 94 | double filtered_EMG_calf_1; |
Renate | 23:4572750a5c59 | 95 | |
Renate | 23:4572750a5c59 | 96 | double filtered_EMG_biceps_right_abs; |
Renate | 23:4572750a5c59 | 97 | double filtered_EMG_biceps_left_abs; |
Renate | 23:4572750a5c59 | 98 | double filtered_EMG_calf_abs; |
Renate | 23:4572750a5c59 | 99 | |
Renate | 23:4572750a5c59 | 100 | double filtered_EMG_biceps_right_total; |
Renate | 23:4572750a5c59 | 101 | double filtered_EMG_biceps_left_total; |
Renate | 23:4572750a5c59 | 102 | double filtered_EMG_calf_total; |
Renate | 19:1fd39a2afc30 | 103 | |
Renate | 23:4572750a5c59 | 104 | // Variabelen voor HIDScope |
Renate | 23:4572750a5c59 | 105 | HIDScope scope(3); |
Renate | 23:4572750a5c59 | 106 | |
Renate | 23:4572750a5c59 | 107 | // VARIABELEN VOOR (INITIATIE VAN) EMG KALIBRATIE LOOP |
Renate | 23:4572750a5c59 | 108 | bool calib = false; |
Renate | 23:4572750a5c59 | 109 | static int i_calib = 0; |
Renate | 21:456acc79726c | 110 | |
Renate | 23:4572750a5c59 | 111 | double mean_EMG_biceps_right; |
Renate | 23:4572750a5c59 | 112 | double mean_EMG_biceps_left; |
Renate | 23:4572750a5c59 | 113 | double mean_EMG_calf; |
Renate | 23:4572750a5c59 | 114 | |
Renate | 23:4572750a5c59 | 115 | // VARIABELEN VOOR OPERATION MODE |
Renate | 23:4572750a5c59 | 116 | double normalized_EMG_biceps_right; |
Renate | 23:4572750a5c59 | 117 | double normalized_EMG_biceps_left; |
Renate | 23:4572750a5c59 | 118 | double normalized_EMG_calf; |
Renate | 23:4572750a5c59 | 119 | |
Renate | 23:4572750a5c59 | 120 | // VOIDS |
Renate | 23:4572750a5c59 | 121 | |
Renate | 23:4572750a5c59 | 122 | // Noodfunctie waarbij alles uitgaat (evt. nog een rood LEDje laten branden). |
Renate | 23:4572750a5c59 | 123 | // Enige optie is resetten, dan wordt het script opnieuw opgestart. |
Renate | 8:c7d3b67346db | 124 | void emergency() |
Rosalie | 3:6ee0b20c23b0 | 125 | { |
Renate | 11:4bc0304978e2 | 126 | loop_ticker.detach(); |
Renate | 8:c7d3b67346db | 127 | motor1.write(0); |
Renate | 8:c7d3b67346db | 128 | motor2.write(0); |
Renate | 11:4bc0304978e2 | 129 | pc.printf("Ik ga exploderen!!!\r\n"); |
Renate | 8:c7d3b67346db | 130 | } |
Renate | 11:4bc0304978e2 | 131 | |
Renate | 11:4bc0304978e2 | 132 | // Motoren uitzetten |
Renate | 8:c7d3b67346db | 133 | void motors_off() |
Renate | 8:c7d3b67346db | 134 | { |
Renate | 8:c7d3b67346db | 135 | motor1.write(0); |
Renate | 8:c7d3b67346db | 136 | motor2.write(0); |
Renate | 9:4de589636f50 | 137 | pc.printf("Motoren uit functie\r\n"); |
Renate | 8:c7d3b67346db | 138 | } |
Renate | 8:c7d3b67346db | 139 | |
Renate | 14:54343b9fd708 | 140 | // Motoren aanzetten |
Renate | 15:ad065ab92d11 | 141 | void motors_on() |
Renate | 15:ad065ab92d11 | 142 | { |
Renate | 15:ad065ab92d11 | 143 | motor1.write(0.9); |
Renate | 15:ad065ab92d11 | 144 | motor1_dir.write(1); |
Renate | 15:ad065ab92d11 | 145 | motor2.write(0.1); |
Renate | 15:ad065ab92d11 | 146 | motor1_dir.write(1); |
Renate | 15:ad065ab92d11 | 147 | pc.printf("Motoren aan functie\r\n"); |
Renate | 15:ad065ab92d11 | 148 | } |
Rosalie | 3:6ee0b20c23b0 | 149 | |
Renate | 6:64146e16e10c | 150 | // Finite state machine programming (calibration servo motor?) |
Renate | 6:64146e16e10c | 151 | void ProcessStateMachine(void) |
Renate | 9:4de589636f50 | 152 | { |
Renate | 23:4572750a5c59 | 153 | // Berekenen van de motorhoeken (in radialen) |
Renate | 23:4572750a5c59 | 154 | counts1 = Encoder1.getPulses(); |
Renate | 23:4572750a5c59 | 155 | counts2 = Encoder2.getPulses(); |
Renate | 23:4572750a5c59 | 156 | theta_h_1_deg=(counts1/(double)CPR)*(double)full_degrees; |
Renate | 23:4572750a5c59 | 157 | theta_h_2_deg=(counts2/(double)CPR)*(double)full_degrees; |
Renate | 23:4572750a5c59 | 158 | theta_h_1_rad=(theta_h_1_deg/half_degrees)*M_PI; |
Renate | 23:4572750a5c59 | 159 | theta_h_2_rad=(theta_h_2_deg/half_degrees)*M_PI; |
Renate | 23:4572750a5c59 | 160 | |
Renate | 23:4572750a5c59 | 161 | // Eerste deel van de filters (High-pass + Notch) over het ruwe EMG signaal |
Renate | 23:4572750a5c59 | 162 | // doen. Het ruwe signaal wordt gelezen binnen een ticker en wordt daardoor 'gesampled' |
Renate | 23:4572750a5c59 | 163 | filtered_EMG_biceps_right_1=bqbr1.step(EMG_biceps_right_raw.read()); |
Renate | 23:4572750a5c59 | 164 | filtered_EMG_biceps_left_1=bqcbl.step(EMG_biceps_left_raw.read()); |
Renate | 23:4572750a5c59 | 165 | filtered_EMG_calf_1=bqck.step(EMG_calf_raw.read()); |
Renate | 23:4572750a5c59 | 166 | |
Renate | 23:4572750a5c59 | 167 | // Vervolgens wordt de absolute waarde hiervan genomen |
Renate | 23:4572750a5c59 | 168 | filtered_EMG_biceps_right_abs=abs(filtered_EMG_biceps_right_1); |
Renate | 23:4572750a5c59 | 169 | filtered_EMG_biceps_left_abs=abs(filtered_EMG_biceps_left_1); |
Renate | 23:4572750a5c59 | 170 | filtered_EMG_calf_abs=abs(filtered_EMG_calf_1); |
Renate | 23:4572750a5c59 | 171 | |
Renate | 23:4572750a5c59 | 172 | // Tenslotte wordt het tweede deel van de filters (twee low-pass, voor 4e orde filter) |
Renate | 23:4572750a5c59 | 173 | // over het signaal gedaan |
Renate | 23:4572750a5c59 | 174 | filtered_EMG_biceps_right=bqcbr2.step(filtered_EMG_biceps_right_abs); |
Renate | 23:4572750a5c59 | 175 | filtered_EMG_biceps_left=bqcbl2.step(filtered_EMG_biceps_left_abs); |
Renate | 23:4572750a5c59 | 176 | filtered_EMG_calf=bqck2.step(filtered_EMG_calf_abs); |
Renate | 23:4572750a5c59 | 177 | |
Renate | 23:4572750a5c59 | 178 | // De gefilterde EMG-signalen kunnen tevens visueel worden weergegeven in de HIDScope |
Renate | 23:4572750a5c59 | 179 | scope.set(0, filtered_EMG_biceps_right); |
Renate | 23:4572750a5c59 | 180 | scope.set(1, normalized_EMG_biceps_right); |
Renate | 23:4572750a5c59 | 181 | scope.set(2, filtered_EMG_calf); |
Renate | 23:4572750a5c59 | 182 | scope.send(); |
Renate | 23:4572750a5c59 | 183 | |
Renate | 23:4572750a5c59 | 184 | // Tijdens de kalibratie moet vervolgens een maximale spierspanning worden bepaald, die |
Renate | 23:4572750a5c59 | 185 | // later kan worden gebruikt voor een normalisatie. De spieren worden hiertoe gedurende |
Renate | 23:4572750a5c59 | 186 | // 5 seconden maximaal aangespannen. De EMG waarden worden bij elkaar opgeteld, |
Renate | 23:4572750a5c59 | 187 | // waarna het gemiddelde wordt bepaald. |
Renate | 23:4572750a5c59 | 188 | if (calib) |
Renate | 23:4572750a5c59 | 189 | { |
Renate | 23:4572750a5c59 | 190 | if (i_calib == 0) |
Renate | 23:4572750a5c59 | 191 | { |
Renate | 23:4572750a5c59 | 192 | filtered_EMG_biceps_right_total=0; |
Renate | 23:4572750a5c59 | 193 | filtered_EMG_biceps_left_total=0; |
Renate | 23:4572750a5c59 | 194 | filtered_EMG_calf_total=0; |
Renate | 23:4572750a5c59 | 195 | } |
Renate | 23:4572750a5c59 | 196 | if (i_calib <= 2500) |
Renate | 23:4572750a5c59 | 197 | { |
Renate | 23:4572750a5c59 | 198 | filtered_EMG_biceps_right_total+=filtered_EMG_biceps_right; |
Renate | 23:4572750a5c59 | 199 | filtered_EMG_biceps_left_total+=filtered_EMG_biceps_left; |
Renate | 23:4572750a5c59 | 200 | filtered_EMG_calf_total+=filtered_EMG_calf; |
Renate | 23:4572750a5c59 | 201 | i_calib++; |
Renate | 23:4572750a5c59 | 202 | } |
Renate | 23:4572750a5c59 | 203 | if (i_calib > 2500) |
Renate | 23:4572750a5c59 | 204 | { |
Renate | 23:4572750a5c59 | 205 | mean_EMG_biceps_right=filtered_EMG_biceps_right_total/2500.0; |
Renate | 23:4572750a5c59 | 206 | mean_EMG_biceps_left=filtered_EMG_biceps_left_total/2500.0; |
Renate | 23:4572750a5c59 | 207 | mean_EMG_calf=filtered_EMG_calf_total/2500.0; |
Renate | 23:4572750a5c59 | 208 | pc.printf("Ontspan spieren\r\n"); |
Renate | 23:4572750a5c59 | 209 | pc.printf("Rechterbiceps_max = %f, Linkerbiceps_max = %f, Kuit_max = %f\r\n", mean_EMG_biceps_right, mean_EMG_biceps_left, mean_EMG_calf); |
Renate | 23:4572750a5c59 | 210 | calib = false; |
Renate | 23:4572750a5c59 | 211 | } |
Renate | 23:4572750a5c59 | 212 | } |
Renate | 23:4572750a5c59 | 213 | |
Renate | 23:4572750a5c59 | 214 | // Genormaliseerde EMG's berekenen |
Renate | 23:4572750a5c59 | 215 | normalized_EMG_biceps_right=filtered_EMG_biceps_right/mean_EMG_biceps_right; |
Renate | 23:4572750a5c59 | 216 | normalized_EMG_biceps_left=filtered_EMG_biceps_left/mean_EMG_biceps_left; |
Renate | 23:4572750a5c59 | 217 | normalized_EMG_calf=filtered_EMG_calf/mean_EMG_calf; |
Renate | 23:4572750a5c59 | 218 | |
Renate | 6:64146e16e10c | 219 | switch (currentState) |
Renate | 6:64146e16e10c | 220 | { |
Renate | 6:64146e16e10c | 221 | case Motors_off: |
Renate | 6:64146e16e10c | 222 | |
Renate | 9:4de589636f50 | 223 | if (stateChanged) |
Renate | 6:64146e16e10c | 224 | { |
Renate | 8:c7d3b67346db | 225 | motors_off(); // functie waarbij motoren uitgaan |
Renate | 11:4bc0304978e2 | 226 | stateChanged = false; |
Renate | 9:4de589636f50 | 227 | pc.printf("Motors off state\r\n"); |
Renate | 9:4de589636f50 | 228 | } |
Renate | 11:4bc0304978e2 | 229 | if (Power_button_pressed.read() == false) // Normaal waarde 1 bij indrukken, nu nul -> false |
Renate | 6:64146e16e10c | 230 | { |
Renate | 15:ad065ab92d11 | 231 | motors_on(); |
Renate | 9:4de589636f50 | 232 | currentState = Calib_motor; |
Renate | 11:4bc0304978e2 | 233 | stateChanged = true; |
Renate | 11:4bc0304978e2 | 234 | pc.printf("Moving to Calib_motor state\r\n"); |
Renate | 6:64146e16e10c | 235 | } |
Renate | 11:4bc0304978e2 | 236 | if (Emergency_button_pressed.read() == false) // Normaal waarde 1 bij indrukken, nu nul -> false |
Renate | 8:c7d3b67346db | 237 | { |
Renate | 10:83f3cec8dd1c | 238 | emergency(); |
Renate | 9:4de589636f50 | 239 | } |
Renate | 6:64146e16e10c | 240 | break; |
Renate | 6:64146e16e10c | 241 | |
Renate | 9:4de589636f50 | 242 | case Calib_motor: |
Renate | 9:4de589636f50 | 243 | |
Renate | 22:8585d41a670b | 244 | if (stateChanged && Motor_calib_button_pressed.read() == false) |
Renate | 9:4de589636f50 | 245 | { |
Renate | 21:456acc79726c | 246 | theta_h_1_rad = 0; |
Renate | 21:456acc79726c | 247 | theta_h_2_rad = 0; |
Renate | 21:456acc79726c | 248 | pc.printf("Huidige hoek in radialen motor 1:%f en motor 2: %f (moet 0 zijn) \r\n", theta_h_1_rad, theta_h_2_rad); |
Renate | 11:4bc0304978e2 | 249 | currentState = Calib_EMG; |
Renate | 11:4bc0304978e2 | 250 | stateChanged = true; |
Renate | 9:4de589636f50 | 251 | pc.printf("Moving to Calib_EMG state\r\n"); |
Renate | 9:4de589636f50 | 252 | } |
Renate | 11:4bc0304978e2 | 253 | if (Emergency_button_pressed.read() == false) |
Renate | 11:4bc0304978e2 | 254 | { |
Renate | 11:4bc0304978e2 | 255 | emergency(); |
Renate | 11:4bc0304978e2 | 256 | } |
Renate | 11:4bc0304978e2 | 257 | break; |
Renate | 11:4bc0304978e2 | 258 | |
Renate | 15:ad065ab92d11 | 259 | case Calib_EMG: |
Renate | 11:4bc0304978e2 | 260 | |
Renate | 11:4bc0304978e2 | 261 | if (stateChanged) |
Renate | 19:1fd39a2afc30 | 262 | { |
Renate | 21:456acc79726c | 263 | motors_off(); |
Renate | 22:8585d41a670b | 264 | i_calib = 0; |
Renate | 21:456acc79726c | 265 | calib = true; |
Renate | 21:456acc79726c | 266 | pc.printf("Span spieren aan\r\n"); |
Renate | 21:456acc79726c | 267 | stateChanged = false; |
Renate | 21:456acc79726c | 268 | } |
Renate | 21:456acc79726c | 269 | |
Renate | 23:4572750a5c59 | 270 | if (i_calib > 2500) |
Renate | 21:456acc79726c | 271 | { |
Renate | 21:456acc79726c | 272 | calib = false; |
Renate | 19:1fd39a2afc30 | 273 | currentState = Homing; |
Renate | 19:1fd39a2afc30 | 274 | stateChanged = true; |
Renate | 19:1fd39a2afc30 | 275 | pc.printf("Moving to Homing state\r\n"); |
Renate | 19:1fd39a2afc30 | 276 | } |
Renate | 21:456acc79726c | 277 | |
Renate | 10:83f3cec8dd1c | 278 | if (Emergency_button_pressed.read() == false) |
Renate | 10:83f3cec8dd1c | 279 | { |
Renate | 11:4bc0304978e2 | 280 | emergency(); |
Renate | 11:4bc0304978e2 | 281 | } |
Renate | 11:4bc0304978e2 | 282 | break; |
Renate | 11:4bc0304978e2 | 283 | |
Renate | 23:4572750a5c59 | 284 | case Homing: // NOG NAAR KIJKEN |
Renate | 11:4bc0304978e2 | 285 | |
Renate | 11:4bc0304978e2 | 286 | if (stateChanged) |
Renate | 11:4bc0304978e2 | 287 | { |
Renate | 11:4bc0304978e2 | 288 | // Ervoor zorgen dat de motoren zo bewegen dat de robotarm |
Renate | 11:4bc0304978e2 | 289 | // (inclusief de end-effector) in de juiste home positie wordt gezet |
Renate | 21:456acc79726c | 290 | motors_on(); |
Renate | 11:4bc0304978e2 | 291 | currentState = Operation_mode; |
Renate | 11:4bc0304978e2 | 292 | stateChanged = true; |
Renate | 23:4572750a5c59 | 293 | motors_off(); |
Renate | 12:93ad9781eeef | 294 | pc.printf("Moving to operation mode \r\n"); |
Renate | 11:4bc0304978e2 | 295 | } |
Renate | 11:4bc0304978e2 | 296 | if (Emergency_button_pressed.read() == false) |
Renate | 11:4bc0304978e2 | 297 | { |
Renate | 10:83f3cec8dd1c | 298 | emergency(); |
Renate | 10:83f3cec8dd1c | 299 | } |
Renate | 11:4bc0304978e2 | 300 | break; |
Renate | 11:4bc0304978e2 | 301 | |
Renate | 14:54343b9fd708 | 302 | case Operation_mode: // Overgaan tot emergency wanneer referentie niet |
Renate | 14:54343b9fd708 | 303 | // overeenkomt met werkelijkheid |
Renate | 21:456acc79726c | 304 | |
Renate | 23:4572750a5c59 | 305 | // pc.printf("normalized_EMG_biceps_right= %f, mean_EMG_biceps_right = %f, filtered_EMG_biceps_right = %f\r\n", normalized_EMG_biceps_right, mean_EMG_biceps_right, filtered_EMG_biceps_right); |
Renate | 11:4bc0304978e2 | 306 | if (stateChanged) |
Renate | 12:93ad9781eeef | 307 | |
Renate | 11:4bc0304978e2 | 308 | // Hier moet een functie worden aangeroepen die ervoor zorgt dat |
Renate | 11:4bc0304978e2 | 309 | // aan de hand van EMG-signalen de motoren kunnen worden aangestuurd, |
Renate | 23:4572750a5c59 | 310 | // zodat de robotarm kan bewegen |
Renate | 11:4bc0304978e2 | 311 | |
Renate | 21:456acc79726c | 312 | { |
Renate | 20:a6a5bdd7d118 | 313 | if (normalized_EMG_biceps_right >= 0.3) |
Renate | 20:a6a5bdd7d118 | 314 | { |
Renate | 26:7ae60739b310 | 315 | motor1.write(0.3); |
Renate | 21:456acc79726c | 316 | motor1_dir.write(1); |
Renate | 26:7ae60739b310 | 317 | motor2.write(0.3); |
Renate | 21:456acc79726c | 318 | motor2_dir.write(1); |
Renate | 21:456acc79726c | 319 | if (normalized_EMG_calf >= 0.3) |
Renate | 21:456acc79726c | 320 | { |
Renate | 23:4572750a5c59 | 321 | motor1.write(0.1); |
Renate | 23:4572750a5c59 | 322 | motor1_dir = !motor1_dir; |
Renate | 21:456acc79726c | 323 | } |
Renate | 23:4572750a5c59 | 324 | if (normalized_EMG_biceps_left >= 0.3) |
Renate | 23:4572750a5c59 | 325 | { |
Renate | 23:4572750a5c59 | 326 | motor2.write(0.9); |
Renate | 23:4572750a5c59 | 327 | motor2_dir.write(1); |
Renate | 26:7ae60739b310 | 328 | motor1.write(0.9); |
Renate | 23:4572750a5c59 | 329 | motor1_dir.write(1); |
Renate | 23:4572750a5c59 | 330 | if (normalized_EMG_calf >= 0.3) |
Renate | 23:4572750a5c59 | 331 | { |
Renate | 23:4572750a5c59 | 332 | motor2.write(0.1); |
Renate | 23:4572750a5c59 | 333 | motor2_dir = !motor2_dir; |
Renate | 23:4572750a5c59 | 334 | } |
Renate | 23:4572750a5c59 | 335 | } |
Renate | 20:a6a5bdd7d118 | 336 | } |
Renate | 20:a6a5bdd7d118 | 337 | if (normalized_EMG_biceps_right < 0.3) |
Renate | 20:a6a5bdd7d118 | 338 | { |
Renate | 20:a6a5bdd7d118 | 339 | motor1.write(0); |
Renate | 20:a6a5bdd7d118 | 340 | motor2.write(0); |
Renate | 23:4572750a5c59 | 341 | if (normalized_EMG_calf >= 0.3) |
Renate | 23:4572750a5c59 | 342 | { |
Renate | 23:4572750a5c59 | 343 | // motor1_dir = !motor1_dir; |
Renate | 23:4572750a5c59 | 344 | // pc.printf("Richting zou om moeten draaien"); |
Renate | 23:4572750a5c59 | 345 | // motor2_dir = !motor2_dir; |
Renate | 23:4572750a5c59 | 346 | } |
Renate | 23:4572750a5c59 | 347 | if (normalized_EMG_biceps_left >= 0.3) |
Renate | 23:4572750a5c59 | 348 | { |
Renate | 23:4572750a5c59 | 349 | motor2.write(0.9); |
Renate | 23:4572750a5c59 | 350 | motor2_dir.write(1); |
Renate | 26:7ae60739b310 | 351 | motor1.write(0.9); |
Renate | 23:4572750a5c59 | 352 | motor1_dir.write(1); |
Renate | 23:4572750a5c59 | 353 | if (normalized_EMG_calf >= 0.3) |
Renate | 23:4572750a5c59 | 354 | { |
Renate | 23:4572750a5c59 | 355 | // motor1_dir = !motor1_dir; |
Renate | 23:4572750a5c59 | 356 | // pc.printf("Richting zou om moeten draaien"); |
Renate | 23:4572750a5c59 | 357 | // motor2_dir = !motor2_dir; |
Renate | 23:4572750a5c59 | 358 | } |
Renate | 23:4572750a5c59 | 359 | } |
Renate | 20:a6a5bdd7d118 | 360 | } |
Renate | 20:a6a5bdd7d118 | 361 | if (normalized_EMG_biceps_left >= 0.3) |
Renate | 20:a6a5bdd7d118 | 362 | { |
Renate | 20:a6a5bdd7d118 | 363 | motor2.write(0.9); |
Renate | 21:456acc79726c | 364 | motor2_dir.write(1); |
Renate | 26:7ae60739b310 | 365 | motor1.write(0.9); |
Renate | 21:456acc79726c | 366 | motor1_dir.write(1); |
Renate | 21:456acc79726c | 367 | if (normalized_EMG_calf >= 0.3) |
Renate | 21:456acc79726c | 368 | { |
Renate | 21:456acc79726c | 369 | // motor1_dir = !motor1_dir; |
Renate | 23:4572750a5c59 | 370 | // pc.printf("Richting zou om moeten draaien"); |
Renate | 21:456acc79726c | 371 | // motor2_dir = !motor2_dir; |
Renate | 21:456acc79726c | 372 | } |
Renate | 23:4572750a5c59 | 373 | if (normalized_EMG_biceps_right >= 0.3) |
Renate | 23:4572750a5c59 | 374 | { |
Renate | 26:7ae60739b310 | 375 | motor1.write(0.3); |
Renate | 23:4572750a5c59 | 376 | motor1_dir.write(1); |
Renate | 26:7ae60739b310 | 377 | motor2.write(0.3); |
Renate | 23:4572750a5c59 | 378 | motor2_dir.write(1); |
Renate | 23:4572750a5c59 | 379 | if (normalized_EMG_calf >= 0.3) |
Renate | 23:4572750a5c59 | 380 | { |
Renate | 23:4572750a5c59 | 381 | // motor1_dir = !motor1_dir; |
Renate | 23:4572750a5c59 | 382 | // pc.printf("Richting zou om moeten draaien"); |
Renate | 23:4572750a5c59 | 383 | // motor2_dir = !motor2_dir; |
Renate | 23:4572750a5c59 | 384 | } |
Renate | 23:4572750a5c59 | 385 | } |
Renate | 20:a6a5bdd7d118 | 386 | } |
Renate | 20:a6a5bdd7d118 | 387 | if (normalized_EMG_biceps_left < 0.3) |
Renate | 20:a6a5bdd7d118 | 388 | { |
Renate | 20:a6a5bdd7d118 | 389 | motor2.write(0); |
Renate | 20:a6a5bdd7d118 | 390 | motor1.write(0); |
Renate | 23:4572750a5c59 | 391 | if (normalized_EMG_biceps_right >= 0.3) |
Renate | 23:4572750a5c59 | 392 | { |
Renate | 26:7ae60739b310 | 393 | motor1.write(0.3); |
Renate | 23:4572750a5c59 | 394 | motor1_dir.write(1); |
Renate | 26:7ae60739b310 | 395 | motor2.write(0.3); |
Renate | 23:4572750a5c59 | 396 | motor2_dir.write(1); |
Renate | 23:4572750a5c59 | 397 | if (normalized_EMG_calf >= 0.3) |
Renate | 23:4572750a5c59 | 398 | { |
Renate | 23:4572750a5c59 | 399 | // motor1_dir = !motor1_dir; |
Renate | 23:4572750a5c59 | 400 | // pc.printf("Richting zou om moeten draaien"); |
Renate | 23:4572750a5c59 | 401 | // motor2_dir = !motor2_dir; |
Renate | 23:4572750a5c59 | 402 | } |
Renate | 23:4572750a5c59 | 403 | } |
Renate | 23:4572750a5c59 | 404 | } |
Renate | 23:4572750a5c59 | 405 | } |
Renate | 23:4572750a5c59 | 406 | |
Renate | 21:456acc79726c | 407 | if (Power_button_pressed.read() == false) // Normaal waarde 1 bij indrukken, nu nul -> false |
Renate | 21:456acc79726c | 408 | { |
Renate | 21:456acc79726c | 409 | motors_off(); |
Renate | 21:456acc79726c | 410 | currentState = Motors_off; |
Renate | 21:456acc79726c | 411 | stateChanged = true; |
Renate | 21:456acc79726c | 412 | pc.printf("Terug naar de state Motors_off\r\n"); |
Renate | 21:456acc79726c | 413 | } |
Renate | 21:456acc79726c | 414 | if (Emergency_button_pressed.read() == false) |
Renate | 21:456acc79726c | 415 | { |
Renate | 21:456acc79726c | 416 | emergency(); |
Renate | 21:456acc79726c | 417 | } |
Renate | 21:456acc79726c | 418 | // wait(25); |
Renate | 21:456acc79726c | 419 | // else |
Renate | 21:456acc79726c | 420 | // { |
Renate | 21:456acc79726c | 421 | // currentState = Homing; |
Renate | 21:456acc79726c | 422 | // stateChanged = true; |
Renate | 21:456acc79726c | 423 | // pc.printf("Terug naar de state Homing\r\n"); |
Renate | 21:456acc79726c | 424 | // } |
Renate | 21:456acc79726c | 425 | break; |
Renate | 20:a6a5bdd7d118 | 426 | |
Renate | 7:1d57463393c6 | 427 | default: |
Renate | 7:1d57463393c6 | 428 | // Zelfde functie als die eerder is toegepast om motoren uit te schakelen -> safety! |
Renate | 14:54343b9fd708 | 429 | motors_off(); |
Renate | 9:4de589636f50 | 430 | pc.printf("Unknown or uninplemented state reached!\r\n"); |
Renate | 8:c7d3b67346db | 431 | |
WiesjeRoskamp | 2:aee655d11b6d | 432 | } |
Renate | 11:4bc0304978e2 | 433 | } |
WiesjeRoskamp | 2:aee655d11b6d | 434 | |
Renate | 8:c7d3b67346db | 435 | int main(void) |
Renate | 8:c7d3b67346db | 436 | { |
Renate | 9:4de589636f50 | 437 | pc.printf("Opstarten\r\n"); |
Renate | 21:456acc79726c | 438 | |
Renate | 21:456acc79726c | 439 | // Chain voor rechter biceps |
Renate | 21:456acc79726c | 440 | bqcbr.add(&bqbr1).add(&bqbr2); |
Renate | 21:456acc79726c | 441 | bqcbr2.add(&bqbr3).add(&bqbr4); |
Renate | 21:456acc79726c | 442 | // Chain voor linker biceps |
Renate | 21:456acc79726c | 443 | bqcbl.add(&bqbl1).add(&bqbl2); |
Renate | 21:456acc79726c | 444 | bqcbl2.add(&bqbl3).add(&bqbl4); |
Renate | 21:456acc79726c | 445 | // Chain voor kuit |
Renate | 21:456acc79726c | 446 | bqck.add(&bqk1).add(&bqk2); |
Renate | 21:456acc79726c | 447 | bqck2.add(&bqk3).add(&bqk4); |
Renate | 23:4572750a5c59 | 448 | |
Renate | 23:4572750a5c59 | 449 | loop_ticker.attach(&ProcessStateMachine, 0.002f); |
Renate | 21:456acc79726c | 450 | |
Renate | 8:c7d3b67346db | 451 | while(true) |
Renate | 21:456acc79726c | 452 | { |
Renate | 21:456acc79726c | 453 | // wait(0.2); |
Renate | 21:456acc79726c | 454 | /* do nothing */ |
Renate | 21:456acc79726c | 455 | } |
Renate | 8:c7d3b67346db | 456 | } |