Renate de Boer / Mbed 2 deprecated script_voor_project_copy

Script 15-10-2019

Dependencies:   Servoaansturing mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

main.cpp.orig@43:2f946b617d62, 2019-11-05 (annotated)

Committer:
Renate
Date:
Tue Nov 05 20:21:27 2019 +0000
Revision:
43:2f946b617d62
Parent:
19:1fd39a2afc30 
Klaar om in te leveren

Who changed what in which revision?

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