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@21:456acc79726c, 2019-10-28 (annotated)

Committer:
Renate
Date:
Mon Oct 28 14:29:13 2019 +0000
Revision:
21:456acc79726c
Parent:
20:a6a5bdd7d118
Child:
22:8585d41a670b
Script 28-10 voor grote schoonmaak

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