My Version of the Crealab MotorLib.
Fork of MotorLib by
Diff: motor.cpp
- Revision:
- 24:932ea7bdc850
- Parent:
- 23:6060422cd6eb
--- a/motor.cpp Wed Apr 17 21:48:14 2019 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,252 +0,0 @@ -#include "motor.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 - - setStatus(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: - rotate_angle_deg = 0; // calculated wheel rotation angle - 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) /**< Helper; set diameter and perim to values */ -{ diam_cm = Adiam_cm; - perim_cm = PI*diam_cm; - degree_per_cm=360.0f/perim_cm; -} - -/** Helper: calculate the needed wheel angle from a turn angle and a turn_radius_cm */ -float CreaMot:RunTurnAngle(float turn_angle_deg, float turn_radius_cm) -{ // a turn radius smaller than 0 make no sense - if( turn_radius_cm>= 0 ) - rotate_angle_deg = turn_angle_deg * PI_OVER_180 * turn_radius_cm * degree_per_cm; - else rotate_angle_deg = 0; - return rotate_angle_deg; -} - -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) { - setStatus(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) - { setStatus(MOTOR_run, AClockWise ^^ (steps<0), abs(steps)); - StartTick(); } -} - -void CreaMot::PauseRun() -{ if (CurrCmd==MOTOR_run) CurrCmd = MOTOR_pause; } - -void CreaMot::RestartRun() -{ if (CurrCmd==MOTOR_pause) CurrCmd = MOTOR_run; } - -void CreaMot::StopRun() -{ CurrCmd = MOTOR_stop; } - -MotStatus CreaMot::getStatus() { return Status; } - -/******************************************************* - ** 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::setStatus(motCmands aCmd, bool AClockWise, int32_t aNSteps) { - cmd = aCmd; - AClockWise = AClockWise; - NSteps = aNSteps; -}; - - - 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 Status 'direction' -{ if (Status.AClockWise) 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]; } } -