Renate de Boer / Mbed 2 deprecated script_voor_project_copy

Script 15-10-2019

Dependencies:   Servoaansturing mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

main.cpp@17:e371bced66da, 2019-10-18 (annotated)

Committer:
Renate
Date:
Fri Oct 18 13:30:09 2019 +0000
Revision:
17:e371bced66da
Parent:
16:733a71a177e8
Child:
18:315a7670d0ca
Uitbreiding aan kalibratie e.d. voor testen robot

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 17:e371bced66da 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 15:ad065ab92d11 24 Analogin EMG_calf_raw (A2);
Renate 15:ad065ab92d11 25
Renate 8:c7d3b67346db 26 Ticker loop_ticker;
Renate 17:e371bced66da 27 Ticker HIDScope_ticker;
Renate 17:e371bced66da 28 Ticker emgSampleTicker;
Renate 17:e371bced66da 29
Renate 17:e371bced66da 30 HIDScope scope(3);
Renate 17:e371bced66da 31
Renate 17:e371bced66da 32 BiQuadChain bqc; // Let op !!! Deze coëfficiënten moeten nog worden veranderd
Renate 17:e371bced66da 33 BiQuad bq1(0.0030, 0.0059, 0.0030, -1.8404,0.8522); //voor low-pass
Renate 17:e371bced66da 34 BiQuad bq2(0.9737, -1.9474, 0.9737, -1.9467, 0.9481); //voor high-pass
Renate 17:e371bced66da 35 BiQuad bq3(0.9912, -1.9823,0.9912, -1.9822, 0.9824); //lage piek eruit-> voor coëfficienten, zie matlab
Renate 17:e371bced66da 36
Renate 17:e371bced66da 37 void emgSampleFilter() // Deze functie wordt aangeroepen dmv een ticker. Het sampled
Renate 17:e371bced66da 38 // hierdoor het EMG signaal en het haalt er een filter overheen
Renate 17:e371bced66da 39 {
Renate 17:e371bced66da 40 double filtered_EMG_biceps_right=bqc.step(EMG_biceps_right_raw.read());
Renate 17:e371bced66da 41 double filtered_EMG_biceps_left=bqc.step(EMG_biceps_left_raw.read());
Renate 17:e371bced66da 42 double filtered_EMG_calf=bqc.step(EMG_calf_raw.read());
Renate 17:e371bced66da 43 }
Renate 17:e371bced66da 44
Renate 17:e371bced66da 45 void sendHIDScope() // Deze functie geeft de gefilterde EMG-signalen weer in de HIDScope
Renate 17:e371bced66da 46 // Wordt eveneens gerund dmv een ticker
Renate 17:e371bced66da 47 {
Renate 17:e371bced66da 48 /* Set the sampled emg values in channel 0 (the first channel) and 1 (the second channel) in the 'HIDScope' instance named 'scope' */
Renate 17:e371bced66da 49 scope.set(0, filtered_EMG_biceps_right() ); // Werkt dit zo? Of nog met .read?
Renate 17:e371bced66da 50 scope.set(1, filtered_EMG_biceps_left() );
Renate 17:e371bced66da 51 scope.set(2, filtered_EMG_calf() );
Renate 17:e371bced66da 52 /* Repeat the step above if required for more channels of required (channel 0 up to 5 = 6 channels)
Renate 17:e371bced66da 53 * Ensure that enough channels are available (HIDScope scope( 2 ))
Renate 17:e371bced66da 54 * Finally, send all channels to the PC at once */
Renate 17:e371bced66da 55 scope.send();
Renate 17:e371bced66da 56 // Eventueel nog een ledje laten branden
Renate 17:e371bced66da 57 }
Renate 11:4bc0304978e2 58
Renate 11:4bc0304978e2 59 // Emergency
Renate 8:c7d3b67346db 60 void emergency()
Rosalie 3:6ee0b20c23b0 61 {
Renate 11:4bc0304978e2 62 loop_ticker.detach();
Renate 8:c7d3b67346db 63 motor1.write(0);
Renate 8:c7d3b67346db 64 motor2.write(0);
Renate 11:4bc0304978e2 65 pc.printf("Ik ga exploderen!!!\r\n");
Renate 11:4bc0304978e2 66 // Alles moet uitgaan (evt. een rood LEDje laten branden), moet
Renate 14:54343b9fd708 67 // opnieuw worden opgestart. Mogelijk kan dit door de ticker te
Renate 11:4bc0304978e2 68 // detachen
Renate 8:c7d3b67346db 69 }
Renate 11:4bc0304978e2 70
Renate 11:4bc0304978e2 71 // Motoren uitzetten
Renate 8:c7d3b67346db 72 void motors_off()
Renate 8:c7d3b67346db 73 {
Renate 8:c7d3b67346db 74 motor1.write(0);
Renate 8:c7d3b67346db 75 motor2.write(0);
Renate 9:4de589636f50 76 pc.printf("Motoren uit functie\r\n");
Renate 8:c7d3b67346db 77 }
Renate 8:c7d3b67346db 78
Renate 14:54343b9fd708 79 // Motoren aanzetten
Renate 15:ad065ab92d11 80 void motors_on()
Renate 15:ad065ab92d11 81 {
Renate 15:ad065ab92d11 82 motor1.write(0.9);
Renate 15:ad065ab92d11 83 motor1_dir.write(1);
Renate 15:ad065ab92d11 84 motor2.write(0.1);
Renate 15:ad065ab92d11 85 motor1_dir.write(1);
Renate 15:ad065ab92d11 86 pc.printf("Motoren aan functie\r\n");
Renate 15:ad065ab92d11 87 }
Renate 16:733a71a177e8 88
Renate 16:733a71a177e8 89 // EMG kalibreren
Renate 16:733a71a177e8 90 void emg_calibration()
Renate 16:733a71a177e8 91 {
Renate 17:e371bced66da 92 // Gedurende bijv. 5 seconden EMG meten, wanneer de spieren maximaal
Renate 17:e371bced66da 93 // worden aangespannen -> maximaal potentiaal verkrijgen. Een fractie
Renate 17:e371bced66da 94 // hiervan kan als drempel worden gebruikt voor beweging
Renate 17:e371bced66da 95
Renate 17:e371bced66da 96 // *Tijd instellen*
Renate 17:e371bced66da 97
Renate 17:e371bced66da 98 filtered_EMG_biceps_right_max
Renate 17:e371bced66da 99 filtered_EMG_biceps_left_max
Renate 17:e371bced66da 100 filtered_EMG_calf_max
Renate 17:e371bced66da 101
Renate 16:733a71a177e8 102 }
Renate 16:733a71a177e8 103
Renate 6:64146e16e10c 104 // Finite state machine programming (calibration servo motor?)
Renate 12:93ad9781eeef 105 enum states {Motors_off, Calib_motor, Calib_EMG, Homing, Operation_mode};
Renate 6:64146e16e10c 106
Renate 6:64146e16e10c 107 states currentState = Motors_off;
Renate 6:64146e16e10c 108 bool stateChanged = true; // Make sure the initialization of first state is executed
Renate 6:64146e16e10c 109
Renate 6:64146e16e10c 110 void ProcessStateMachine(void)
Renate 9:4de589636f50 111 {
Renate 6:64146e16e10c 112 switch (currentState)
Renate 6:64146e16e10c 113 {
Renate 6:64146e16e10c 114 case Motors_off:
Renate 6:64146e16e10c 115
Renate 9:4de589636f50 116 if (stateChanged)
Renate 6:64146e16e10c 117 {
Renate 8:c7d3b67346db 118 motors_off(); // functie waarbij motoren uitgaan
Renate 11:4bc0304978e2 119 stateChanged = false;
Renate 9:4de589636f50 120 pc.printf("Motors off state\r\n");
Renate 9:4de589636f50 121 }
Renate 11:4bc0304978e2 122 if (Power_button_pressed.read() == false) // Normaal waarde 1 bij indrukken, nu nul -> false
Renate 6:64146e16e10c 123 {
Renate 15:ad065ab92d11 124 motors_on();
Renate 9:4de589636f50 125 currentState = Calib_motor;
Renate 11:4bc0304978e2 126 stateChanged = true;
Renate 11:4bc0304978e2 127 pc.printf("Moving to Calib_motor state\r\n");
Renate 6:64146e16e10c 128 }
Renate 11:4bc0304978e2 129 if (Emergency_button_pressed.read() == false) // Normaal waarde 1 bij indrukken, nu nul -> false
Renate 8:c7d3b67346db 130 {
Renate 10:83f3cec8dd1c 131 emergency();
Renate 9:4de589636f50 132 }
Renate 6:64146e16e10c 133 break;
Renate 6:64146e16e10c 134
Renate 9:4de589636f50 135 case Calib_motor:
Renate 9:4de589636f50 136
Renate 9:4de589636f50 137 if (stateChanged)
Renate 9:4de589636f50 138 {
Renate 9:4de589636f50 139 // Hier wordt een kalibratie uitgevoerd, waarbij de motorhoeken worden bepaald
Renate 11:4bc0304978e2 140 currentState = Calib_EMG;
Renate 11:4bc0304978e2 141 stateChanged = true;
Renate 9:4de589636f50 142 pc.printf("Moving to Calib_EMG state\r\n");
Renate 9:4de589636f50 143 }
Renate 11:4bc0304978e2 144 if (Emergency_button_pressed.read() == false)
Renate 11:4bc0304978e2 145 {
Renate 11:4bc0304978e2 146 emergency();
Renate 11:4bc0304978e2 147 }
Renate 11:4bc0304978e2 148 break;
Renate 11:4bc0304978e2 149
Renate 15:ad065ab92d11 150 case Calib_EMG:
Renate 11:4bc0304978e2 151
Renate 14:54343b9fd708 152 motors_off();
Renate 11:4bc0304978e2 153 if (stateChanged)
Renate 11:4bc0304978e2 154 {
Renate 11:4bc0304978e2 155 // Hierbij wordt een een kalibratie uitgevoerd, waarbij de maximale EMG-amplitude waarde wordt bepaald
Renate 16:733a71a177e8 156 // -> functies aanroepen (emg_load(), emg_discrete_filter(), emg_calibration())
Renate 11:4bc0304978e2 157 currentState = Homing;
Renate 11:4bc0304978e2 158 stateChanged = true;
Renate 11:4bc0304978e2 159 pc.printf("Moving to Homing state\r\n");
Renate 11:4bc0304978e2 160 }
Renate 10:83f3cec8dd1c 161 if (Emergency_button_pressed.read() == false)
Renate 10:83f3cec8dd1c 162 {
Renate 11:4bc0304978e2 163 emergency();
Renate 11:4bc0304978e2 164 }
Renate 11:4bc0304978e2 165 break;
Renate 11:4bc0304978e2 166
Renate 11:4bc0304978e2 167 case Homing:
Renate 11:4bc0304978e2 168
Renate 15:ad065ab92d11 169 motors_on();
Renate 11:4bc0304978e2 170 if (stateChanged)
Renate 11:4bc0304978e2 171 {
Renate 11:4bc0304978e2 172 // Ervoor zorgen dat de motoren zo bewegen dat de robotarm
Renate 11:4bc0304978e2 173 // (inclusief de end-effector) in de juiste home positie wordt gezet
Renate 11:4bc0304978e2 174 currentState = Operation_mode;
Renate 11:4bc0304978e2 175 stateChanged = true;
Renate 12:93ad9781eeef 176 pc.printf("Moving to operation mode \r\n");
Renate 11:4bc0304978e2 177 }
Renate 11:4bc0304978e2 178 if (Emergency_button_pressed.read() == false)
Renate 11:4bc0304978e2 179 {
Renate 10:83f3cec8dd1c 180 emergency();
Renate 10:83f3cec8dd1c 181 }
Renate 11:4bc0304978e2 182 break;
Renate 11:4bc0304978e2 183
Renate 14:54343b9fd708 184 case Operation_mode: // Overgaan tot emergency wanneer referentie niet
Renate 14:54343b9fd708 185 // overeenkomt met werkelijkheid
Renate 11:4bc0304978e2 186
Renate 11:4bc0304978e2 187 if (stateChanged)
Renate 12:93ad9781eeef 188
Renate 11:4bc0304978e2 189 // Hier moet een functie worden aangeroepen die ervoor zorgt dat
Renate 11:4bc0304978e2 190 // aan de hand van EMG-signalen de motoren kunnen worden aangestuurd,
Renate 11:4bc0304978e2 191 // zodat de robotarm kan bewegen
Renate 11:4bc0304978e2 192
Renate 11:4bc0304978e2 193 if (Power_button_pressed.read() == false) // Normaal waarde 1 bij indrukken, nu nul -> false
Renate 11:4bc0304978e2 194 {
Renate 15:ad065ab92d11 195 motors_off();
Renate 11:4bc0304978e2 196 currentState = Motors_off;
Renate 11:4bc0304978e2 197 stateChanged = true;
Renate 12:93ad9781eeef 198 pc.printf("Terug naar de state Motors_off\r\n");
Renate 11:4bc0304978e2 199 }
Renate 11:4bc0304978e2 200 if (Emergency_button_pressed.read() == false)
Renate 11:4bc0304978e2 201 {
Renate 11:4bc0304978e2 202 emergency();
Renate 11:4bc0304978e2 203 }
Renate 12:93ad9781eeef 204 // wait(5);
Renate 11:4bc0304978e2 205 else
Renate 11:4bc0304978e2 206 {
Renate 11:4bc0304978e2 207 currentState = Homing;
Renate 11:4bc0304978e2 208 stateChanged = true;
Renate 12:93ad9781eeef 209 pc.printf("Terug naar de state Homing\r\n");
Renate 11:4bc0304978e2 210 }
Renate 11:4bc0304978e2 211 break;
Renate 9:4de589636f50 212
Renate 7:1d57463393c6 213 default:
Renate 7:1d57463393c6 214 // Zelfde functie als die eerder is toegepast om motoren uit te schakelen -> safety!
Renate 14:54343b9fd708 215 motors_off();
Renate 9:4de589636f50 216 pc.printf("Unknown or uninplemented state reached!\r\n");
Renate 8:c7d3b67346db 217
WiesjeRoskamp 2:aee655d11b6d 218 }
Renate 11:4bc0304978e2 219 }
WiesjeRoskamp 2:aee655d11b6d 220
Renate 8:c7d3b67346db 221 int main(void)
Renate 8:c7d3b67346db 222 {
Renate 9:4de589636f50 223 pc.printf("Opstarten\r\n");
Renate 17:e371bced66da 224 bqc.add(&bq1).add(&bq2).add(&bq3);
Renate 17:e371bced66da 225 emgSampleTicker.attach(&emgSampleFilter, 0.01f);
Renate 17:e371bced66da 226 HIDScope_ticker.attach(&sendHIDScope, 0.01f);
Renate 12:93ad9781eeef 227 loop_ticker.attach(&ProcessStateMachine, 5.0f);
Renate 8:c7d3b67346db 228 while(true)
Renate 11:4bc0304978e2 229 { /* do nothing */}
Renate 8:c7d3b67346db 230 }