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@22:8585d41a670b, 2019-10-28 (annotated)

Committer:
Renate
Date:
Mon Oct 28 15:40:31 2019 +0000
Revision:
22:8585d41a670b
Parent:
21:456acc79726c
Child:
23:4572750a5c59
Versie voordat alles in een ticker wordt gezet

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
Renate 11:4bc0304978e2 12 // Definieer objecten
WiesjeRoskamp 2:aee655d11b6d 13 Serial pc(USBTX, USBRX);
Rosalie 3:6ee0b20c23b0 14
Renate 14:54343b9fd708 15 PwmOut motor1(D6); // Misschien moeten we hiervoor DigitalOut gebruiken, moet
Renate 14:54343b9fd708 16 PwmOut motor2(D5); // samen kunnen gaan met de servo motor
Renate 15:ad065ab92d11 17
Renate 15:ad065ab92d11 18 DigitalOut motor1_dir(D7);
Renate 15:ad065ab92d11 19 DigitalOut motor2_dir(D4);
Renate 14:54343b9fd708 20
Renate 14:54343b9fd708 21 DigitalIn Power_button_pressed(D1); // Geen InterruptIn gebruiken!
Renate 9:4de589636f50 22 DigitalIn Emergency_button_pressed(D2);
Renate 22:8585d41a670b 23 DigitalIn Motor_calib_button_pressed(SW2);
WiesjeRoskamp 2:aee655d11b6d 24
Renate 15:ad065ab92d11 25 AnalogIn EMG_biceps_right_raw (A0);
Renate 15:ad065ab92d11 26 AnalogIn EMG_biceps_left_raw (A1);
Renate 19:1fd39a2afc30 27 AnalogIn EMG_calf_raw (A2);
Renate 15:ad065ab92d11 28
Renate 8:c7d3b67346db 29 Ticker loop_ticker;
Renate 19:1fd39a2afc30 30 Ticker HIDScope_ticker;
Renate 19:1fd39a2afc30 31 Ticker emgSampleTicker;
Renate 21:456acc79726c 32 Ticker motorTicker;
Renate 21:456acc79726c 33
Renate 21:456acc79726c 34 QEI Encoder1(D12, D13, NC, 8400, QEI::X4_ENCODING); //Checken of die D12 etc wel kloppen.
Renate 21:456acc79726c 35 QEI Encoder2(D9, D10, NC, 8400, QEI::X4_ENCODING); //Checken of die D9 etc wel kloppen.
Renate 21:456acc79726c 36 // 8400= gear ratio x 64
Renate 21:456acc79726c 37
Renate 21:456acc79726c 38 // Definities variabelen encoder -> motorhoek
Renate 21:456acc79726c 39 int counts1;
Renate 21:456acc79726c 40 int counts2;
Renate 21:456acc79726c 41 const int CPR = 64; // Counts per revolution
Renate 21:456acc79726c 42 const int full_degrees = 360;
Renate 21:456acc79726c 43 const int half_degrees = 180;
Renate 21:456acc79726c 44 double theta_h_1_deg;
Renate 21:456acc79726c 45 double theta_h_2_deg;
Renate 21:456acc79726c 46 double theta_h_1_rad;
Renate 21:456acc79726c 47 double theta_h_2_rad;
Renate 21:456acc79726c 48
Renate 21:456acc79726c 49 void Calculate_motor_angle()
Renate 21:456acc79726c 50 {
Renate 21:456acc79726c 51 counts1 = Encoder1.getPulses();
Renate 21:456acc79726c 52 counts2 = Encoder2.getPulses();
Renate 21:456acc79726c 53 theta_h_1_deg=(counts1/(double)CPR)*(double)full_degrees;
Renate 21:456acc79726c 54 theta_h_2_deg=(counts2/(double)CPR)*(double)full_degrees;
Renate 21:456acc79726c 55 theta_h_1_rad=(theta_h_1_deg/half_degrees)*M_PI;
Renate 21:456acc79726c 56 theta_h_2_rad=(theta_h_2_deg/half_degrees)*M_PI;
Renate 21:456acc79726c 57 }
Renate 19:1fd39a2afc30 58
Renate 19:1fd39a2afc30 59 bool calib = false; // MOGELIJK GAAT HET HIER FOUT
Renate 21:456acc79726c 60 static int i_calib = 0;
Renate 19:1fd39a2afc30 61
Renate 20:a6a5bdd7d118 62 HIDScope scope(3);
Renate 20:a6a5bdd7d118 63
Renate 21:456acc79726c 64 // Defining global variables
Renate 21:456acc79726c 65 double mean_EMG_biceps_right;
Renate 21:456acc79726c 66 double mean_EMG_biceps_left;
Renate 21:456acc79726c 67 double mean_EMG_calf;
Renate 21:456acc79726c 68 double normalized_EMG_biceps_right;
Renate 21:456acc79726c 69 double normalized_EMG_biceps_left;
Renate 21:456acc79726c 70 double normalized_EMG_calf;
Renate 21:456acc79726c 71 static double filtered_EMG_biceps_right;
Renate 21:456acc79726c 72 double filtered_EMG_biceps_left;
Renate 21:456acc79726c 73 double filtered_EMG_calf;
Renate 21:456acc79726c 74 double filtered_EMG_biceps_left_1;
Renate 21:456acc79726c 75 double filtered_EMG_biceps_right_1;
Renate 21:456acc79726c 76 double filtered_EMG_calf_1;
Renate 21:456acc79726c 77 double filtered_EMG_biceps_right_abs;
Renate 21:456acc79726c 78 double filtered_EMG_biceps_left_abs;
Renate 21:456acc79726c 79 double filtered_EMG_calf_abs;
Renate 21:456acc79726c 80 static double filtered_EMG_biceps_right_total;
Renate 21:456acc79726c 81 double filtered_EMG_biceps_left_total;
Renate 21:456acc79726c 82 double filtered_EMG_calf_total;
Renate 20:a6a5bdd7d118 83
Renate 21:456acc79726c 84 // BICEPS-RECHTS
Renate 21:456acc79726c 85 // Definities voor eerste BiQuadChain (High-pass en Notch)
Renate 21:456acc79726c 86 BiQuadChain bqcbr;
Renate 21:456acc79726c 87 BiQuad bqbr1(0.8006, -1.6012, 0.8006, -1.5610, 0.6414); // High-pass
Renate 21:456acc79726c 88 BiQuad bqbr2(1, -1.6180, 1, -1.6019, 0.9801); // Notch
Renate 21:456acc79726c 89 // Na het nemen van de absolute waarde moet de tweede BiQuadChain worden toegepast.
Renate 21:456acc79726c 90 // Definieer (twee Low-pass -> vierde orde verkrijgen):
Renate 21:456acc79726c 91 BiQuadChain bqcbr2;
Renate 21:456acc79726c 92 BiQuad bqbr3(1.5515e-4, 3.1030e-4, 1.5515e-4, -1.9645, 0.9651); // Low-pass
Renate 21:456acc79726c 93 BiQuad bqbr4(1.5515e-4, 3.1030e-4, 1.5515e-4, -1.9645, 0.9651); // Low-pass
Renate 20:a6a5bdd7d118 94
Renate 21:456acc79726c 95 // BICEPS-LINKS
Renate 21:456acc79726c 96 // Definities voor eerste BiQuadChain (High-pass en Notch)
Renate 21:456acc79726c 97 BiQuadChain bqcbl;
Renate 21:456acc79726c 98 BiQuad bqbl1(0.8006, -1.6012, 0.8006, -1.5610, 0.6414); // High-pass
Renate 21:456acc79726c 99 BiQuad bqbl2(1, -1.6180, 1, -1.6019, 0.9801); // Notch
Renate 20:a6a5bdd7d118 100 // Na het nemen van de absolute waarde moet de tweede BiQuadChain worden toegepast.
Renate 21:456acc79726c 101 // Definieer (twee Low-pass -> vierde orde verkrijgen):
Renate 21:456acc79726c 102 BiQuadChain bqcbl2;
Renate 21:456acc79726c 103 BiQuad bqbl3(1.5515e-4, 3.1030e-4, 1.5515e-4, -1.9645, 0.9651); // Low-pass
Renate 21:456acc79726c 104 BiQuad bqbl4(1.5515e-4, 3.1030e-4, 1.5515e-4, -1.9645, 0.9651); // Low-pass
Renate 21:456acc79726c 105
Renate 21:456acc79726c 106 // KUIT
Renate 21:456acc79726c 107 // Definities voor eerste BiQuadChain (High-pass en Notch)
Renate 21:456acc79726c 108 BiQuadChain bqck;
Renate 21:456acc79726c 109 BiQuad bqk1(0.8006, -1.6012, 0.8006, -1.5610, 0.6414); // High-pass
Renate 21:456acc79726c 110 BiQuad bqk2(1, -1.6180, 1, -1.6019, 0.9801); // Notch
Renate 21:456acc79726c 111 // Na het nemen van de absolute waarde moet de tweede BiQuadChain worden toegepast.
Renate 21:456acc79726c 112 // Definieer (twee Low-pass -> vierde orde verkrijgen):
Renate 21:456acc79726c 113 BiQuadChain bqck2;
Renate 21:456acc79726c 114 BiQuad bqk3(1.5515e-4, 3.1030e-4, 1.5515e-4, -1.9645, 0.9651); // Low-pass
Renate 21:456acc79726c 115 BiQuad bqk4(1.5515e-4, 3.1030e-4, 1.5515e-4, -1.9645, 0.9651); // Low-pass
Renate 20:a6a5bdd7d118 116
Renate 19:1fd39a2afc30 117 void emgSampleFilter() // Deze functie wordt aangeroepen dmv een ticker. Het sampled
Renate 20:a6a5bdd7d118 118 // hierdoor het EMG signaal en het haalt er een filter overheen.
Renate 21:456acc79726c 119 // Tevens wordt er een stuk script gerund, wanneer de robot
Renate 20:a6a5bdd7d118 120 // zich in de kalibratie toestand bevindt.
Renate 19:1fd39a2afc30 121 {
Renate 21:456acc79726c 122 filtered_EMG_biceps_right_1=bqbr1.step(EMG_biceps_right_raw.read());
Renate 21:456acc79726c 123 filtered_EMG_biceps_left_1=bqcbl.step(EMG_biceps_left_raw.read());
Renate 21:456acc79726c 124 filtered_EMG_calf_1=bqck.step(EMG_calf_raw.read());
Renate 20:a6a5bdd7d118 125
Renate 21:456acc79726c 126 filtered_EMG_biceps_right_abs=abs(filtered_EMG_biceps_right_1);
Renate 21:456acc79726c 127 filtered_EMG_biceps_left_abs=abs(filtered_EMG_biceps_left_1);
Renate 21:456acc79726c 128 filtered_EMG_calf_abs=abs(filtered_EMG_calf_1);
Renate 19:1fd39a2afc30 129
Renate 21:456acc79726c 130 filtered_EMG_biceps_right=bqcbr2.step(filtered_EMG_biceps_right_abs);
Renate 21:456acc79726c 131 filtered_EMG_biceps_left=bqcbl2.step(filtered_EMG_biceps_left_abs);
Renate 21:456acc79726c 132 filtered_EMG_calf=bqck2.step(filtered_EMG_calf_abs);
Renate 19:1fd39a2afc30 133
Renate 20:a6a5bdd7d118 134 if (calib) // In de kalibratie staat treedt deze loop in werking. De spier wordt
Renate 20:a6a5bdd7d118 135 // dan maximaal aangespannen (gedurende 5 seconden). De EMG waarden
Renate 21:456acc79726c 136 // worden bij elkaar opgeteld, waarna het gemiddelde wordt bepaald.
Renate 19:1fd39a2afc30 137 {
Renate 22:8585d41a670b 138 if (i_calib == 0)
Renate 22:8585d41a670b 139 {
Renate 22:8585d41a670b 140 filtered_EMG_biceps_right_total=0;
Renate 22:8585d41a670b 141 filtered_EMG_biceps_left_total=0;
Renate 22:8585d41a670b 142 filtered_EMG_calf_total=0;
Renate 22:8585d41a670b 143 }
Renate 22:8585d41a670b 144 if (i_calib <= 2500)
Renate 19:1fd39a2afc30 145 {
Renate 21:456acc79726c 146 filtered_EMG_biceps_right_total+=filtered_EMG_biceps_right;
Renate 21:456acc79726c 147 // pc.printf("%f\r\n", filtered_EMG_biceps_right_total);
Renate 21:456acc79726c 148 filtered_EMG_biceps_left_total+=filtered_EMG_biceps_left;
Renate 21:456acc79726c 149 filtered_EMG_calf_total+=filtered_EMG_calf;
Renate 19:1fd39a2afc30 150 i_calib++;
Renate 19:1fd39a2afc30 151 }
Renate 22:8585d41a670b 152 if (i_calib > 2500 && calib)
Renate 19:1fd39a2afc30 153 {
Renate 21:456acc79726c 154 mean_EMG_biceps_right=filtered_EMG_biceps_right_total/2500.0;
Renate 21:456acc79726c 155 mean_EMG_biceps_left=filtered_EMG_biceps_left_total/2500.0;
Renate 21:456acc79726c 156 mean_EMG_calf=filtered_EMG_calf_total/2500.0;
Renate 21:456acc79726c 157 pc.printf("Ontspan spieren\r\n");
Renate 21:456acc79726c 158 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 21:456acc79726c 159 pc.printf("Rechterbiceps_max = %f\r\n", filtered_EMG_biceps_right_total);
Renate 19:1fd39a2afc30 160 calib = false;
Renate 19:1fd39a2afc30 161 }
Renate 19:1fd39a2afc30 162 }
Renate 19:1fd39a2afc30 163 }
Renate 19:1fd39a2afc30 164
Renate 21:456acc79726c 165 void sendHIDScope() // Deze functie geeft de gefilterde EMG-signalen weer in de HIDScope
Renate 21:456acc79726c 166 // Wordt eveneens gerund dmv een ticker
Renate 21:456acc79726c 167 {
Renate 21:456acc79726c 168 /* Set the sampled emg values in channel 0 (the first channel) and 1 (the second channel) in the 'HIDScope' instance named 'scope' */
Renate 21:456acc79726c 169 scope.set(0, filtered_EMG_biceps_right);
Renate 21:456acc79726c 170 scope.set(1, EMG_biceps_right_raw);
Renate 21:456acc79726c 171 scope.set(2, normalized_EMG_calf);
Renate 21:456acc79726c 172 /* Repeat the step above if required for more channels of required (channel 0 up to 5 = 6 channels)
Renate 21:456acc79726c 173 * Ensure that enough channels are available (HIDScope scope( 2 ))
Renate 21:456acc79726c 174 * Finally, send all channels to the PC at once */
Renate 21:456acc79726c 175 scope.send();
Renate 21:456acc79726c 176 // Eventueel nog een ledje laten branden
Renate 21:456acc79726c 177 }
Renate 21:456acc79726c 178
Renate 11:4bc0304978e2 179 // Emergency
Renate 8:c7d3b67346db 180 void emergency()
Rosalie 3:6ee0b20c23b0 181 {
Renate 11:4bc0304978e2 182 loop_ticker.detach();
Renate 8:c7d3b67346db 183 motor1.write(0);
Renate 8:c7d3b67346db 184 motor2.write(0);
Renate 11:4bc0304978e2 185 pc.printf("Ik ga exploderen!!!\r\n");
Renate 11:4bc0304978e2 186 // Alles moet uitgaan (evt. een rood LEDje laten branden), moet
Renate 14:54343b9fd708 187 // opnieuw worden opgestart. Mogelijk kan dit door de ticker te
Renate 11:4bc0304978e2 188 // detachen
Renate 8:c7d3b67346db 189 }
Renate 11:4bc0304978e2 190
Renate 11:4bc0304978e2 191 // Motoren uitzetten
Renate 8:c7d3b67346db 192 void motors_off()
Renate 8:c7d3b67346db 193 {
Renate 8:c7d3b67346db 194 motor1.write(0);
Renate 8:c7d3b67346db 195 motor2.write(0);
Renate 9:4de589636f50 196 pc.printf("Motoren uit functie\r\n");
Renate 8:c7d3b67346db 197 }
Renate 8:c7d3b67346db 198
Renate 14:54343b9fd708 199 // Motoren aanzetten
Renate 15:ad065ab92d11 200 void motors_on()
Renate 15:ad065ab92d11 201 {
Renate 15:ad065ab92d11 202 motor1.write(0.9);
Renate 15:ad065ab92d11 203 motor1_dir.write(1);
Renate 15:ad065ab92d11 204 motor2.write(0.1);
Renate 15:ad065ab92d11 205 motor1_dir.write(1);
Renate 15:ad065ab92d11 206 pc.printf("Motoren aan functie\r\n");
Renate 15:ad065ab92d11 207 }
Renate 19:1fd39a2afc30 208
Renate 19:1fd39a2afc30 209 // EMG kalibreren
Renate 19:1fd39a2afc30 210 void emg_calibration()
Renate 19:1fd39a2afc30 211 {
Renate 19:1fd39a2afc30 212 // Gedurende bijv. 5 seconden EMG meten, wanneer de spieren maximaal
Renate 19:1fd39a2afc30 213 // worden aangespannen -> maximaal potentiaal verkrijgen. Een fractie
Renate 19:1fd39a2afc30 214 // hiervan kan als drempel worden gebruikt voor beweging
Renate 19:1fd39a2afc30 215
Renate 19:1fd39a2afc30 216 // *Tijd instellen*
Renate 19:1fd39a2afc30 217 // Iets met DOUBLE_MAX? https://docs.microsoft.com/en-us/cpp/c-language/cpp-integer-limits?view=vs-2019
Renate 19:1fd39a2afc30 218
Renate 19:1fd39a2afc30 219 // Ledje van kleur laten veranderen
Renate 19:1fd39a2afc30 220
Renate 19:1fd39a2afc30 221 // MOGELIJK NIET MEER NODIG???
Renate 19:1fd39a2afc30 222
Renate 19:1fd39a2afc30 223 }
Rosalie 3:6ee0b20c23b0 224
Renate 6:64146e16e10c 225 // Finite state machine programming (calibration servo motor?)
Renate 12:93ad9781eeef 226 enum states {Motors_off, Calib_motor, Calib_EMG, Homing, Operation_mode};
Renate 6:64146e16e10c 227
Renate 6:64146e16e10c 228 states currentState = Motors_off;
Renate 6:64146e16e10c 229 bool stateChanged = true; // Make sure the initialization of first state is executed
Renate 6:64146e16e10c 230
Renate 6:64146e16e10c 231 void ProcessStateMachine(void)
Renate 9:4de589636f50 232 {
Renate 6:64146e16e10c 233 switch (currentState)
Renate 6:64146e16e10c 234 {
Renate 6:64146e16e10c 235 case Motors_off:
Renate 6:64146e16e10c 236
Renate 9:4de589636f50 237 if (stateChanged)
Renate 6:64146e16e10c 238 {
Renate 8:c7d3b67346db 239 motors_off(); // functie waarbij motoren uitgaan
Renate 11:4bc0304978e2 240 stateChanged = false;
Renate 9:4de589636f50 241 pc.printf("Motors off state\r\n");
Renate 9:4de589636f50 242 }
Renate 11:4bc0304978e2 243 if (Power_button_pressed.read() == false) // Normaal waarde 1 bij indrukken, nu nul -> false
Renate 6:64146e16e10c 244 {
Renate 15:ad065ab92d11 245 motors_on();
Renate 9:4de589636f50 246 currentState = Calib_motor;
Renate 11:4bc0304978e2 247 stateChanged = true;
Renate 11:4bc0304978e2 248 pc.printf("Moving to Calib_motor state\r\n");
Renate 6:64146e16e10c 249 }
Renate 11:4bc0304978e2 250 if (Emergency_button_pressed.read() == false) // Normaal waarde 1 bij indrukken, nu nul -> false
Renate 8:c7d3b67346db 251 {
Renate 10:83f3cec8dd1c 252 emergency();
Renate 9:4de589636f50 253 }
Renate 6:64146e16e10c 254 break;
Renate 6:64146e16e10c 255
Renate 9:4de589636f50 256 case Calib_motor:
Renate 9:4de589636f50 257
Renate 22:8585d41a670b 258 if (stateChanged && Motor_calib_button_pressed.read() == false)
Renate 9:4de589636f50 259 {
Renate 21:456acc79726c 260 theta_h_1_rad = 0;
Renate 21:456acc79726c 261 theta_h_2_rad = 0;
Renate 21:456acc79726c 262 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 263 currentState = Calib_EMG;
Renate 11:4bc0304978e2 264 stateChanged = true;
Renate 9:4de589636f50 265 pc.printf("Moving to Calib_EMG state\r\n");
Renate 9:4de589636f50 266 }
Renate 11:4bc0304978e2 267 if (Emergency_button_pressed.read() == false)
Renate 11:4bc0304978e2 268 {
Renate 11:4bc0304978e2 269 emergency();
Renate 11:4bc0304978e2 270 }
Renate 11:4bc0304978e2 271 break;
Renate 11:4bc0304978e2 272
Renate 15:ad065ab92d11 273 case Calib_EMG:
Renate 11:4bc0304978e2 274
Renate 11:4bc0304978e2 275 if (stateChanged)
Renate 19:1fd39a2afc30 276 {
Renate 21:456acc79726c 277 // Hierbij wordt een een kalibratie uitgevoerd, waarbij de maximale EMG-amplitude waarde wordt bepaald
Renate 21:456acc79726c 278
Renate 21:456acc79726c 279 motors_off();
Renate 22:8585d41a670b 280 i_calib = 0;
Renate 21:456acc79726c 281 // 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 21:456acc79726c 282 calib = true;
Renate 21:456acc79726c 283 pc.printf("Span spieren aan\r\n");
Renate 21:456acc79726c 284 // emgSampleFilter(); // Gaat dit nu goed? -> moet sws worden toegevoegd bij relevante onderdelen?
Renate 21:456acc79726c 285 stateChanged = false;
Renate 21:456acc79726c 286 }
Renate 21:456acc79726c 287
Renate 22:8585d41a670b 288 if (i_calib > 2500) // of wait(10);?
Renate 21:456acc79726c 289 {
Renate 21:456acc79726c 290 calib = false;
Renate 19:1fd39a2afc30 291 currentState = Homing;
Renate 19:1fd39a2afc30 292 stateChanged = true;
Renate 21:456acc79726c 293 normalized_EMG_biceps_right=filtered_EMG_biceps_right/mean_EMG_biceps_right;
Renate 21:456acc79726c 294 normalized_EMG_biceps_left=filtered_EMG_biceps_left/mean_EMG_biceps_left;
Renate 21:456acc79726c 295 normalized_EMG_calf=filtered_EMG_calf/mean_EMG_calf;
Renate 21:456acc79726c 296 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 19:1fd39a2afc30 297 pc.printf("Moving to Homing state\r\n");
Renate 19:1fd39a2afc30 298 }
Renate 21:456acc79726c 299
Renate 10:83f3cec8dd1c 300 if (Emergency_button_pressed.read() == false)
Renate 10:83f3cec8dd1c 301 {
Renate 11:4bc0304978e2 302 emergency();
Renate 11:4bc0304978e2 303 }
Renate 11:4bc0304978e2 304 break;
Renate 11:4bc0304978e2 305
Renate 11:4bc0304978e2 306 case Homing:
Renate 11:4bc0304978e2 307
Renate 11:4bc0304978e2 308 if (stateChanged)
Renate 11:4bc0304978e2 309 {
Renate 11:4bc0304978e2 310 // Ervoor zorgen dat de motoren zo bewegen dat de robotarm
Renate 11:4bc0304978e2 311 // (inclusief de end-effector) in de juiste home positie wordt gezet
Renate 21:456acc79726c 312 motors_on();
Renate 11:4bc0304978e2 313 currentState = Operation_mode;
Renate 11:4bc0304978e2 314 stateChanged = true;
Renate 12:93ad9781eeef 315 pc.printf("Moving to operation mode \r\n");
Renate 11:4bc0304978e2 316 }
Renate 11:4bc0304978e2 317 if (Emergency_button_pressed.read() == false)
Renate 11:4bc0304978e2 318 {
Renate 10:83f3cec8dd1c 319 emergency();
Renate 10:83f3cec8dd1c 320 }
Renate 11:4bc0304978e2 321 break;
Renate 11:4bc0304978e2 322
Renate 14:54343b9fd708 323 case Operation_mode: // Overgaan tot emergency wanneer referentie niet
Renate 14:54343b9fd708 324 // overeenkomt met werkelijkheid
Renate 21:456acc79726c 325
Renate 11:4bc0304978e2 326 if (stateChanged)
Renate 12:93ad9781eeef 327
Renate 11:4bc0304978e2 328 // Hier moet een functie worden aangeroepen die ervoor zorgt dat
Renate 11:4bc0304978e2 329 // aan de hand van EMG-signalen de motoren kunnen worden aangestuurd,
Renate 11:4bc0304978e2 330 // zodat de robotarm kan bewegen
Renate 11:4bc0304978e2 331
Renate 21:456acc79726c 332 {
Renate 20:a6a5bdd7d118 333 if (normalized_EMG_biceps_right >= 0.3)
Renate 20:a6a5bdd7d118 334 {
Renate 20:a6a5bdd7d118 335 motor1.write(0.5);
Renate 21:456acc79726c 336 motor1_dir.write(1);
Renate 20:a6a5bdd7d118 337 motor2.write(0);
Renate 21:456acc79726c 338 motor2_dir.write(1);
Renate 21:456acc79726c 339 if (normalized_EMG_calf >= 0.3)
Renate 21:456acc79726c 340 {
Renate 21:456acc79726c 341 // motor1_dir = !motor1_dir;
Renate 21:456acc79726c 342 // motor2_dir = !motor2_dir;
Renate 21:456acc79726c 343 }
Renate 20:a6a5bdd7d118 344 }
Renate 20:a6a5bdd7d118 345 if (normalized_EMG_biceps_right < 0.3)
Renate 20:a6a5bdd7d118 346 {
Renate 20:a6a5bdd7d118 347 motor1.write(0);
Renate 20:a6a5bdd7d118 348 motor2.write(0);
Renate 20:a6a5bdd7d118 349 }
Renate 20:a6a5bdd7d118 350 if (normalized_EMG_biceps_left >= 0.3)
Renate 20:a6a5bdd7d118 351 {
Renate 20:a6a5bdd7d118 352 motor2.write(0.9);
Renate 21:456acc79726c 353 motor2_dir.write(1);
Renate 20:a6a5bdd7d118 354 motor1.write(0);
Renate 21:456acc79726c 355 motor1_dir.write(1);
Renate 21:456acc79726c 356 if (normalized_EMG_calf >= 0.3)
Renate 21:456acc79726c 357 {
Renate 21:456acc79726c 358 // motor1_dir = !motor1_dir;
Renate 21:456acc79726c 359 // motor2_dir = !motor2_dir;
Renate 21:456acc79726c 360 }
Renate 20:a6a5bdd7d118 361 }
Renate 20:a6a5bdd7d118 362 if (normalized_EMG_biceps_left < 0.3)
Renate 20:a6a5bdd7d118 363 {
Renate 20:a6a5bdd7d118 364 motor2.write(0);
Renate 20:a6a5bdd7d118 365 motor1.write(0);
Renate 20:a6a5bdd7d118 366 }
Renate 21:456acc79726c 367 }
Renate 21:456acc79726c 368 if (Power_button_pressed.read() == false) // Normaal waarde 1 bij indrukken, nu nul -> false
Renate 21:456acc79726c 369 {
Renate 21:456acc79726c 370 motors_off();
Renate 21:456acc79726c 371 currentState = Motors_off;
Renate 21:456acc79726c 372 stateChanged = true;
Renate 21:456acc79726c 373 pc.printf("Terug naar de state Motors_off\r\n");
Renate 21:456acc79726c 374 }
Renate 21:456acc79726c 375 if (Emergency_button_pressed.read() == false)
Renate 21:456acc79726c 376 {
Renate 21:456acc79726c 377 emergency();
Renate 21:456acc79726c 378 }
Renate 21:456acc79726c 379 // wait(25);
Renate 21:456acc79726c 380 // else
Renate 21:456acc79726c 381 // {
Renate 21:456acc79726c 382 // currentState = Homing;
Renate 21:456acc79726c 383 // stateChanged = true;
Renate 21:456acc79726c 384 // pc.printf("Terug naar de state Homing\r\n");
Renate 21:456acc79726c 385 // }
Renate 21:456acc79726c 386 break;
Renate 20:a6a5bdd7d118 387
Renate 7:1d57463393c6 388 default:
Renate 7:1d57463393c6 389 // Zelfde functie als die eerder is toegepast om motoren uit te schakelen -> safety!
Renate 14:54343b9fd708 390 motors_off();
Renate 9:4de589636f50 391 pc.printf("Unknown or uninplemented state reached!\r\n");
Renate 8:c7d3b67346db 392
WiesjeRoskamp 2:aee655d11b6d 393 }
Renate 11:4bc0304978e2 394 }
WiesjeRoskamp 2:aee655d11b6d 395
Renate 8:c7d3b67346db 396 int main(void)
Renate 8:c7d3b67346db 397 {
Renate 9:4de589636f50 398 pc.printf("Opstarten\r\n");
Renate 21:456acc79726c 399
Renate 21:456acc79726c 400 // Chain voor rechter biceps
Renate 21:456acc79726c 401 bqcbr.add(&bqbr1).add(&bqbr2);
Renate 21:456acc79726c 402 bqcbr2.add(&bqbr3).add(&bqbr4);
Renate 21:456acc79726c 403 // Chain voor linker biceps
Renate 21:456acc79726c 404 bqcbl.add(&bqbl1).add(&bqbl2);
Renate 21:456acc79726c 405 bqcbl2.add(&bqbl3).add(&bqbl4);
Renate 21:456acc79726c 406 // Chain voor kuit
Renate 21:456acc79726c 407 bqck.add(&bqk1).add(&bqk2);
Renate 21:456acc79726c 408 bqck2.add(&bqk3).add(&bqk4);
Renate 21:456acc79726c 409
Renate 21:456acc79726c 410 emgSampleTicker.attach(&emgSampleFilter, 0.002f);
Renate 21:456acc79726c 411 HIDScope_ticker.attach(&sendHIDScope, 0.002f);
Renate 21:456acc79726c 412 motorTicker.attach(&Calculate_motor_angle, 0.002f);
Renate 12:93ad9781eeef 413 loop_ticker.attach(&ProcessStateMachine, 5.0f);
Renate 21:456acc79726c 414
Renate 8:c7d3b67346db 415 while(true)
Renate 21:456acc79726c 416 {
Renate 21:456acc79726c 417 // wait(0.2);
Renate 21:456acc79726c 418 /* do nothing */
Renate 21:456acc79726c 419 }
Renate 8:c7d3b67346db 420 }