Renate de Boer / Mbed 2 deprecated script_voor_project_copy

Script 15-10-2019

Dependencies:   Servoaansturing mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

main.cpp@15:ad065ab92d11, 2019-10-15 (annotated)

Committer:
Renate
Date:
Tue Oct 15 13:17:49 2019 +0000
Revision:
15:ad065ab92d11
Parent:
14:54343b9fd708
Child:
16:733a71a177e8
Child:
19:1fd39a2afc30
Goed werkende versie, voordat aan EMG kalibratie wordt begonnen

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