Diff: motor.cpp

Revision:

1:9519ac966b79

Parent:

0:bd05fd602a6e

Child:

2:c91c5ef00d25

--- a/motor.cpp	Fri Jun 17 11:16:07 2016 +0000
+++ b/motor.cpp	Fri Jun 17 11:44:22 2016 +0000
@@ -2,14 +2,141 @@
 
 
 
-Motor::Motor(PinName _MPh0, PinName _MPh1, PinName _MPh2, PinName _MPh3):DigitalOut(_MPh0), DigitalOut(_MPh1), DigitalOut(_MPh2), DigitalOut(_MPh3) {
-     
+Motor::Motor(PinName _MPh0, PinName _MPh1, PinName _MPh2, PinName _MPh3) {
+ 
+    MPh0 = DigitalOut(_MPh0);
+    MPh1 = DigitalOut(_MPh1);
+    MPh2 = DigitalOut(_MPh2);
+    MPh3 = DigitalOut(_MPh3);
+        
     MotorIndex = 0;
  
+    //
+    // Connect Interrupt routine in which the motor and all the state machine is performed
+    //
+    MotorDir = d_clock;         // Default direction is clockwise
+    MotorState = Motor_IDLE;    // Default state is IDLE
+    MotorCommand = k_nop;       // Default command is NOP
+    MotorStepTime = 30000;      // value in micro second for one step
+    MotorFullTurn = 2140;       // Initial Calibration
+    NumSteps = 0;               // Default
+    MotorSystemTick.attach_us(&this->ProcessMotorStateMachine, MotorStepTime);
+
 }
+
+void Motor::TurnMotor(uint32_t NumTurns) {
+         NumSteps = NumTurns*MotorFullTurn;
+         TimeToRunSec = (NumSteps * (MotorStepTime / 1000))/1000;
+         TimeToRunMin = TimeToRunSec / 60;
+          TimeToRunSec %= 60;
+    //DEBUG("%d  --> Step = %d; Time = %d,%2d (min, sec)\n\r", NumTurns, NumSteps, TimeToRunMin, TimeToRunSec);
+ 
  
 void Motor::StopMotor() // --- Stop Motor
 {
-        MPh0 = 0;  MPh1 = 0;  MPh2 = 0;  MPh3 = 0;
+        *MPh0 = 0;  *MPh1 = 0;  *MPh2 = 0;  *MPh3 = 0;
+        MotorIndex = 0;
+}
+
+
+void    Motor::StartMotor()
+{
         MotorIndex = 0;
+        *MPh0 = 1;  *MPh1 = 0;  *MPh2 = 0;   *MPh3 = 0;
+}
+
+void    Motor::RightMotor()    // Move the Motor one step Right
+{
+    const   int RPh0[4] = {0, 1, 0, 0};
+    const   int RPh1[4] = {0, 0, 1, 0};
+    const   int RPh2[4] = {0, 0, 0, 1};
+    const   int RPh3[4] = {1, 0, 0, 0};
+    *MPh0 = RPh0[MotorIndex];  *MPh1 = RPh1[MotorIndex];  *MPh2 = RPh2[MotorIndex];  *MPh3 = RPh3[MotorIndex];
+    if (MotorIndex<3) MotorIndex++;
+    else    MotorIndex = 0;
+}
+void    Motor::LeftMotor()    // Move the Motor one step Right
+{
+    const   int LPh0[4] = {0, 0, 1, 0};
+    const   int LPh1[4] = {0, 1, 0, 0};
+    const   int LPh2[4] = {1, 0, 0, 0};
+    const   int LPh3[4] = {0, 0, 0, 1};
+    *MPh0 = LPh0[MotorIndex];  *MPh1 = LPh1[MotorIndex];  *MPh2 = LPh2[MotorIndex];  *MPh3 = LPh3[MotorIndex];
+    if (MotorIndex<3) MotorIndex++;
+    else    MotorIndex = 0;
+}
+
+void    Motor::RunMotor()     // Move the Motor in the user direction
+{
+        if (MotorDir==d_clock) RightMotor();
+        else LeftMotor();
+}
+
+
+void Motor::ProcessMotorStateMachine()
+{
+        if (MotorState==Motor_IDLE) {
+            uint32_t    led = 0;
+            if (MotorCommand == k_wire) {
+                // Start the Wiring
+                StartMotor();
+                led = 1;
+                MotorState = Motor_RUN;
+                }
+            else if (MotorCommand == k_zero) {
+                // Start zeroing the Motor
+                StartMotor();
+                led = 1;
+                MotorState = Motor_ZERO;
+                }
+            MotorCommand = k_nop;
+            myled = led;        // LED is on when motor in use
+            }
+        else if (MotorState==Motor_RUN) {
+            // Action always performed in that state
+             if (NumSteps>0) {
+                RunMotor();
+                NumSteps--;
+                }
+            // Check next state
+            if (MotorCommand == k_pause) {
+                MotorState = Motor_PAUSE;
+                }
+            else if ((MotorCommand == k_stop)||(NumSteps<=0)) {
+                StopMotor();
+                MotorState = Motor_IDLE;
+                }
+            MotorCommand = k_nop;
+            }
+        else if (MotorState==Motor_PAUSE) {
+            if (MotorCommand == k_stop) {
+                StopMotor();
+                NumSteps=0;
+                MotorState = Motor_IDLE;
+                }
+            else if (MotorCommand == k_restart) {
+                MotorState = Motor_RUN;
+                }
+            MotorCommand = k_nop;
+            }
+        else if (MotorState==Motor_ZERO) {
+            MotorCommand = k_nop;
+            }
+}
+
+
+void Motor::TestMotor()    // Just to check that it make a full taurn back and forth
+{
+    int i;
+    StartMotor();
+    for (i=0; i<MotorFullTurn; i++) {
+        wait(0.005);
+        RightMotor();
+        }   
+    wait(0.5);
+    for (i=0; i<MotorFullTurn; i++) {
+        wait(0.005);
+        LeftMotor();
+        }   
+    StopMotor();
 }
\ No newline at end of file