File content as of revision 10:1df5a7a265e8:

#include "motor.h"
//extern Serial pc_uart;

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
    motorCallback = NULL;
    itOnStop = true;
            tuneTimings.reset();
         tuneTimings.start();
 toremove=false;
}

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() {
    motorCallback = NULL;
}
/*
void Motor::setMotorCallback(void (*mIT)()) {
    motorCallback = mIT;
    itOnStop = true;
}
void Motor::setMotorCallback(void (*mIT)(), bool onStop) {
    motorCallback = mIT;
}
*/

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;
    toremove=true;
    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();
     //   init=false;
        MotorSystemTick.attach_us(callback(this, &Motor::ProcessMotorStateMachine), MotorStepTime);
 //     pc_uart.printf("Changed icktime to %d %d\n\r",tickTime, 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);
    }
    //init=false;
}

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);
    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];
 //   pc_uart.printf("   STEP: %d, %d.%d.%d.%d\n\r",MotorIndex,RPh0[MotorIndex], RPh1[MotorIndex],RPh2[MotorIndex], 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};
//   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>0) MotorIndex--;
    else    MotorIndex = 7;
}

void    Motor::RunMotor()     // Move the Motor in the user direction
{
        if (direction==CLOCKWISE) RightMotor();
        else LeftMotor();
}


void Motor::ProcessMotorStateMachine()
{        /*
static int count = 0;
static uint32_t vmax=0;
if(state!=Motor_IDLE && toremove) {
   uint32_t v = tuneTimings.read_us();
   count = count+1;
   vmax = vmax+v-last;
last=v;
if(count==500) {
    printf("MOTOR TICK : %d - %d - => id %d\n\r",v,vmax/500,MotorIndex);
    count = 0;
    vmax=0;
}
}*/
        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) {
                        if(motorCallback != NULL) motorCallback();
                        _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();
}