Diff: MotorLib/motor.cpp

Revision:

0:0e577ce96b2f

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MotorLib/motor.cpp	Sat Feb 08 09:48:46 2020 +0000
@@ -0,0 +1,248 @@
+#include "motor.h"
+
+void Motor::initialization(PinName _MPh0, PinName _MPh1, PinName _MPh2, PinName _MPh3, uint32_t tickTime) {
+//pc_uart.printf("MOTOR INIT\n");
+    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
+    //
+    direction = CLOCKWISE;         // Default direction is clockwise
+    state = Motor_IDLE;    // Default state is IDLE
+    command = MOTOR_nop;       // Default command is NOP
+    MotorStepTime = tickTime;      // value in micro second for one step
+    MotorFullTurn = MOTOR_TICKS_FOR_A_TURN;       // Initial Calibration
+    NumSteps = MotorFullTurn;               // Default 1 turn
+    itOnStop = true;
+            tuneTimings.reset();
+         tuneTimings.start();
+}
+
+Motor::Motor(PinName _MPh0, PinName _MPh1, PinName _MPh2, PinName _MPh3) {
+    initialization( _MPh0,  _MPh1,  _MPh2,  _MPh3, MOTOR_STEP_TIME_DEFAULT);
+ 
+}
+
+
+Motor::Motor(PinName _MPh0, PinName _MPh1, PinName _MPh2, PinName _MPh3, uint32_t tickTime) {
+ 
+    initialization( _MPh0,  _MPh1,  _MPh2,  _MPh3, tickTime);
+ 
+}
+
+
+void Motor::removeMotorCallback() {
+    
+    _callback = NULL;
+    
+}
+
+void Motor::setMotorCallback(void (*function)(void))
+{
+    setMotorCallback(function, true);
+}
+void Motor::setMotorCallback(void (*function)(void), bool onStop)
+{
+    _callback = function;
+    itOnStop = onStop;  
+}
+
+
+uint32_t Motor::getCalibration()
+{
+    return MotorFullTurn;
+}
+
+void Motor::setCalibration(uint32_t nbTicksforFullTurn) {
+    MotorFullTurn = nbTicksforFullTurn;
+}
+
+void Motor::setDelayBtwTicks(uint32_t tickTime) {
+   if(MotorStepTime == tickTime) return;
+    MotorStepTime = tickTime;
+    if(MotorStepTime < MOTOR_STEP_TIME_MIN)  MotorStepTime = MOTOR_STEP_TIME_MIN;
+    // pc_uart.printf("Change ticktime to %d %d\n\r",tickTime, MotorStepTime);
+   if(!init) {
+        MotorSystemTick.detach();
+        MotorSystemTick.attach_us(callback(this, &Motor::ProcessMotorStateMachine), MotorStepTime);
+   }
+}
+
+void Motor::setSpeed(float sForOneTurn) {
+    MotorStepTime = 1000*sForOneTurn / MotorFullTurn;
+    if(MotorStepTime < MOTOR_STEP_TIME_MIN)  MotorStepTime = MOTOR_STEP_TIME_MIN;
+    if(!init) {
+        
+        MotorSystemTick.detach();
+       MotorSystemTick.attach_us(callback(this, &Motor::ProcessMotorStateMachine), MotorStepTime);
+    }
+}
+
+void Motor::Start() {
+        SetCommand(MOTOR_start);   
+};
+
+void Motor::Stop() {
+        SetCommand(MOTOR_stop);
+}
+
+
+void Motor::Pause() {
+        SetCommand(MOTOR_pause);
+}
+
+
+void Motor::Restart() {
+        SetCommand(MOTOR_restart);
+}
+
+void Motor::SetZero() {
+        SetCommand(MOTOR_zero);
+}
+
+void Motor::RunInfinite(MotorDir dir) {
+    SetDirection( dir);
+    NumSteps = -1;
+    SetCommand(MOTOR_start);
+}
+
+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 = (int)(degree * (float)MotorFullTurn / (float)360.0);
+    SetCommand(MOTOR_start);
+}
+void Motor::SetDirection(MotorDir dir) {
+    direction=dir;
+}
+
+void Motor::SetCommand(MotorCommand cmd) {
+    if(init)   {
+        MotorSystemTick.attach_us(callback(this, &Motor::ProcessMotorStateMachine), MotorStepTime);
+        last=tuneTimings.read_us();
+        init=false;
+        }
+    command=cmd;
+}
+
+void Motor::StopMotor() // --- Stop Motor
+{
+        *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
+{
+    static const   int RPh0[8] =  {1, 1, 0, 0, 0, 0, 0, 1};
+    static const   int RPh1[8] = {0, 1, 1, 1, 0, 0, 0, 0};
+    static const   int RPh2[8] = {0, 0, 0, 1, 1, 1, 0, 0};
+    static const   int RPh3[8] = {0, 0, 0, 0, 0, 1, 1, 1};
+    *MPh0 = RPh0[MotorIndex];  *MPh1 = RPh1[MotorIndex];  *MPh2 = RPh2[MotorIndex];  *MPh3 = RPh3[MotorIndex];
+    if (MotorIndex<7) MotorIndex++;
+    else    MotorIndex = 0;
+}
+void    Motor::LeftMotor()    // Move the Motor one step Right
+{
+    static const   int LPh0[8] =  {1, 1, 0, 0, 0, 0, 0, 1};
+    static const   int LPh1[8] = {0, 1, 1, 1, 0, 0, 0, 0};
+    static const   int LPh2[8] = {0, 0, 0, 1, 1, 1, 0, 0};
+    static const   int LPh3[8] = {0, 0, 0, 0, 0, 1, 1, 1};
+   *MPh0 = LPh0[MotorIndex];  *MPh1 = LPh1[MotorIndex];  *MPh2 = LPh2[MotorIndex];  *MPh3 = LPh3[MotorIndex];
+    if (MotorIndex>0) MotorIndex--;
+    else    MotorIndex = 7;
+}
+
+void    Motor::RunMotor()     // Move the Motor in the user direction
+{
+        if (direction==CLOCKWISE) RightMotor();
+        else LeftMotor();
+}
+
+
+void Motor::ProcessMotorStateMachine()
+{        
+
+        if (state==Motor_IDLE) {
+            if (command == MOTOR_start) {
+                // Start the Motor
+                StartMotor();
+                state = Motor_RUN;
+                }
+            else if (command == MOTOR_zero) {
+                // Start zeroing the Motor
+                StartMotor();
+                state = Motor_ZERO;
+                }
+            command = MOTOR_nop;
+            }
+        else if (state==Motor_RUN) {
+            // Action always performed in that state
+             if (NumSteps>0 || NumSteps <0) {
+                RunMotor();
+                NumSteps--;
+                }
+            // Check next state
+            if (command == MOTOR_pause) {
+                state = Motor_PAUSE;
+                }
+            else if ((command == MOTOR_stop)||(NumSteps==0)) {
+                StopMotor();
+                     if(state != Motor_IDLE ){//|| itOnStop == true) {
+                        _callback.call();
+                    }
+                state = Motor_IDLE;
+               }
+            command = MOTOR_nop;
+            }
+        else if (state==Motor_PAUSE) {
+            if (command == MOTOR_stop) {
+                StopMotor();
+                NumSteps=0;
+                state = Motor_IDLE;
+                }
+            else if (command == MOTOR_restart) {
+                state = Motor_RUN;
+                }
+            command = MOTOR_nop;
+            }
+        else if (state==Motor_ZERO) {
+            command = MOTOR_nop;
+            }
+}
+
+MotorStateList Motor::getState() {
+   return state;    
+}
+
+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