Renate de Boer / Mbed 2 deprecated script_voor_project_copy

Script 15-10-2019

Dependencies:   Servoaansturing mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

main.cpp@13:cb40c3037f9c, 2019-10-14 (annotated)

Committer:
Renate
Date:
Mon Oct 14 15:15:12 2019 +0000
Revision:
13:cb40c3037f9c
Parent:
12:93ad9781eeef
Child:
14:54343b9fd708
Verschillende snelheden

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