Tarek Lule
My Version of the Crealab MotorLib.
Diff: CreaMot.cpp
- Revision:
- 24:932ea7bdc850
diff -r 6060422cd6eb -r 932ea7bdc850 CreaMot.cpp
--- /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]; } }
+