Renate de Boer / Mbed 2 deprecated script_voor_project_copy

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   Servoaansturing mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

main.cpp@24:764b71885785, 2019-10-29 (annotated)

Committer:
Renate
Date:
Tue Oct 29 09:34:25 2019 +0000
Revision:
24:764b71885785
Parent:
23:4572750a5c59
Child:
25:582e0b1a9a78
Werkt niet, alle opties apart

Who changed what in which revision?

UserRevisionLine numberNew 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 24:764b71885785 315 motor1.write(0.3);
Renate 24:764b71885785 316 motor2.write(0.3);
Renate 24:764b71885785 317 // motor1_dir.write(1);
Renate 24:764b71885785 318 // motor2.write(0);
Renate 24:764b71885785 319 // motor2_dir.write(1);
Renate 21:456acc79726c 320 if (normalized_EMG_calf >= 0.3)
Renate 21:456acc79726c 321 {
Renate 23:4572750a5c59 322 motor1.write(0.1);
Renate 24:764b71885785 323 motor2.write(0.1);
Renate 23:4572750a5c59 324 motor1_dir = !motor1_dir;
Renate 24:764b71885785 325 motor2_dir = !motor2_dir;
Renate 24:764b71885785 326 motor1.write(0.3);
Renate 24:764b71885785 327 motor2.write(0.3);
Renate 21:456acc79726c 328 }
Renate 23:4572750a5c59 329 if (normalized_EMG_biceps_left >= 0.3)
Renate 23:4572750a5c59 330 {
Renate 24:764b71885785 331 motor1.write(0.8);
Renate 24:764b71885785 332 motor2.write(0.8);
Renate 24:764b71885785 333 // motor2_dir.write(1);
Renate 24:764b71885785 334 // motor1.write(0);
Renate 24:764b71885785 335 // motor1_dir.write(1);
Renate 23:4572750a5c59 336 if (normalized_EMG_calf >= 0.3)
Renate 24:764b71885785 337 {
Renate 24:764b71885785 338 motor1.write(0.1);
Renate 23:4572750a5c59 339 motor2.write(0.1);
Renate 24:764b71885785 340 motor1_dir = !motor1_dir;
Renate 23:4572750a5c59 341 motor2_dir = !motor2_dir;
Renate 24:764b71885785 342 motor1.write(0.8);
Renate 24:764b71885785 343 motor2.write(0.8);
Renate 23:4572750a5c59 344 }
Renate 23:4572750a5c59 345 }
Renate 20:a6a5bdd7d118 346 }
Renate 20:a6a5bdd7d118 347 if (normalized_EMG_biceps_right < 0.3)
Renate 20:a6a5bdd7d118 348 {
Renate 20:a6a5bdd7d118 349 motor1.write(0);
Renate 20:a6a5bdd7d118 350 motor2.write(0);
Renate 23:4572750a5c59 351 if (normalized_EMG_biceps_left >= 0.3)
Renate 23:4572750a5c59 352 {
Renate 24:764b71885785 353 motor1.write(0.8);
Renate 24:764b71885785 354 motor2.write(0.8);
Renate 24:764b71885785 355 // motor1.write(0);
Renate 24:764b71885785 356 // motor1_dir.write(1);
Renate 23:4572750a5c59 357 if (normalized_EMG_calf >= 0.3)
Renate 23:4572750a5c59 358 {
Renate 24:764b71885785 359 motor1.write(0.1);
Renate 24:764b71885785 360 motor2.write(0.1);
Renate 24:764b71885785 361 motor1_dir = !motor1_dir;
Renate 24:764b71885785 362 motor2_dir = !motor2_dir;
Renate 24:764b71885785 363 motor1.write(0.8);
Renate 24:764b71885785 364 motor2.write(0.8);
Renate 23:4572750a5c59 365 }
Renate 23:4572750a5c59 366 }
Renate 20:a6a5bdd7d118 367 }
Renate 20:a6a5bdd7d118 368 if (normalized_EMG_biceps_left >= 0.3)
Renate 20:a6a5bdd7d118 369 {
Renate 24:764b71885785 370 motor1.write(0.8);
Renate 24:764b71885785 371 motor2.write(0.8);
Renate 24:764b71885785 372 // motor1.write(0);
Renate 24:764b71885785 373 // motor1_dir.write(1);
Renate 21:456acc79726c 374 if (normalized_EMG_calf >= 0.3)
Renate 21:456acc79726c 375 {
Renate 24:764b71885785 376 motor1.write(0.1);
Renate 24:764b71885785 377 motor2.write(0.1);
Renate 24:764b71885785 378 motor1_dir = !motor1_dir;
Renate 24:764b71885785 379 motor2_dir = !motor2_dir;
Renate 24:764b71885785 380 motor1.write(0.8);
Renate 24:764b71885785 381 motor2.write(0.8);
Renate 21:456acc79726c 382 }
Renate 23:4572750a5c59 383 if (normalized_EMG_biceps_right >= 0.3)
Renate 23:4572750a5c59 384 {
Renate 24:764b71885785 385 motor1.write(0.3);
Renate 24:764b71885785 386 motor2.write(0.3);
Renate 23:4572750a5c59 387 if (normalized_EMG_calf >= 0.3)
Renate 23:4572750a5c59 388 {
Renate 24:764b71885785 389 motor1.write(0.1);
Renate 24:764b71885785 390 motor2.write(0.1);
Renate 24:764b71885785 391 motor1_dir = !motor1_dir;
Renate 24:764b71885785 392 motor2_dir = !motor2_dir;
Renate 24:764b71885785 393 motor1.write(0.3);
Renate 24:764b71885785 394 motor2.write(0.3);
Renate 23:4572750a5c59 395 }
Renate 23:4572750a5c59 396 }
Renate 20:a6a5bdd7d118 397 }
Renate 20:a6a5bdd7d118 398 if (normalized_EMG_biceps_left < 0.3)
Renate 20:a6a5bdd7d118 399 {
Renate 24:764b71885785 400 motor1.write(0);
Renate 20:a6a5bdd7d118 401 motor2.write(0);
Renate 23:4572750a5c59 402 if (normalized_EMG_biceps_right >= 0.3)
Renate 23:4572750a5c59 403 {
Renate 24:764b71885785 404 motor1.write(0.3);
Renate 24:764b71885785 405 motor2.write(0.3);
Renate 24:764b71885785 406 // motor2.write(0);
Renate 24:764b71885785 407 // motor2_dir.write(1);
Renate 23:4572750a5c59 408 if (normalized_EMG_calf >= 0.3)
Renate 23:4572750a5c59 409 {
Renate 24:764b71885785 410 motor1.write(0.1);
Renate 24:764b71885785 411 motor2.write(0.1);
Renate 24:764b71885785 412 motor1_dir = !motor1_dir;
Renate 24:764b71885785 413 motor2_dir = !motor2_dir;
Renate 24:764b71885785 414 motor1.write(0.3);
Renate 24:764b71885785 415 motor2.write(0.3);
Renate 23:4572750a5c59 416 }
Renate 23:4572750a5c59 417 }
Renate 23:4572750a5c59 418 }
Renate 23:4572750a5c59 419 }
Renate 23:4572750a5c59 420
Renate 21:456acc79726c 421 if (Power_button_pressed.read() == false) // Normaal waarde 1 bij indrukken, nu nul -> false
Renate 21:456acc79726c 422 {
Renate 21:456acc79726c 423 motors_off();
Renate 21:456acc79726c 424 currentState = Motors_off;
Renate 21:456acc79726c 425 stateChanged = true;
Renate 21:456acc79726c 426 pc.printf("Terug naar de state Motors_off\r\n");
Renate 21:456acc79726c 427 }
Renate 21:456acc79726c 428 if (Emergency_button_pressed.read() == false)
Renate 21:456acc79726c 429 {
Renate 21:456acc79726c 430 emergency();
Renate 21:456acc79726c 431 }
Renate 21:456acc79726c 432 // wait(25);
Renate 21:456acc79726c 433 // else
Renate 21:456acc79726c 434 // {
Renate 21:456acc79726c 435 // currentState = Homing;
Renate 21:456acc79726c 436 // stateChanged = true;
Renate 21:456acc79726c 437 // pc.printf("Terug naar de state Homing\r\n");
Renate 21:456acc79726c 438 // }
Renate 21:456acc79726c 439 break;
Renate 20:a6a5bdd7d118 440
Renate 7:1d57463393c6 441 default:
Renate 7:1d57463393c6 442 // Zelfde functie als die eerder is toegepast om motoren uit te schakelen -> safety!
Renate 14:54343b9fd708 443 motors_off();
Renate 9:4de589636f50 444 pc.printf("Unknown or uninplemented state reached!\r\n");
Renate 8:c7d3b67346db 445
WiesjeRoskamp 2:aee655d11b6d 446 }
Renate 11:4bc0304978e2 447 }
WiesjeRoskamp 2:aee655d11b6d 448
Renate 8:c7d3b67346db 449 int main(void)
Renate 8:c7d3b67346db 450 {
Renate 9:4de589636f50 451 pc.printf("Opstarten\r\n");
Renate 21:456acc79726c 452
Renate 21:456acc79726c 453 // Chain voor rechter biceps
Renate 21:456acc79726c 454 bqcbr.add(&bqbr1).add(&bqbr2);
Renate 21:456acc79726c 455 bqcbr2.add(&bqbr3).add(&bqbr4);
Renate 21:456acc79726c 456 // Chain voor linker biceps
Renate 21:456acc79726c 457 bqcbl.add(&bqbl1).add(&bqbl2);
Renate 21:456acc79726c 458 bqcbl2.add(&bqbl3).add(&bqbl4);
Renate 21:456acc79726c 459 // Chain voor kuit
Renate 21:456acc79726c 460 bqck.add(&bqk1).add(&bqk2);
Renate 21:456acc79726c 461 bqck2.add(&bqk3).add(&bqk4);
Renate 23:4572750a5c59 462
Renate 23:4572750a5c59 463 loop_ticker.attach(&ProcessStateMachine, 0.002f);
Renate 21:456acc79726c 464
Renate 8:c7d3b67346db 465 while(true)
Renate 21:456acc79726c 466 {
Renate 21:456acc79726c 467 // wait(0.2);
Renate 21:456acc79726c 468 /* do nothing */
Renate 21:456acc79726c 469 }
Renate 8:c7d3b67346db 470 }