My Version of the Crealab MotorLib.
Fork of MotorLib by
Diff: CreaMot.cpp
- Revision:
- 24:932ea7bdc850
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/CreaMot.cpp Thu Apr 18 12:12:48 2019 +0000 @@ -0,0 +1,249 @@ +#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]; } } +