Diff: main.cpp

Revision:

9:c19f6f0f5080

Parent:

6:246b05228f52

--- a/main.cpp	Mon Oct 23 08:15:07 2017 +0000
+++ b/main.cpp	Mon Oct 23 14:13:47 2017 +0000
@@ -34,28 +34,33 @@
 PwmOut motor2MagnitudePin(D6);
 InterruptIn button1(D2);                                                        // CONNECT BUT1 TO D2
 InterruptIn button2(D3);                                                        // CONNECT BUT2 TO D3
-AnalogIn potPin(A1);
+InterruptIn button3(SW2);
+InterruptIn button4(SW3);
+AnalogIn pot1Pin(A0);
+AnalogIn pot2Pin(A1);
 
 // DEFINITIONS
 const float motorVelocity = 4.2;                                                  // 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
+int motor2State = 0;
 float motorValue = 0;
 
 //TICKERS
 Ticker encoder;
-Ticker checkMotorState;
+Ticker checkMotor1State;
+Ticker checkMotor2State;
 
 // FUNCTIONS
 void RotateMotor1CW()
 {
     motor1DirectionPin = 1;
     //motor1MagnitudePin = fabs(motorVelocity / motorGain);
-    motorValue = potPin;
+    motorValue = pot1Pin;
     motor1MagnitudePin = motorValue;
     //pc.printf("Rotating motor 1 CW \r\n");
-    pc.printf("%f \r\n", motorValue);   
+    pc.printf("Motor 1 CW %f \r\n", motorValue);   
 }
 
 void RotateMotor1CCW()
@@ -63,16 +68,16 @@
     motor1DirectionPin = 0;
     //motor1MagnitudePin = fabs(motorVelocity / motorGain);
     //pc.printf("Rotating motor 1 CounterCW \r\n");
-    motorValue = potPin;
+    motorValue = pot1Pin;
     motor1MagnitudePin = motorValue;
     //pc.printf("Rotating motor 1 CW \r\n");
-    pc.printf("%f \r\n", motorValue);   
+    pc.printf("Motor 1 CCW %f \r\n", motorValue);   
 }
 
 void TurnMotor1Off()
 {
     motor1MagnitudePin = 0;
-    //pc.printf("Motors off \r\n");
+    pc.printf("Motor 1 off \r\n");
 }
 
 void CheckMotor1()                                                               // Checks the state of motor1 and rotates motor1 depending on the state
@@ -92,6 +97,51 @@
         break;
     }
 }
+
+void RotateMotor2CW()
+{
+    motor2DirectionPin = 1;
+    //motor1MagnitudePin = fabs(motorVelocity / motorGain);
+    motorValue = pot2Pin;
+    motor2MagnitudePin = motorValue;
+    //pc.printf("Rotating motor 1 CW \r\n");
+    pc.printf("Motor 2 CW %f \r\n", motorValue);   
+}
+
+void RotateMotor2CCW()
+{
+    motor2DirectionPin = 0;
+    //motor1MagnitudePin = fabs(motorVelocity / motorGain);
+    //pc.printf("Rotating motor 1 CounterCW \r\n");
+    motorValue = pot2Pin;
+    motor2MagnitudePin = motorValue;
+    //pc.printf("Rotating motor 1 CW \r\n");
+    pc.printf("Motor 2 CCW %f \r\n", motorValue);   
+}
+
+void TurnMotor2Off()
+{
+    motor2MagnitudePin = 0;
+    pc.printf("Motor 2 off \r\n");
+}
+
+void CheckMotor2()                                                               // Checks the state of motor1 and rotates motor1 depending on the state
+{
+    switch(motor2State)
+    {
+        case 0:                                                                 // Turn motors off
+        {TurnMotor2Off(); break;}
+        
+        case 1:                                                                 // Rotate motor 1 CW
+        {RotateMotor2CW(); break;}
+        
+        case 2:                                                                 // Rotate motor 2 CCW
+        {RotateMotor2CCW(); break;}
+        
+        default:
+        break;
+    }
+}
 /*
 void EncoderCalc()
 {
@@ -147,6 +197,10 @@
             else if(!button2){motor1State = 2;}
             else{motor1State = 0;}
             
+            if(!button3){motor2State = 1;}
+            else if(!button4){motor2State = 2;}
+            else{motor2State = 0;}
+            
             /*
             // Hit command    
             if(// HIT COMMAND)
@@ -177,6 +231,7 @@
         default:
         {
             TurnMotor1Off();                                                    // Turn motors off for safety
+            TurnMotor2Off();
             break;
         }
     }
@@ -191,7 +246,8 @@
     pc.printf("START \r\n");
     
     //encoder.attach(EncoderCalc, 0.5);
-    checkMotorState.attach(CheckMotor1, 0.0002);
+    checkMotor1State.attach(&CheckMotor1, 0.01);
+    checkMotor2State.attach(&CheckMotor2, 0.01);
     
     while(true)
     {