Diff: main.cpp

Revision:

4:ea7689bf97e1

Parent:

3:5c3edcd29448

Child:

5:0d3e8694726e

Child:

6:246b05228f52

--- a/main.cpp	Mon Oct 09 13:59:45 2017 +0000
+++ b/main.cpp	Wed Oct 11 10:33:11 2017 +0000
@@ -14,72 +14,85 @@
 states currentState = MOTORS_OFF;                                               // Start with motors off
 bool stateChanged = true;                                                       // Make sure the initialization of first state is executed
 
+// ENCODER
+QEI Encoder(D12,D13,NC,32);                                                     // CONNECT ENC2A TO D13, ENC2B TO D12
+float vorig_omwentelingen = 0;
+float degrees;
+float vorig_degrees = 0;
+float omwentelingen;
+float counts;
+float snelheid_tpm;
+float snelheid_degps;
+float snelheid_tps;
+
 // PINS
-DigitalOut motor1DirectionPin(D4);                                              // Value 0: CW or CCW?; 1: CW or CCW?
+DigitalOut motor1DirectionPin(D4);                                              // Value 0: CCW; 1: CW
 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!!!
+InterruptIn button1(D2);                                                        // CONNECT BUT1 TO D2
+InterruptIn button2(D3);                                                        // CONNECT BUT2 TO D3
 
 // DEFINITIONS
-const float motorVelocity = 4;                                                  // rad/s
-const float motorGain = 4;                                                      // (rad/s) / PWM
+const float motorVelocity = 1;                                                  // unit: rad/s
+const float motorGain = 8.4;                                                    // unit: (rad/s) / PWM
+const int motorRatio = 131;                                                     // Ratio of the gearbox in the motors
+int motor1State = 0;                                                            // 0: Off, 1: CW, 2: CCW
+
+//TICKERS
+Ticker encoder;
+Ticker checkMotorState;
 
 // FUNCTIONS
-// Turn motors off
-/*void TurnMotorsOff()
+void RotateMotor1CW()
 {
-    // Turn motors off
+    motor1DirectionPin = 1;
+    motor1MagnitudePin = fabs(motorVelocity / motorGain);
+    //pc.printf("Rotating motor 1 CW \r\n");   
 }
 
-// Move to home
-void MoveToHome()
+void RotateMotor1CCW()
 {
-    // Move to home
-}
-
-// Filter signals
-float FilterSignal(// Signal)
-{
-    // Filter signal
-    return // Voltage
+    motor1DirectionPin = 0;
+    motor1MagnitudePin = fabs(motorVelocity / motorGain);
+    //pc.printf("Rotating motor 1 CounterCW \r\n");   
 }
 
-// Motor 1
-void RotateMotor1(// Voltage)
+void TurnMotor1Off()
 {
-    // Rotate motor 1
-}
-
-// Motor 2
-void RotateMotor2(// Voltage)
-{
-    // Rotate motor 2
+    motor1MagnitudePin = 0;
+    //pc.printf("Motors off \r\n");
 }
 
-// Hit the ball
-void HitBall()
+void CheckMotor1()                                                               // Checks the state of motor1 and rotates motor1 depending on the state
 {
-    // Rotate motor 3
-}
-*/
-
-void TurnMotor1CW()
-{
-    motor1DirectionPin = 0;
-    motor1MagnitudePin = fabs(motorVelocity / motorGain);   
+    switch(motor1State)
+    {
+        case 0:                                                                 // Turn motors off
+        {TurnMotor1Off(); break;}
+        
+        case 1:                                                                 // Rotate motor 1 CW
+        {RotateMotor1CW(); break;}
+        
+        case 2:                                                                 // Rotate motor 2 CCW
+        {RotateMotor1CCW(); break;}
+        
+        default:
+        break;
+    }
 }
 
-void TurnMotor1CCW()
+void EncoderCalc()
 {
-    motor1DirectionPin = 1;
-    motor1MagnitudePin = fabs(motorVelocity / motorGain);   
-}
-
-void TurnMotorsOff()
-{
-    motor1MagnitudePin = 0;
+    counts = Encoder.getPulses()/motorRatio;
+    degrees = counts/32*360;
+    omwentelingen = counts/32;
+    snelheid_tpm = (omwentelingen-vorig_omwentelingen)/0.5*60;
+    snelheid_tps = (omwentelingen-vorig_omwentelingen)/0.5;
+    snelheid_degps = (degrees-vorig_degrees)/0.5;
+    vorig_omwentelingen = omwentelingen;
+    vorig_degrees = degrees;
+    //pc.printf("%.1f pulsen, %.2f graden, %.2f omwentelingen, %.2f tpm, %.2f tps, %.2f deg/s\r\n",counts, degrees, omwentelingen, snelheid_tpm, snelheid_tps, snelheid_degps);
 }
 
 // States function
@@ -93,17 +106,18 @@
             if(stateChanged)
             {
                 pc.printf("Entering MOTORS_OFF \r\n");
-                TurnMotorsOff();                                                // Turn motors off
+                TurnMotor1Off();                                                // Turn motors off
                 stateChanged = false;
             }
             
             // Home command
-            if(button1)
+            if(!button1)
             {
                 currentState = MOVING;
                 stateChanged = true;
                 break;
             }
+            break;
         }
         
         case MOVING:
@@ -114,21 +128,14 @@
                 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
-            }
+
+            // Move commands
             
-            // EMG signals to rotate motor 2
-            if(// EMG signal)
-            {
-                FilterSignal(// Signal);                                        // Filter the signal
-                RotateMotor2(// Voltage);                                       // Rotate motor 2
-            }
+            if(!button1){motor1State = 1;}
+            else if(!button2){motor1State = 2;}
+            else{motor1State = 0;}
             
+            /*
             // Hit command    
             if(// HIT COMMAND)
             {
@@ -137,20 +144,7 @@
                 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();
+            break;
         }
         
         case HITTING:
@@ -159,17 +153,19 @@
             if(stateChanged)
             {
                 pc.printf("Entering HITTING \r\n");
-                //HitBall();                                                      // Hit the ball
+                //HitBall();                                                    // Hit the ball
                 stateChanged = false;
                 currentState = MOVING;
                 stateChanged = true;
                 break;
             }
+            break;
         }
         
         default:
         {
-            TurnMotorsOff();                                                    // Turn motors off for safety
+            TurnMotor1Off();                                                    // Turn motors off for safety
+            break;
         }
     }
 }
@@ -180,6 +176,11 @@
     // Serial communication
     pc.baud(115200);
     
+    pc.printf("START \r\n");
+    
+    encoder.attach(EncoderCalc, 0.5);
+    checkMotorState.attach(CheckMotor1, 0.01);
+    
     while(true)
     {
         ProcessStateMachine();                                                  // Execute states function