File content as of revision 4:c009bcd5518c:

#include "motor.h"


Motor::Motor(PinName _MPh0, PinName _MPh1, PinName _MPh2, PinName _MPh3, uint32_t tickTime) {
 
    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 = 2140;       // Initial Calibration
    NumSteps = 2000;               // Default
 
}

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::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 * 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::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, 0, 0, 0, 0, 0, 1, 1};
    static const   int LPh1[8] = { 0, 0, 0, 0, 1, 1, 1, 0};
    static const   int LPh2[8] = { 0, 0, 1, 1, 1, 0, 0, 0};
    static const   int LPh3[8] = { 1, 1, 1, 0, 0, 0, 0, 0};
   *MPh0 = LPh0[MotorIndex];  *MPh1 = LPh1[MotorIndex];  *MPh2 = LPh2[MotorIndex];  *MPh3 = LPh3[MotorIndex];
    if (MotorIndex<7) MotorIndex++;
    else    MotorIndex = 0;
}

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) {
                RunMotor();
                NumSteps--;
                }
            // Check next state
            if (command == MOTOR_pause) {
                state = Motor_PAUSE;
                }
            else if ((command == MOTOR_stop)||(NumSteps<=0)) {
                StopMotor();
                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;
            }
}


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();
}