Renate de Boer / Mbed 2 deprecated script_voor_project_copy

Script 15-10-2019

Dependencies:   Servoaansturing mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

main.cpp@16:733a71a177e8, 2019-10-15 (annotated)

Committer:
Renate
Date:
Tue Oct 15 13:52:25 2019 +0000
Revision:
16:733a71a177e8
Parent:
15:ad065ab92d11
Child:
17:e371bced66da
Script 15-10-2019

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 15:ad065ab92d11 24 Analogin EMG_calf_raw (A2);
Renate 15:ad065ab92d11 25
Renate 8:c7d3b67346db 26 Ticker loop_ticker;
Renate 16:733a71a177e8 27 Ticker sample_timer;
Renate 11:4bc0304978e2 28
Renate 11:4bc0304978e2 29 // Emergency
Renate 8:c7d3b67346db 30 void emergency()
Rosalie 3:6ee0b20c23b0 31 {
Renate 11:4bc0304978e2 32 loop_ticker.detach();
Renate 8:c7d3b67346db 33 motor1.write(0);
Renate 8:c7d3b67346db 34 motor2.write(0);
Renate 11:4bc0304978e2 35 pc.printf("Ik ga exploderen!!!\r\n");
Renate 11:4bc0304978e2 36 // Alles moet uitgaan (evt. een rood LEDje laten branden), moet
Renate 14:54343b9fd708 37 // opnieuw worden opgestart. Mogelijk kan dit door de ticker te
Renate 11:4bc0304978e2 38 // detachen
Renate 8:c7d3b67346db 39 }
Renate 11:4bc0304978e2 40
Renate 11:4bc0304978e2 41 // Motoren uitzetten
Renate 8:c7d3b67346db 42 void motors_off()
Renate 8:c7d3b67346db 43 {
Renate 8:c7d3b67346db 44 motor1.write(0);
Renate 8:c7d3b67346db 45 motor2.write(0);
Renate 9:4de589636f50 46 pc.printf("Motoren uit functie\r\n");
Renate 8:c7d3b67346db 47 }
Renate 8:c7d3b67346db 48
Renate 14:54343b9fd708 49 // Motoren aanzetten
Renate 15:ad065ab92d11 50 void motors_on()
Renate 15:ad065ab92d11 51 {
Renate 15:ad065ab92d11 52 motor1.write(0.9);
Renate 15:ad065ab92d11 53 motor1_dir.write(1);
Renate 15:ad065ab92d11 54 motor2.write(0.1);
Renate 15:ad065ab92d11 55 motor1_dir.write(1);
Renate 15:ad065ab92d11 56 pc.printf("Motoren aan functie\r\n");
Renate 15:ad065ab92d11 57 }
Rosalie 3:6ee0b20c23b0 58
Renate 16:733a71a177e8 59 // EMG inladen
Renate 16:733a71a177e8 60 void emg_load()
Renate 16:733a71a177e8 61 {
Renate 16:733a71a177e8 62 EMG_biceps_right_raw.read();
Renate 16:733a71a177e8 63 EMG_biceps_left_raw.read();
Renate 16:733a71a177e8 64 EMG_calf_raw.read();
Renate 16:733a71a177e8 65 pc.printf("EMG data inladen\r\n");
Renate 16:733a71a177e8 66 }
Renate 16:733a71a177e8 67
Renate 16:733a71a177e8 68
Renate 16:733a71a177e8 69 void emg_discrete_filter()
Renate 16:733a71a177e8 70 {
Renate 16:733a71a177e8 71 // De ticker functie sample_timer hier toepassen voor het discretiseren?
Renate 16:733a71a177e8 72 // -> werkt een ticker in dit geval binnnen een void?
Renate 16:733a71a177e8 73 // Gemaakte filter toepassen op ruwe EMG data
Renate 16:733a71a177e8 74 }
Renate 16:733a71a177e8 75
Renate 16:733a71a177e8 76 // EMG kalibreren
Renate 16:733a71a177e8 77 void emg_calibration()
Renate 16:733a71a177e8 78 {
Renate 16:733a71a177e8 79 // Spieren maximaal aanspannen om het maximale potentiaal te
Renate 16:733a71a177e8 80 // verkrijgen. Een fractie hiervan kan als drempel worden gebruikt voor
Renate 16:733a71a177e8 81 // beweging
Renate 16:733a71a177e8 82 }
Renate 16:733a71a177e8 83
Renate 6:64146e16e10c 84 // Finite state machine programming (calibration servo motor?)
Renate 12:93ad9781eeef 85 enum states {Motors_off, Calib_motor, Calib_EMG, Homing, Operation_mode};
Renate 6:64146e16e10c 86
Renate 6:64146e16e10c 87 states currentState = Motors_off;
Renate 6:64146e16e10c 88 bool stateChanged = true; // Make sure the initialization of first state is executed
Renate 6:64146e16e10c 89
Renate 6:64146e16e10c 90 void ProcessStateMachine(void)
Renate 9:4de589636f50 91 {
Renate 6:64146e16e10c 92 switch (currentState)
Renate 6:64146e16e10c 93 {
Renate 6:64146e16e10c 94 case Motors_off:
Renate 6:64146e16e10c 95
Renate 9:4de589636f50 96 if (stateChanged)
Renate 6:64146e16e10c 97 {
Renate 8:c7d3b67346db 98 motors_off(); // functie waarbij motoren uitgaan
Renate 11:4bc0304978e2 99 stateChanged = false;
Renate 9:4de589636f50 100 pc.printf("Motors off state\r\n");
Renate 9:4de589636f50 101 }
Renate 11:4bc0304978e2 102 if (Power_button_pressed.read() == false) // Normaal waarde 1 bij indrukken, nu nul -> false
Renate 6:64146e16e10c 103 {
Renate 15:ad065ab92d11 104 motors_on();
Renate 9:4de589636f50 105 currentState = Calib_motor;
Renate 11:4bc0304978e2 106 stateChanged = true;
Renate 11:4bc0304978e2 107 pc.printf("Moving to Calib_motor state\r\n");
Renate 6:64146e16e10c 108 }
Renate 11:4bc0304978e2 109 if (Emergency_button_pressed.read() == false) // Normaal waarde 1 bij indrukken, nu nul -> false
Renate 8:c7d3b67346db 110 {
Renate 10:83f3cec8dd1c 111 emergency();
Renate 9:4de589636f50 112 }
Renate 6:64146e16e10c 113 break;
Renate 6:64146e16e10c 114
Renate 9:4de589636f50 115 case Calib_motor:
Renate 9:4de589636f50 116
Renate 9:4de589636f50 117 if (stateChanged)
Renate 9:4de589636f50 118 {
Renate 9:4de589636f50 119 // Hier wordt een kalibratie uitgevoerd, waarbij de motorhoeken worden bepaald
Renate 11:4bc0304978e2 120 currentState = Calib_EMG;
Renate 11:4bc0304978e2 121 stateChanged = true;
Renate 9:4de589636f50 122 pc.printf("Moving to Calib_EMG state\r\n");
Renate 9:4de589636f50 123 }
Renate 11:4bc0304978e2 124 if (Emergency_button_pressed.read() == false)
Renate 11:4bc0304978e2 125 {
Renate 11:4bc0304978e2 126 emergency();
Renate 11:4bc0304978e2 127 }
Renate 11:4bc0304978e2 128 break;
Renate 11:4bc0304978e2 129
Renate 15:ad065ab92d11 130 case Calib_EMG:
Renate 11:4bc0304978e2 131
Renate 14:54343b9fd708 132 motors_off();
Renate 11:4bc0304978e2 133 if (stateChanged)
Renate 11:4bc0304978e2 134 {
Renate 11:4bc0304978e2 135 // Hierbij wordt een een kalibratie uitgevoerd, waarbij de maximale EMG-amplitude waarde wordt bepaald
Renate 16:733a71a177e8 136 // -> functies aanroepen (emg_load(), emg_discrete_filter(), emg_calibration())
Renate 11:4bc0304978e2 137 currentState = Homing;
Renate 11:4bc0304978e2 138 stateChanged = true;
Renate 11:4bc0304978e2 139 pc.printf("Moving to Homing state\r\n");
Renate 11:4bc0304978e2 140 }
Renate 10:83f3cec8dd1c 141 if (Emergency_button_pressed.read() == false)
Renate 10:83f3cec8dd1c 142 {
Renate 11:4bc0304978e2 143 emergency();
Renate 11:4bc0304978e2 144 }
Renate 11:4bc0304978e2 145 break;
Renate 11:4bc0304978e2 146
Renate 11:4bc0304978e2 147 case Homing:
Renate 11:4bc0304978e2 148
Renate 15:ad065ab92d11 149 motors_on();
Renate 11:4bc0304978e2 150 if (stateChanged)
Renate 11:4bc0304978e2 151 {
Renate 11:4bc0304978e2 152 // Ervoor zorgen dat de motoren zo bewegen dat de robotarm
Renate 11:4bc0304978e2 153 // (inclusief de end-effector) in de juiste home positie wordt gezet
Renate 11:4bc0304978e2 154 currentState = Operation_mode;
Renate 11:4bc0304978e2 155 stateChanged = true;
Renate 12:93ad9781eeef 156 pc.printf("Moving to operation mode \r\n");
Renate 11:4bc0304978e2 157 }
Renate 11:4bc0304978e2 158 if (Emergency_button_pressed.read() == false)
Renate 11:4bc0304978e2 159 {
Renate 10:83f3cec8dd1c 160 emergency();
Renate 10:83f3cec8dd1c 161 }
Renate 11:4bc0304978e2 162 break;
Renate 11:4bc0304978e2 163
Renate 14:54343b9fd708 164 case Operation_mode: // Overgaan tot emergency wanneer referentie niet
Renate 14:54343b9fd708 165 // overeenkomt met werkelijkheid
Renate 11:4bc0304978e2 166
Renate 11:4bc0304978e2 167 if (stateChanged)
Renate 12:93ad9781eeef 168
Renate 11:4bc0304978e2 169 // Hier moet een functie worden aangeroepen die ervoor zorgt dat
Renate 11:4bc0304978e2 170 // aan de hand van EMG-signalen de motoren kunnen worden aangestuurd,
Renate 11:4bc0304978e2 171 // zodat de robotarm kan bewegen
Renate 11:4bc0304978e2 172
Renate 11:4bc0304978e2 173 if (Power_button_pressed.read() == false) // Normaal waarde 1 bij indrukken, nu nul -> false
Renate 11:4bc0304978e2 174 {
Renate 15:ad065ab92d11 175 motors_off();
Renate 11:4bc0304978e2 176 currentState = Motors_off;
Renate 11:4bc0304978e2 177 stateChanged = true;
Renate 12:93ad9781eeef 178 pc.printf("Terug naar de state Motors_off\r\n");
Renate 11:4bc0304978e2 179 }
Renate 11:4bc0304978e2 180 if (Emergency_button_pressed.read() == false)
Renate 11:4bc0304978e2 181 {
Renate 11:4bc0304978e2 182 emergency();
Renate 11:4bc0304978e2 183 }
Renate 12:93ad9781eeef 184 // wait(5);
Renate 11:4bc0304978e2 185 else
Renate 11:4bc0304978e2 186 {
Renate 11:4bc0304978e2 187 currentState = Homing;
Renate 11:4bc0304978e2 188 stateChanged = true;
Renate 12:93ad9781eeef 189 pc.printf("Terug naar de state Homing\r\n");
Renate 11:4bc0304978e2 190 }
Renate 11:4bc0304978e2 191 break;
Renate 9:4de589636f50 192
Renate 7:1d57463393c6 193 default:
Renate 7:1d57463393c6 194 // Zelfde functie als die eerder is toegepast om motoren uit te schakelen -> safety!
Renate 14:54343b9fd708 195 motors_off();
Renate 9:4de589636f50 196 pc.printf("Unknown or uninplemented state reached!\r\n");
Renate 8:c7d3b67346db 197
WiesjeRoskamp 2:aee655d11b6d 198 }
Renate 11:4bc0304978e2 199 }
WiesjeRoskamp 2:aee655d11b6d 200
Renate 8:c7d3b67346db 201 int main(void)
Renate 8:c7d3b67346db 202 {
Renate 9:4de589636f50 203 pc.printf("Opstarten\r\n");
Renate 12:93ad9781eeef 204 loop_ticker.attach(&ProcessStateMachine, 5.0f);
Renate 8:c7d3b67346db 205 while(true)
Renate 11:4bc0304978e2 206 { /* do nothing */}
Renate 8:c7d3b67346db 207 }