Renate de Boer / Mbed 2 deprecated script_voor_project_copy

Script 15-10-2019

Dependencies:   Servoaansturing mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

main.cpp@9:4de589636f50, 2019-10-14 (annotated)

Committer:
Renate
Date:
Mon Oct 14 12:41:38 2019 +0000
Revision:
9:4de589636f50
Parent:
8:c7d3b67346db
Child:
10:83f3cec8dd1c
Integrated emergency

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
Rosalie 3:6ee0b20c23b0 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 8:c7d3b67346db 21 // Motoren 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 8:c7d3b67346db 28
Renate 8:c7d3b67346db 29 void emergency()
Rosalie 3:6ee0b20c23b0 30 {
Renate 8:c7d3b67346db 31 motor1.write(0);
Renate 8:c7d3b67346db 32 motor2.write(0);
Renate 9:4de589636f50 33 pc.printf("Noodgeval functie\r\n");
Renate 8:c7d3b67346db 34 }
Renate 8:c7d3b67346db 35
Renate 8:c7d3b67346db 36 void motors_off()
Renate 8:c7d3b67346db 37 {
Renate 8:c7d3b67346db 38 motor1.write(0);
Renate 8:c7d3b67346db 39 motor2.write(0);
Renate 9:4de589636f50 40 pc.printf("Motoren uit functie\r\n");
Renate 8:c7d3b67346db 41 }
Renate 8:c7d3b67346db 42
Renate 8:c7d3b67346db 43
Rosalie 3:6ee0b20c23b0 44
Renate 6:64146e16e10c 45 // Finite state machine programming (calibration servo motor?)
Renate 9:4de589636f50 46 enum states {Motors_off, Calib_motor};
Renate 6:64146e16e10c 47
Renate 6:64146e16e10c 48 states currentState = Motors_off;
Renate 6:64146e16e10c 49 bool stateChanged = true; // Make sure the initialization of first state is executed
Renate 6:64146e16e10c 50
Renate 6:64146e16e10c 51 void ProcessStateMachine(void)
Renate 9:4de589636f50 52 {
Renate 6:64146e16e10c 53 switch (currentState)
Renate 6:64146e16e10c 54 {
Renate 6:64146e16e10c 55 case Motors_off:
Renate 6:64146e16e10c 56
Renate 9:4de589636f50 57 if (stateChanged)
Renate 6:64146e16e10c 58 {
Renate 8:c7d3b67346db 59 motors_off(); // functie waarbij motoren uitgaan
Renate 9:4de589636f50 60 pc.printf("Motors off state\r\n");
Renate 6:64146e16e10c 61 stateChanged = false;
Renate 9:4de589636f50 62 // wait_ms(5000);
Renate 9:4de589636f50 63 }
Renate 9:4de589636f50 64 if (Power_button_pressed.read() == false)
Renate 6:64146e16e10c 65 {
Renate 9:4de589636f50 66 // Power_button_pressed.mode(PullUp);
Renate 9:4de589636f50 67 // Power_button_pressed.rise(motors_on_off); // functie waarbij de motoren aan-/ uitgaan
Renate 6:64146e16e10c 68 stateChanged = true;
Renate 9:4de589636f50 69 pc.printf("Moving to Calib_motor state\r\n");
Renate 9:4de589636f50 70 currentState = Calib_motor;
Renate 6:64146e16e10c 71 }
Renate 9:4de589636f50 72 if (Emergency_button_pressed.read() == false)
Renate 8:c7d3b67346db 73 {
Renate 9:4de589636f50 74 // Emergency_button_pressed.mode(PullUp);
Renate 9:4de589636f50 75 // Emergency_button_pressed.rise(emergency); // functie die de motoren uitzet
Renate 9:4de589636f50 76 pc.printf("Ik ga exploderen!!!\r\n");
Renate 8:c7d3b67346db 77 loop_ticker.detach();
Renate 7:1d57463393c6 78 // Alles moet uitgaan (evt. een rood LEDje laten branden), moet
Renate 7:1d57463393c6 79 // opnieuw worden opgestart. Mogelijk kan dit door de ticker de
Renate 7:1d57463393c6 80 // detachen
Renate 9:4de589636f50 81 }
Renate 6:64146e16e10c 82 break;
Renate 6:64146e16e10c 83
Renate 9:4de589636f50 84 case Calib_motor:
Renate 9:4de589636f50 85
Renate 9:4de589636f50 86 if (stateChanged)
Renate 9:4de589636f50 87 {
Renate 9:4de589636f50 88 // Hier wordt een kalibratie uitgevoerd, waarbij de motorhoeken worden bepaald
Renate 9:4de589636f50 89 pc.printf("Moving to Calib_EMG state\r\n");
Renate 9:4de589636f50 90 }
Renate 9:4de589636f50 91 break;
Renate 9:4de589636f50 92
Renate 7:1d57463393c6 93 default:
Renate 7:1d57463393c6 94 // Zelfde functie als die eerder is toegepast om motoren uit te schakelen -> safety!
Renate 9:4de589636f50 95 pc.printf("Unknown or uninplemented state reached!\r\n");
Renate 8:c7d3b67346db 96
Renate 7:1d57463393c6 97 }
WiesjeRoskamp 2:aee655d11b6d 98 }
WiesjeRoskamp 2:aee655d11b6d 99
Renate 8:c7d3b67346db 100 int main(void)
Renate 8:c7d3b67346db 101 {
Renate 9:4de589636f50 102 pc.printf("Opstarten\r\n");
Renate 9:4de589636f50 103 loop_ticker.attach(&ProcessStateMachine, 3.0f);
Renate 8:c7d3b67346db 104 while(true)
Renate 9:4de589636f50 105 {
Renate 9:4de589636f50 106 // pc.printf("powerbutton: %d\r\n", Power_button_pressed.read());
Renate 9:4de589636f50 107 // pc.printf("emergencybutton: %d\r\n", Emergency_button_pressed.read());
Renate 9:4de589636f50 108 // wait(0.5);
Renate 9:4de589636f50 109 /* do nothing */}
Renate 8:c7d3b67346db 110 }