My Version of the Crealab MotorLib.
Fork of MotorLib by
motor.cpp
- Committer:
- sepp_nepp
- Date:
- 2019-04-17
- Revision:
- 23:6060422cd6eb
- Parent:
- 22:d2c742bdae16
File content as of revision 23:6060422cd6eb:
#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]; } }