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