Project BioRobotics Group 19
Dependencies: FastPWM HIDScope MODSERIAL QEI biquadFilter mbed
Diff: main.cpp
- Revision:
- 0:80ac024b84cb
- Child:
- 1:1221419474b3
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Mon Sep 25 10:12:20 2017 +0000 @@ -0,0 +1,159 @@ +#include "BiQuad.h" +#include "FastPWM.h" +#include "HIDScope.h" +#include "MODSERIAL.h" +#include "mbed.h" +#include "QEI.h" + +// SERIAL COMMUNICATION WITH PC +MODSERIAL pc(USBTX, USBRX); + +// STATES +enum states{MOTORS_OFF, HOMING, MOVING, HITTING}; + +states currentState = MOTORS_OFF; // Start with motors off +bool stateChanged = true; // Make sure the initialization of first state is executed + +// DEFINITIONS + + +// FUNCTIONS +// Turn motors off +void TurnMotorsOff() +{ + // Turn motors off +} + +// Move to home +void MoveToHome() +{ + // Move to home +} + +// Filter signals +float FilterSignal(// Signal) +{ + // Filter signal + return // Voltage +} + +// Motor 1 +void RotateMotor1(// Voltage) +{ + // Rotate motor 1 +} + +// Motor 2 +void RotateMotor2(// Voltage) +{ + // Rotate motor 2 +} + +// Hit the ball +void HitBall() +{ + // Rotate motor 3 +} + +// States function +void ProcessStateMachine() +{ + switch(currentState) + { + case MOTORS_OFF: + { + // State initialization + if(stateChanged) + { + pc.printf("Entering MOTORS_OFF \r\n"); + TurnMotorsOff(); // Turn motors off + stateChanged = false; + } + + // Home command + if(//HOME COMMAND) + { + currentState = HOMING; + stateChanged = true; + break; + } + } + + case HOMING: + { + // State initialization + if(stateChanged) + { + pc.printf("Entering HOMING \r\n"); + MoveToHome(); // Move to home position + stateChanged = false; + currentState = MOVING; + stateChanged = true; + break; + } + } + + case MOVING: + { + // State initialization + if(stateChanged) + { + pc.printf("Entering MOVING \r\n"); + stateChanged = false; + } + + // EMG signals to rotate motor 1 + if(// EMG signal) + { + FilterSignal(// Signal); // Filter the signal + RotateMotor1(// Voltage); // Rotate motor 1 + } + + // EMG signals to rotate motor 2 + if(// EMG signal) + { + FilterSignal(// Signal); // Filter the signal + RotateMotor2(// Voltage); // Rotate motor 2 + } + + // Hit command + if(// HIT COMMAND) + { + currentState = HITTING; + stateChanged = true; + break; + } + } + + case HITTING: + { + // State initialization + if(stateChanged) + { + pc.printf("Entering HITTING \r\n"); + HitBall(); // Hit the ball + stateChanged = false; + currentState = MOVING; + stateChanged = true; + break; + } + } + + default: + { + TurnMotorsOff(); // Turn motors off for safety + } + } +} + +// Main function +int main() +{ + // Serial communication + pc.baud(115200); + + while(true) + { + ProcessStateMachine(); // Execute states function + } +} \ No newline at end of file