File content as of revision 24:932ea7bdc850:

#include "CreaMot.h"

  // -------------------- CreaMot Class ---------------------------

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

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

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

void CreaMot::initialization(PinName _MPh[4], uint32_t AStepTime_us) {

    for (int ph=0; ph<4; ph++) { MPh[ph] = new DigitalOut(_MPh[ph]); } 
    
    MotorOFF();    // Default state is all phases are OFF
    StepPhase = 0; // initial phase is Zero
    
    setCommand(MOTOR_nop, CLOCKWISE, 0);// Default command is NOP, clockwise direction, 0 steps
    
    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
    
    // All geometric information defaults to 0:
    diam_cm          = 0; // wheel diameter in centimeter
    perim_cm         = 0; // wheel perimeter in centimeter
    degree_per_cm    = 0; // rotation angle in degrees per cm circumference
    defaultDirection = CLOCKWISE;
}

// *****************************************************************
//   all following functions are based on centimeter information
// *****************************************************************

/* High Level: Run CreaMot for a given number of Dist_cm, Dist_cm<0 are run in opposite direction. */
void CreaMot::RunDist_cm(bool AClockWise, float Dist_cm)
{  RunDegrees(AClockWise,Dist_cm * degree_per_cm); }

/* High Level: Run CreaMot for a given number of Dist_cm in default direction. */
void CreaMot::RunDist_cm(float Dist_cm)
{  RunDegrees(defaultDirection, Dist_cm * degree_per_cm); }

/** Additional geometric information: set the wheel diameter, also sets perimeter and degrees per cm.*/
void CreaMot::setDiamCM( float Adiam_cm) 
{   diam_cm = Adiam_cm;
    perim_cm = PI*diam_cm;
    degree_per_cm=360.0f/perim_cm;
} 

/** Calculate the needed wheel angle from a turn angle and a turn_radius_cm  */
void CreaMot::RunTurnAngle(float turn_angle_deg, float turn_radius_cm) 
{   // a turn radius smaller than 0 make no sense is not executed
    if( turn_radius_cm>= 0 ) 
        RunDegrees(  turn_angle_deg * PI_OVER_180 * turn_radius_cm * degree_per_cm);
} 

void CreaMot::setSpeed_cm_sec(float speed_cm_sec)
{   if (speed_cm_sec < MIN_SPEED_CM_SEC) // catch too small or negative speeds
          setRotationPeriodSec( perim_cm / MIN_SPEED_CM_SEC); 
    else  setRotationPeriodSec( perim_cm / speed_cm_sec );
}

// *****************************************************************
//  all following functions are agnostic of centimeter-information
// *****************************************************************

void CreaMot::RunInfinite(bool AClockWise) 
{   setCommand(MOTOR_run, AClockWise, -1);
    StartTick();
}

/* High Level: Run CreaMot for a given angle, angles<0 are run in opposite direction. */
void CreaMot::RunDegrees(bool AClockWise, float angle_deg) {
 RunSteps( AClockWise, (int32_t)(angle_deg * (float)Steps_FullTurn / 360.0f) );   
}

/* High Level: Run CreaMot for a given angle in default direction, angles<0 are run in opposite direction. */
void CreaMot::RunDegrees(float angle_deg) {
 RunSteps( defaultDirection, (int32_t)(angle_deg * (float)Steps_FullTurn / 360.0f) );   
}

/* High Level: Run CreaMot for a given number of steps, steps<0 are run in opposite direction. */
void CreaMot::RunSteps(bool AClockWise, int32_t steps) 
{   if (steps<0) { AClockWise = !AClockWise; steps=-steps; }
    if (steps>0) 
        { setCommand( MOTOR_run, AClockWise, steps );
          StartTick(); }
}

void CreaMot::PauseRun() 
{   if (CurrCmd==MOTOR_run) CurrCmd = MOTOR_pause;  }

void CreaMot::RestartRun() 
{   if (CurrCmd==MOTOR_pause) CurrCmd = MOTOR_run;  }

// not only halt the state machine but also set NSteps to 0
void CreaMot::StopRun() 
{   setCommand(MOTOR_stop,ClockWise,0);  } 

void CreaMot::setCommand(motCmands aCmd, bool aClockWise, int32_t  aNSteps) {
    CurrCmd    = aCmd;
    ClockWise  = aClockWise;
    NStepsToDo = aNSteps;
};


/*******************************************************
 **   Ticker / Timing procedures
 *******************************************************/
//Get, set the scaling
uint32_t CreaMot::getStepsFullTurn()
{    return Steps_FullTurn;  }

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

void CreaMot::setRotationPeriodSec(float Seconds_Per_Turn) {
    // rescale to usec and pass on to the next handler. 
    setStepTime_us( uint32_t(Seconds_Per_Turn / Steps_FullTurn * 1000000) ) ;
}
void CreaMot::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(TickIsAttached) { StopTick(); StartTick(); }
 }

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

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

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

/*******************************************************
 **   all the low level direct CreaMot HW access
 *******************************************************/
 

 void CreaMot::MotorTest()    // 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);
        StepClkW();
        }   
    wait(0.5);
    for (i=0; i<Steps_FullTurn; i++) {
        wait(0.005);
        StepCCW();
        }   
    MotorOFF();
}

 /** Turn off all CreaMot Phases, no more current flowing */
void CreaMot::MotorOFF() 
{   for (int ph=0; ph<4; ph++) *MPh[ph] = 0;  
    CurrState=Motor_OFF;
}

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

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

void    CreaMot::StepOnce()     // Move the CreaMot in the requested 'direction'
{   if (ClockWise) StepClkW(); else StepCCW();
}

void    CreaMot::StepClkW()    // Move the CreaMot one step Clockwise
{   if (StepPhase<7) StepPhase++;  else  StepPhase = 0;
    SetPhases();
}
void    CreaMot::StepCCW()    // Move the CreaMot one step Clockwise
{   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    CreaMot::SetPhases()    // Engage CreaMot Phases according to StepPhase
{ for (int ph=0; ph<4; ph++) { *MPh[ph] = MotPh[ph][StepPhase]; }   }