Diff: main.cpp

Revision:

3:5c3edcd29448

Parent:

2:d3687b2c4e37

Child:

4:ea7689bf97e1

--- a/main.cpp	Mon Sep 25 14:32:19 2017 +0000
+++ b/main.cpp	Mon Oct 09 13:59:45 2017 +0000
@@ -9,17 +9,26 @@
 MODSERIAL pc(USBTX, USBRX);
 
 // STATES
-enum states{MOTORS_OFF, HOMING, MOVING, HITTING};
+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()
+/*void TurnMotorsOff()
 {
     // Turn motors off
 }
@@ -54,6 +63,24 @@
 {
     // 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()
@@ -71,22 +98,8 @@
             }
             
             // Home command
-            if(//HOME COMMAND)
+            if(button1)
             {
-                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;
@@ -101,7 +114,7 @@
                 pc.printf("Entering MOVING \r\n");
                 stateChanged = false;
             }
-            
+            /*
             // EMG signals to rotate motor 1
             if(// EMG signal)
             {
@@ -123,6 +136,21 @@
                 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:
@@ -131,7 +159,7 @@
             if(stateChanged)
             {
                 pc.printf("Entering HITTING \r\n");
-                HitBall();                                                      // Hit the ball
+                //HitBall();                                                      // Hit the ball
                 stateChanged = false;
                 currentState = MOVING;
                 stateChanged = true;