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?
User | Revision | Line number | New 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 | } |