Project BioRobotics Group 19
Dependencies: FastPWM HIDScope MODSERIAL QEI biquadFilter mbed
main.cpp
- Committer:
- jordiluong
- Date:
- 2017-09-25
- Revision:
- 1:1221419474b3
- Parent:
- 0:80ac024b84cb
- Child:
- 2:d3687b2c4e37
File content as of revision 1:1221419474b3:
// TEST VERSION #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 InterruptIn homeButton(SW2); // Button to go to home state InterruptIn hitButton(SW3); // Button to go to hit state float voltage; // FUNCTIONS // Turn motors off void TurnMotorsOff() { pc.printf("Turning motors off \r\n"); // Turn motors off } // Move to home void MoveToHome() { pc.printf("Moving to home \r\n"); // Move to home } // Filter signals float FilterSignal(float signal) { pc.printf("Filtering signal \r\n"); // Filter signal voltage = signal; return voltage; } // Motor 1 void RotateMotor1(float voltage) { pc.printf("Rotating motor 1 \r\n"); // Rotate motor 1 } // Motor 2 void RotateMotor2(float voltage) { pc.printf("Rotating motor 2 \r\n"); // Rotate motor 2 } // Hit the ball void HitBall() { pc.printf("Hitting the ball \r\n"); // 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(!homeButton) { 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(false) { FilterSignal(1); // Filter the signal RotateMotor1(voltage); // Rotate motor 1 pc.printf("%f \r\n", voltage); } // EMG signals to rotate motor 2 if(false) { FilterSignal(2); // Filter the signal RotateMotor2(voltage); // Rotate motor 2 pc.printf("%f \r\n", voltage); } // Hit command if(!hitButton) { 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 } }