Renate de Boer / Mbed 2 deprecated script_voor_project_copy

Script 15-10-2019

Dependencies:   Servoaansturing mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

main.cpp@12:93ad9781eeef, 2019-10-14 (annotated)

Committer:
Renate
Date:
Mon Oct 14 14:44:59 2019 +0000
Revision:
12:93ad9781eeef
Parent:
11:4bc0304978e2
Child:
13:cb40c3037f9c
v

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
Rosalie 4:6742476fadcf 13 PwmOut motor1(D6);
Rosalie 4:6742476fadcf 14 PwmOut motor2(D5);
Rosalie 3:6ee0b20c23b0 15
Renate 9:4de589636f50 16 DigitalIn Power_button_pressed(D1);
Renate 9:4de589636f50 17 DigitalIn Emergency_button_pressed(D2);
WiesjeRoskamp 2:aee655d11b6d 18
Renate 8:c7d3b67346db 19 Ticker loop_ticker;
Renate 8:c7d3b67346db 20
Renate 11:4bc0304978e2 21 // Motoren aan-/uitzetten
Renate 8:c7d3b67346db 22 void motors_on_off()
WiesjeRoskamp 2:aee655d11b6d 23 {
Renate 8:c7d3b67346db 24 motor1=!motor1;
Renate 8:c7d3b67346db 25 motor2=!motor2;
Renate 9:4de589636f50 26 pc.printf("Motoren aan/uit functie\r\n");
Renate 8:c7d3b67346db 27 }
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 11:4bc0304978e2 37 // opnieuw worden opgestart. Mogelijk kan dit door de ticker de
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
Rosalie 3:6ee0b20c23b0 49
Renate 6:64146e16e10c 50 // Finite state machine programming (calibration servo motor?)
Renate 12:93ad9781eeef 51 enum states {Motors_off, Calib_motor, Calib_EMG, Homing, Operation_mode};
Renate 6:64146e16e10c 52
Renate 6:64146e16e10c 53 states currentState = Motors_off;
Renate 6:64146e16e10c 54 bool stateChanged = true; // Make sure the initialization of first state is executed
Renate 6:64146e16e10c 55
Renate 6:64146e16e10c 56 void ProcessStateMachine(void)
Renate 9:4de589636f50 57 {
Renate 6:64146e16e10c 58 switch (currentState)
Renate 6:64146e16e10c 59 {
Renate 6:64146e16e10c 60 case Motors_off:
Renate 6:64146e16e10c 61
Renate 9:4de589636f50 62 if (stateChanged)
Renate 6:64146e16e10c 63 {
Renate 8:c7d3b67346db 64 motors_off(); // functie waarbij motoren uitgaan
Renate 11:4bc0304978e2 65 stateChanged = false;
Renate 9:4de589636f50 66 pc.printf("Motors off state\r\n");
Renate 9:4de589636f50 67 }
Renate 11:4bc0304978e2 68 if (Power_button_pressed.read() == false) // Normaal waarde 1 bij indrukken, nu nul -> false
Renate 6:64146e16e10c 69 {
Renate 10:83f3cec8dd1c 70 motors_on_off();
Renate 9:4de589636f50 71 currentState = Calib_motor;
Renate 11:4bc0304978e2 72 stateChanged = true;
Renate 11:4bc0304978e2 73 pc.printf("Moving to Calib_motor state\r\n");
Renate 6:64146e16e10c 74 }
Renate 11:4bc0304978e2 75 if (Emergency_button_pressed.read() == false) // Normaal waarde 1 bij indrukken, nu nul -> false
Renate 8:c7d3b67346db 76 {
Renate 10:83f3cec8dd1c 77 emergency();
Renate 9:4de589636f50 78 }
Renate 6:64146e16e10c 79 break;
Renate 6:64146e16e10c 80
Renate 9:4de589636f50 81 case Calib_motor:
Renate 9:4de589636f50 82
Renate 9:4de589636f50 83 if (stateChanged)
Renate 9:4de589636f50 84 {
Renate 9:4de589636f50 85 // Hier wordt een kalibratie uitgevoerd, waarbij de motorhoeken worden bepaald
Renate 11:4bc0304978e2 86 currentState = Calib_EMG;
Renate 11:4bc0304978e2 87 stateChanged = true;
Renate 9:4de589636f50 88 pc.printf("Moving to Calib_EMG state\r\n");
Renate 9:4de589636f50 89 }
Renate 11:4bc0304978e2 90 if (Emergency_button_pressed.read() == false)
Renate 11:4bc0304978e2 91 {
Renate 11:4bc0304978e2 92 emergency();
Renate 11:4bc0304978e2 93 }
Renate 11:4bc0304978e2 94 break;
Renate 11:4bc0304978e2 95
Renate 11:4bc0304978e2 96 case Calib_EMG:
Renate 11:4bc0304978e2 97
Renate 11:4bc0304978e2 98 if (stateChanged)
Renate 11:4bc0304978e2 99 {
Renate 11:4bc0304978e2 100 // Hierbij wordt een een kalibratie uitgevoerd, waarbij de maximale EMG-amplitude waarde wordt bepaald
Renate 11:4bc0304978e2 101 currentState = Homing;
Renate 11:4bc0304978e2 102 stateChanged = true;
Renate 11:4bc0304978e2 103 pc.printf("Moving to Homing state\r\n");
Renate 11:4bc0304978e2 104 }
Renate 10:83f3cec8dd1c 105 if (Emergency_button_pressed.read() == false)
Renate 10:83f3cec8dd1c 106 {
Renate 11:4bc0304978e2 107 emergency();
Renate 11:4bc0304978e2 108 }
Renate 11:4bc0304978e2 109 break;
Renate 11:4bc0304978e2 110
Renate 11:4bc0304978e2 111 case Homing:
Renate 11:4bc0304978e2 112
Renate 11:4bc0304978e2 113 if (stateChanged)
Renate 11:4bc0304978e2 114 {
Renate 11:4bc0304978e2 115 // Ervoor zorgen dat de motoren zo bewegen dat de robotarm
Renate 11:4bc0304978e2 116 // (inclusief de end-effector) in de juiste home positie wordt gezet
Renate 11:4bc0304978e2 117 currentState = Operation_mode;
Renate 11:4bc0304978e2 118 stateChanged = true;
Renate 12:93ad9781eeef 119 pc.printf("Moving to operation mode \r\n");
Renate 11:4bc0304978e2 120 }
Renate 11:4bc0304978e2 121 if (Emergency_button_pressed.read() == false)
Renate 11:4bc0304978e2 122 {
Renate 10:83f3cec8dd1c 123 emergency();
Renate 10:83f3cec8dd1c 124 }
Renate 11:4bc0304978e2 125 break;
Renate 11:4bc0304978e2 126
Renate 11:4bc0304978e2 127 case Operation_mode:
Renate 11:4bc0304978e2 128
Renate 11:4bc0304978e2 129 if (stateChanged)
Renate 12:93ad9781eeef 130
Renate 11:4bc0304978e2 131 // Hier moet een functie worden aangeroepen die ervoor zorgt dat
Renate 11:4bc0304978e2 132 // aan de hand van EMG-signalen de motoren kunnen worden aangestuurd,
Renate 11:4bc0304978e2 133 // zodat de robotarm kan bewegen
Renate 11:4bc0304978e2 134
Renate 11:4bc0304978e2 135 if (Power_button_pressed.read() == false) // Normaal waarde 1 bij indrukken, nu nul -> false
Renate 11:4bc0304978e2 136 {
Renate 12:93ad9781eeef 137 motors_on_off();
Renate 11:4bc0304978e2 138 currentState = Motors_off;
Renate 11:4bc0304978e2 139 stateChanged = true;
Renate 12:93ad9781eeef 140 pc.printf("Terug naar de state Motors_off\r\n");
Renate 11:4bc0304978e2 141 }
Renate 11:4bc0304978e2 142 if (Emergency_button_pressed.read() == false)
Renate 11:4bc0304978e2 143 {
Renate 11:4bc0304978e2 144 emergency();
Renate 11:4bc0304978e2 145 }
Renate 12:93ad9781eeef 146 // wait(5);
Renate 11:4bc0304978e2 147 else
Renate 11:4bc0304978e2 148 {
Renate 11:4bc0304978e2 149 currentState = Homing;
Renate 11:4bc0304978e2 150 stateChanged = true;
Renate 12:93ad9781eeef 151 pc.printf("Terug naar de state Homing\r\n");
Renate 11:4bc0304978e2 152 }
Renate 11:4bc0304978e2 153 break;
Renate 9:4de589636f50 154
Renate 7:1d57463393c6 155 default:
Renate 7:1d57463393c6 156 // Zelfde functie als die eerder is toegepast om motoren uit te schakelen -> safety!
Renate 9:4de589636f50 157 pc.printf("Unknown or uninplemented state reached!\r\n");
Renate 8:c7d3b67346db 158
WiesjeRoskamp 2:aee655d11b6d 159 }
Renate 11:4bc0304978e2 160 }
WiesjeRoskamp 2:aee655d11b6d 161
Renate 8:c7d3b67346db 162 int main(void)
Renate 8:c7d3b67346db 163 {
Renate 9:4de589636f50 164 pc.printf("Opstarten\r\n");
Renate 12:93ad9781eeef 165 loop_ticker.attach(&ProcessStateMachine, 5.0f);
Renate 8:c7d3b67346db 166 while(true)
Renate 11:4bc0304978e2 167 { /* do nothing */}
Renate 8:c7d3b67346db 168 }