Diff: motor.cpp

Revision:

3:01b4c058454d

Parent:

2:c91c5ef00d25

Child:

4:c009bcd5518c

--- a/motor.cpp	Fri Jun 17 14:32:16 2016 +0000
+++ b/motor.cpp	Fri Jun 24 14:57:25 2016 +0000
@@ -1,16 +1,14 @@
 #include "motor.h"
 
 
-
 Motor::Motor(PinName _MPh0, PinName _MPh1, PinName _MPh2, PinName _MPh3) {
  
     MPh0 = new DigitalOut(_MPh0);
     MPh1 = new DigitalOut(_MPh1);
     MPh2 = new DigitalOut(_MPh2);
     MPh3 = new DigitalOut(_MPh3);
-        
+        init = true;
     MotorIndex = 0;
- 
     //
     // Connect Interrupt routine in which the motor and all the state machine is performed
     //
@@ -19,27 +17,31 @@
     command = MOTOR_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, &Motor::ProcessMotorStateMachine, MotorStepTime);
-
+    NumSteps = 2000;               // Default
+ 
 }
 
+void Motor::RunSteps(MotorDir dir, uint32_t steps) {
+    SetDirection( dir);
+    NumSteps = steps;
+    SetCommand(MOTOR_start);
+}
+
+void Motor::RunDegrees(MotorDir dir, float degree) {
+    SetDirection( dir);
+    NumSteps = (degree * MotorFullTurn / 360.0);
+    SetCommand(MOTOR_start);
+}
 void Motor::SetDirection(MotorDir dir) {
     direction=dir;
 }
 
 void Motor::SetCommand(MotorCommand cmd) {
+    if(init)   MotorSystemTick.attach_us(this, &Motor::ProcessMotorStateMachine, MotorStepTime);
+    init = false;
     command=cmd;
 }
-void Motor::TurnMotor(uint32_t NumTurns) {
-       // uint32_t TimeToRunSec, TimeToRunMin;
-       // 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;
@@ -82,10 +84,10 @@
 
 
 void Motor::ProcessMotorStateMachine()
-{
+{        
         if (state==Motor_IDLE) {
             if (command == MOTOR_start) {
-                // Start the Wiring
+                // Start the Motor
                 StartMotor();
                 state = Motor_RUN;
                 }