Tarek Lule
My Version of the Crealab MotorLib.
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]; } }
-