File content as of revision 15:88fecbdd191c:

#include "motor.h"

void MotStatus::set(motorCommands aCmd, motorDir aDir, int32_t  aNSteps) {
    cmd = Acmd;
    dir = Adir;
    NSteps  = NSteps;
};

Motor::Motor(PinName _MPh[4]) {
    initialization( _MPh , MOTOR_STEP_TIME_DEFAULT_US);
}

Motor::Motor(PinName _MPh0, PinName _MPh1, PinName _MPh2, PinName _MPh3) {
    PinName _MPh[4] = {_MPh0, _MPh1, _MPh2, _MPh3};
    initialization( _MPh , MOTOR_STEP_TIME_DEFAULT_US);
}

Motor::Motor(PinName _MPh0, PinName _MPh1, PinName _MPh2, PinName _MPh3, uint32_t aStepTime_us) {
    PinName _MPh[4] = {_MPh0, _MPh1, _MPh2, _MPh3};
    initialization( _MPh, aStepTime_us);
}

//void Motor::initialization(PinName _MPh0, PinName _MPh1, PinName _MPh2, PinName _MPh3, uint32_t aStepTime_us) {
void Motor::initialization(PinName _MPh[4], uint32_t aStepTime_us) {

    for (int ph=0; ph<4; ph++) { MPh[ph] = new DigitalOut(_MPh[ph]); } 
    /*MPh[0] = new DigitalOut(_MPh0);
    MPh[1] = new DigitalOut(_MPh1);
    MPh[2] = new DigitalOut(_MPh2);
    MPh[3] = new DigitalOut(_MPh3);*/
    
    MotorOFF();    // Default state is all phases are OFF
    StepPhase = 0; // initial phase is Zero
    
    Status.set(MOTOR_nop, CLOCKWISE, 0);// Default command is NOP, clockwise direction, 0 steps
    
    Status.TickIsAttached = false;
    StepTime_us = aStepTime_us;// duration in micro second for one step

    Steps_FullTurn = MOTOR_STEPS_FOR_A_TURN;  // Default Calibration value
    _callback = NULL; // No default Callback

    // itOnStop = true;    
    // tuneTimings.reset();
    // tuneTimings.start();
}

//Attaching and removing Callbacks
void Motor::removeMotorCallback() 
{ _callback = NULL; }

void Motor::setMotorCallback(void (*function)(void))
{  _callback = function;  }

uint32_t Motor::getStepsFullTurn()
{    return Steps_FullTurn;  }

void Motor::setStepsFullTurn(uint32_t StepsFullTurn) 
{   Steps_FullTurn = StepsFullTurn; }

void Motor::RunInfinite(motorDir direction) {
    Status.set(MOTOR_run, direction, -1);
    StartTick();
}

void Motor::RunSteps(motorDir direction, uint32_t steps) {
    if (steps>0) 
        { Status.set(MOTOR_run, direction, steps);
        StartTick(); }
}

void Motor::Pause() 
{   if (Status.cmd==MOTOR_run) Status.cmd = MOTOR_pause;  }

void Motor::Restart() 
{   if (Status.cmd==MOTOR_pause) Status.cmd = MOTOR_run;  }

void Motor::Stop() 
{   Status.cmd = MOTOR_stop;  } 

MotStatus Motor::getStatus() { return Status; }

void Motor::TestMotor()    // Just to check that it make a full turn back and forth
{
    int i;
    MotorON();
    for (i=0; i<Steps_FullTurn; i++) {
        wait(0.005);
        StepRight();
        }   
    wait(0.5);
    for (i=0; i<Steps_FullTurn; i++) {
        wait(0.005);
        StepLeft();
        }   
    MotorOFF();
}

/*******************************************************
 **   Ticker / Timing procedures
 *******************************************************/

void Motor::setRotationPeriodSec(float Seconds_Per_Turn) {
    // rescale to usec and pass on to the next handler. 
    setStepTime_us(1000 * Seconds_Per_Turn / Steps_FullTurn) ;
}
void Motor::setStepTime_us(uint32_t aStepTime_us) {
    if(StepTime_us == aStepTime_us) return; // avoid futile activity
    if (StepTime_us < MOTOR_STEP_TIME_MIN_US) // filter too small values
       StepTime_us = MOTOR_STEP_TIME_MIN_US;
       else StepTime_us = aStepTime_us; // or if OK then assign value
    // if ticker already running recreate a new ticker;
    if(Status.TickIsAttached) { StopTick(); StartTick(); }
 }

void Motor::StartTick() {
    if(!Status.TickIsAttached)   {
        // Connect Interrupt routine in which the motor and all the state machine is performed
        MotorSysTick.attach_us(callback(this, &Motor::ProcessMotorStateMachine), StepTime_us);
        // last=tuneTimings.read_us();
        Status.TickIsAttached=true;
        }
}

void Motor::ProcessMotorStateMachine()
{
    switch(Status.cmd) {
        case MOTOR_run: {
            switch(Status.state) {
                case Motor_OFF:
                    MotorON(); // First only turn on the motor ..
                    break;
                case Motor_ZERO:
                case Motor_ON:
                        Status.state = Motor_RUN;
                case Motor_RUN:
                    if (Status.NSteps==0)  // No more steps to go
                        { Status.cmd = MOTOR_stop; 
                          if (_callback) _callback.call(); } 
                      else // More steps to go
                        { StepOnce(); 
                        Status.NSteps--;}
                    break;
                } // switch(Status.state)
            } // case MOTOR_run
        case MOTOR_stop:
            StopTick();
            Status.cmd = MOTOR_nop; 
            MotorOFF(); 
            break;       
        case MOTOR_nop:
        case MOTOR_pause:
        default: break;
    } // switch(Status.cmd)
}

void Motor::StopTick() {
    if(Status.TickIsAttached)   
      { MotorSysTick.detach(); Status.TickIsAttached=false; }
}

/*******************************************************
 **   all the low level direct motor HW access
 *******************************************************/
 
 /** Turn off all motor Phases, no more current flowing */
void Motor::MotorOFF() 
{   for (int ph=0; ph<4; ph++) *MPh[ph] = 0;  
    Status.state=Motor_OFF;
}

/** Turn on the motor Phase, In the last used phase, memorized in StepPhases 
*  Equivalent to what previously the function "void Start();" did */
void Motor::MotorON()
{   SetPhases(); // attention, does not change StepPhase!
    if (StepPhase==0)  Status.state=Motor_ZERO;  
        else Status.state=Motor_ON;
} 

/** Motor phases turned on and put to Zero Position*/    
void Motor::MotorZero() {
    StepPhase = 0;  // sets the phases to 0
    SetPhases(); 
    Status.state=Motor_ZERO;
}

void    Motor::StepOnce()     // Move the Motor in the used 'direction'
{   if (Status.dir == CLOCKWISE) StepRight(); else StepLeft();
}

void    Motor::StepRight()    // Move the Motor one step Right
{   if (StepPhase<7) StepPhase++;  else  StepPhase = 0;
    SetPhases();
}
void    Motor::StepLeft()    // Move the Motor one step Right
{   if (StepPhase>0) StepPhase--;  else  StepPhase = 7;
    SetPhases();
}

static const int MotPh[4][8] = 
    { {1, 1, 0, 0, 0, 0, 0, 1}, 
      {0, 1, 1, 1, 0, 0, 0, 0}, 
      {0, 0, 0, 1, 1, 1, 0, 0}, 
      {0, 0, 0, 0, 0, 1, 1, 1}};
      
void    Motor::SetPhases()    // Engage Motor Phases according to StepPhase
{ for (int ph=0; ph<4; ph++) { *MPh[ph] = MotPh[ph][StepPhase]; }   }