Renate de Boer / Mbed 2 deprecated script_voor_project_copy

Script 15-10-2019

Dependencies:   Servoaansturing mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

main.cpp@19:1fd39a2afc30, 2019-10-24 (annotated)

Committer:
Renate
Date:
Thu Oct 24 09:25:27 2019 +0000
Revision:
19:1fd39a2afc30
Parent:
15:ad065ab92d11
Child:
20:a6a5bdd7d118
Versie na 21-10

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