Project BioRobotics Group 19
Dependencies: FastPWM HIDScope MODSERIAL QEI biquadFilter mbed
main.cpp
- Committer:
- jordiluong
- Date:
- 2017-10-09
- Revision:
- 3:5c3edcd29448
- Parent:
- 2:d3687b2c4e37
- Child:
- 4:ea7689bf97e1
File content as of revision 3:5c3edcd29448:
#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, MOVING, HITTING}; states currentState = MOTORS_OFF; // Start with motors off bool stateChanged = true; // Make sure the initialization of first state is executed // PINS DigitalOut motor1DirectionPin(D4); // Value 0: CW or CCW?; 1: CW or CCW? PwmOut motor1MagnitudePin(D5); DigitalOut motor2DirectionPin(D7); // Value 0: CW or CCW?; 1: CW or CCW? PwmOut motor2MagnitudePin(D6); InterruptIn button1(D2); // BUTTON 1 AANSLUITEN OP D2!!! InterruptIn button2(D3); // BUTTON 2 AANSLUITEN OP D3!!! // DEFINITIONS const float motorVelocity = 4; // rad/s const float motorGain = 4; // (rad/s) / PWM // 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 } */ void TurnMotor1CW() { motor1DirectionPin = 0; motor1MagnitudePin = fabs(motorVelocity / motorGain); } void TurnMotor1CCW() { motor1DirectionPin = 1; motor1MagnitudePin = fabs(motorVelocity / motorGain); } void TurnMotorsOff() { motor1MagnitudePin = 0; } // 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(button1) { 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; } */ // Motor testen while(button1) { TurnMotor1CW(); pc.printf("Turning motor 1 CW \r\n"); } while(button2) { TurnMotor1CCW(); pc.printf("Turning motor 1 CounterCW \r\n"); } TurnMotorsOff(); } 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 } }