Renate de Boer / Mbed 2 deprecated script_voor_project_copy

Script 15-10-2019

Dependencies:   Servoaansturing mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

main.cpp@10:83f3cec8dd1c, 2019-10-14 (annotated)

Committer:
Renate
Date:
Mon Oct 14 13:34:03 2019 +0000
Revision:
10:83f3cec8dd1c
Parent:
9:4de589636f50
Child:
11:4bc0304978e2
WERKEND;

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