Script 15-10-2019
Dependencies: Servoaansturing mbed QEI HIDScope biquadFilter MODSERIAL FastPWM
main.cpp
- Committer:
- Renate
- Date:
- 2019-10-14
- Revision:
- 9:4de589636f50
- Parent:
- 8:c7d3b67346db
- Child:
- 10:83f3cec8dd1c
File content as of revision 9:4de589636f50:
#include "mbed.h" #include "HIDScope.h" #include "QEI.h" #include "MODSERIAL.h" #include "BiQuad.h" #include "FastPWM.h" #include <math.h> #include "Servo.h" //definieer objecten Serial pc(USBTX, USBRX); PwmOut motor1(D6); PwmOut motor2(D5); DigitalIn Power_button_pressed(D1); DigitalIn Emergency_button_pressed(D2); Ticker loop_ticker; // Motoren uitzetten void motors_on_off() { motor1=!motor1; motor2=!motor2; pc.printf("Motoren aan/uit functie\r\n"); } void emergency() { motor1.write(0); motor2.write(0); pc.printf("Noodgeval functie\r\n"); } void motors_off() { motor1.write(0); motor2.write(0); pc.printf("Motoren uit functie\r\n"); } // Finite state machine programming (calibration servo motor?) enum states {Motors_off, Calib_motor}; states currentState = Motors_off; bool stateChanged = true; // Make sure the initialization of first state is executed void ProcessStateMachine(void) { switch (currentState) { case Motors_off: if (stateChanged) { motors_off(); // functie waarbij motoren uitgaan pc.printf("Motors off state\r\n"); stateChanged = false; // wait_ms(5000); } if (Power_button_pressed.read() == false) { // Power_button_pressed.mode(PullUp); // Power_button_pressed.rise(motors_on_off); // functie waarbij de motoren aan-/ uitgaan stateChanged = true; pc.printf("Moving to Calib_motor state\r\n"); currentState = Calib_motor; } if (Emergency_button_pressed.read() == false) { // Emergency_button_pressed.mode(PullUp); // Emergency_button_pressed.rise(emergency); // functie die de motoren uitzet pc.printf("Ik ga exploderen!!!\r\n"); loop_ticker.detach(); // Alles moet uitgaan (evt. een rood LEDje laten branden), moet // opnieuw worden opgestart. Mogelijk kan dit door de ticker de // detachen } break; case Calib_motor: if (stateChanged) { // Hier wordt een kalibratie uitgevoerd, waarbij de motorhoeken worden bepaald pc.printf("Moving to Calib_EMG state\r\n"); } break; default: // Zelfde functie als die eerder is toegepast om motoren uit te schakelen -> safety! pc.printf("Unknown or uninplemented state reached!\r\n"); } } int main(void) { pc.printf("Opstarten\r\n"); loop_ticker.attach(&ProcessStateMachine, 3.0f); while(true) { // pc.printf("powerbutton: %d\r\n", Power_button_pressed.read()); // pc.printf("emergencybutton: %d\r\n", Emergency_button_pressed.read()); // wait(0.5); /* do nothing */} }