Diff: motor.h

Revision:

18:00e3d8c71a9c

Parent:

17:86e5af6f7628

Child:

19:2ac158fe414e

--- a/motor.h	Wed Nov 28 21:04:24 2018 +0000
+++ b/motor.h	Sun Dec 30 22:48:24 2018 +0000
@@ -1,372 +1,215 @@
-/**
- * @file motor.h
- * @brief File contains Crealab Motor Library.
- 
- * motor.h contains the class Motor, and related enums and structs.
- * Includes only "mbed.h".
- 
- * Rotation directions are now consistently called Clockwise, and Counterclockwise (CCW), 
- * instead of mix them with Left and Right. 
- * Doxygens Tags are preceeded by either a backslash @\ or by an at symbol @@.
+#include "motor.h"
 
- * @author Tarek Lule, Francois Druilhe, et al.
- * @date 01. Nov. 2018.
- * @see https://os.mbed.com/users/sepp_nepp/code/MotorLib/  */
- 
- // -------------------- Motor ---------------------------
- 
-#ifndef MOTOR_H
-#define MOTOR_H
 
-#include "mbed.h"
-
-#define MOTOR_STEP_TIME_MIN_US 700      /**< Shortest Time between two motor steps = 0.7ms, was MOTOR_STEP_TIME_MIN*/
-#define MOTOR_STEP_TIME_DEFAULT_US 5000 /**< Default Time between two motor steps = 5ms, was MOTOR_STEP_TIME_DEFAULT*/
-#define MOTOR_STEPS_FOR_A_TURN 4096     /**< Default number of motor steps to complete a turn = 4096 steps  */
+ // -------------------- MotStatus Helper ---------------------------
 
-/** \enum motorStates 
-* \brief Possible States of Motor state machine
-* 
-* Motor_CALIB is deprecated, was removed from the enum structure */
-typedef enum {
-    Motor_OFF = 0,  /**< All phase currents is off, replaces Motor_STOP. */  
-    Motor_ZERO,     /**< Motor at phase position 0 and ON, only reached by call of Zero() procedure. */  
-    Motor_ON,       /**< Phases are engaged, but Motor state machine stopped, replaces Motor_PAUSE. */  
-    Motor_RUN       /**< Phases are engaged, and Motor state machine runs*/  
-} motorStates;
-
-/** \enum motorCommands 
-* \brief Commands that are handled by the Motor state machine
-
-* These Commands are issued asynchonously by call Motor class methods.
-* They are executed in the state machine called by the ticker handler.
-
-* OFF and STOP commands do not go through the state machine.
-
-* MOTOR_restart is equivalent to and replaced by MOTOR_run.
-*/
-typedef enum {
-    MOTOR_nop = 0,  /**< No active command to execute. */
-    MOTOR_run,      /**< Run Motor until Nsteps are achieved. */
-    MOTOR_stop,     /**< Stop immediately all activity, turn off Motor. */
-    MOTOR_pause     /**< Motor is temporarily paused from the state run. */
-} motorCommands;
-
-/** \enum motorDir 
-* \brief Gives Motor direction to be Clockwise or Anticlockwise 
-*/
-typedef enum motorDir {
-    CLOCKWISE = 0,      /**< Turn Motor in clockwise direction. */
-    COUNTERCLOCKWISE    /**< Turn Motor in anti-clockwise direction. */
+void MotStatus::set(motorCommands aCmd, motorDir aDir, int32_t  aNSteps) {
+    cmd = aCmd;
+    dir = aDir;
+    NSteps  = aNSteps;
 };
 
-/** \struct MotStatus
-* \brief Structure of Motor Status registers.
+ // -------------------- Motor Class ---------------------------
 
-* Used by Motor Class to hold all Status 'Registers'. 
-* The structure can be requested to get by Motor.getStatus(). */
-typedef struct {
-    motorStates  state; /**< General state that the moter state machine is in.*/
-    motorCommands cmd;  /**< Command asked to be executed currently by the state machine.*/
-    motorDir      dir;  /**< Direction that the Motor is asked to run.*/
-    int32_t       NSteps;/**< Number of steps remain for the Motor to run. 
-    NSteps=0: all steps finished; NSteps<0: indicates to run "forever" */
-    bool         TickIsAttached; /**< Indicates if Ticker is attached.
-    Ticker is automatically attached while Motor runs, or paused; 
-    detaches when finished a run, or stopped.  */
-    void set(motorCommands aCmd, motorDir aDir, int32_t  aNSteps); /**< Helper; set Command, Direction and NSteps in one call. */
-} MotStatus;
+Motor::Motor(PinName _MPh[4]) {
+    initialization( _MPh , MOTOR_STEP_TIME_DEFAULT_US);
+}
+
+Motor::Motor(PinName _MPh0, PinName _MPh1, PinName _MPh2, PinName _MPh3) {
+    PinName _MPh[4] = {_MPh0, _MPh1, _MPh2, _MPh3};
+    initialization( _MPh , MOTOR_STEP_TIME_DEFAULT_US);
+}
 
-/** ATTENTION UNDER CONSTRUCTION, DO NOT YET USE.
-*
-*Class of a Four Phase Stepper Motor.
-*
-*Perform Runs for number of steps, or given amount angle, but also Low-Level steps. 
-*
-*High-Level Run functions have 'Run' in their name.
-*They rely on tickers and return immediately after ticker is set up. 
-*A state-machine evaluates commands versus the Motor state at every tick.
-*When End of Run is detected tickers stop, and Motor turns off. 
-*
-*Callbacks can be attached to react to 'end of run' events. 
-*Attention: the attached Callback is called within a Ticker Callback.
-*    Your code you execute in the Callback should be short, must not use waits, or any long routines.
-*    Do not call any Motor run commands in the callback, as it creates conflict situations.
-*    Long Callback code may impair this and any other Ticker functions that run in your application.
-*
-*Low-Level functions directly talk to the hardware without ticker. 
-*Use of Low-Level functions while tickers still run may lead to unexpected behavior. 
-*/
-class Motor
-{
-public:
-    /** Motor Class Creator
-    *
-    * Creates the class, initiallizes all fields, creates Phase Pins.  
-    * Time between two steps defaults here to MOTOR_STEP_TIME_DEFAULT_US=5000usec.
-    *
-    @code
-       PinName MotPhases[] = {PB_1, PB_15, PB_14, PB_13};
-       Motor MotorName(MotPhases); // Call this creator for example like this:  
-    @endcode 
-    * 
-    * @param _MPh Array of Names of the 4 Digital Pins of type PinNames 
-    */
-    Motor(PinName _MPh[4] );
+Motor::Motor(PinName _MPh0, PinName _MPh1, PinName _MPh2, PinName _MPh3, uint32_t aStepTime_us) {
+    PinName _MPh[4] = {_MPh0, _MPh1, _MPh2, _MPh3};
+    initialization( _MPh, aStepTime_us);
+}
+
+//void Motor::initialization(PinName _MPh0, PinName _MPh1, PinName _MPh2, PinName _MPh3, uint32_t aStepTime_us) {
+void Motor::initialization(PinName _MPh[4], uint32_t aStepTime_us) {
+
+    for (int ph=0; ph<4; ph++) { MPh[ph] = new DigitalOut(_MPh[ph]); } 
+    /*MPh[0] = new DigitalOut(_MPh0);
+    MPh[1] = new DigitalOut(_MPh1);
+    MPh[2] = new DigitalOut(_MPh2);
+    MPh[3] = new DigitalOut(_MPh3);*/
     
-    /** Motor Class Creator
-    *
-    * Creates the class, initiallizes all fields, creates Phase Pins.  
-    * Time between two steps defaults here to MOTOR_STEP_TIME_DEFAULT_US=5000usec.
-    *
-    @code
-      // Call this creator for example like this:
-       Motor MotorName(PB_1, PB_15, PB_14, PB_13);
-    @endcode 
-    * 
-    * @param <_MPh0, _MPh1, _MPh2, _MPh3> List of Names of the 4 Digital Pins of type PinNames    
-    */
-    Motor(PinName _MPh0, PinName _MPh1, PinName _MPh2, PinName _MPh3);
+    MotorOFF();    // Default state is all phases are OFF
+    StepPhase = 0; // initial phase is Zero
+    
+    Status.set(MOTOR_nop, CLOCKWISE, 0);// Default command is NOP, clockwise direction, 0 steps
     
-    /** Motor Class Creator
-    *
-    * Creates the class, initiallizes all fields, creates Phase Pins.  
-    * Time between two steps is passed as parameter.
-    *
-    @code
-      // Call this creator for example like this:
-       Motor MotorName(PB_1, PB_15, PB_14, PB_13);
-    @endcode 
-    *
-    * @param <_MPh0, _MPh1, _MPh2, _MPh3> List of Names of the 4 Digital Pins of type PinNames    
-    * @param <aStepTime_us> Lthe time in usec between two steps, thats used initially.   
-    */
-    Motor(PinName _MPh0, PinName _MPh1, PinName _MPh2, PinName _MPh3, uint32_t aStepTime_us);
+    Status.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
+
+    // itOnStop = true;    
+    // tuneTimings.reset();
+    // tuneTimings.start();
+}
+
+//Attaching and removing Callbacks
+void Motor::callbackSet(void (*CBfunction)(void))
+{  _callback = CBfunction;  }
 
-private:
-    // deprecated: void initialization(PinName _MPh0, PinName _MPh1, PinName _MPh2, PinName _MPh3, uint32_t aStepTime_us);
-    void initialization(PinName _MPh[4], uint32_t aStepTime_us);
+void Motor::callbackRemove() 
+{ _callback = NULL; }
+
+void Motor::RunInfinite(motorDir direction) {
+    Status.set(MOTOR_run, direction, -1);
+    StartTick();
+}
 
-public:
-    /** Attach a basic Callback function.
-    *
-    * Only called when a Run Command reaches it's requested end.
-    * Not called when the Motor is stopped by a call of Stop Function, or any other events.
-    * For use see precautions at Class description above. 
-    * Formerly called setMotorCallback()
-    *
-    @code
-    // Simple callback function, state variable endMove can be polled elsewhere
-    void CallBackFunction()
-    { endMove=true; }
+void Motor::RunSteps(motorDir direction, uint32_t steps) {
+    if (steps>0) 
+        { Status.set(MOTOR_run, direction, steps);
+        StartTick(); }
+}
+void Motor::RunDegrees(motorDir direction, float angle_deg) {
+    RunSteps(direction, (int)(angle_deg * (float)Steps_FullTurn / (float)360.0) );   
+}
 
-    // main routine
-    void main()
-    {   ...
-        // Attach callback function:
-        MotorInstance->callbackSet(CallBackFunction);
-        ...
-        while (true) {
-            ....
-        if (endMove) // poll the endMove flag
-               { ... } // react to Movement End
-               
-            ....
-        }
-    }
-    @endcode
-    * @param <*CBfunction> Callback function, must not be member of a class.
+void Motor::PauseRun() 
+{   if (Status.cmd==MOTOR_run) Status.cmd = MOTOR_pause;  }
 
-    */
-    void callbackSet(void (*CBfunction)(void));
+void Motor::RestartRun() 
+{   if (Status.cmd==MOTOR_pause) Status.cmd = MOTOR_run;  }
+
+void Motor::StopRun() 
+{   Status.cmd = MOTOR_stop;  } 
+
+MotStatus Motor::getStatus() { return Status; }
 
-    /** Attach a Callback function, member of a class.
-    * Only called when a Run Command reaches it's requested end.
-    * Not called when the Motor is stopped by a call of Stop Function, or any other events.
-    * For use see precautions at Class description above. 
-    * @param <*object> Class pointer which possesses callback member.
-    * @param <*CBmember> Pointer to callback function, member of Class.
-    * 
-    @code
-    // Class Creator:
-     AClass::AClass(Class Creation Parameters)
-    {   ...
-        // Attach callback function:
-        MotorInstance->setMotorCallback(this, &AClass::CallBackMemberFunction);
-        ...  
-    }
+/*******************************************************
+ **   Ticker / Timing procedures
+ *******************************************************/
+//Get, set the scaling
+uint32_t Motor::getStepsFullTurn()
+{    return Steps_FullTurn;  }
+
+void Motor::setStepsFullTurn(uint32_t StepsFullTurn) 
+{   Steps_FullTurn = StepsFullTurn; }
 
-    // Simple callback function, state variable endMove can be polled by main thread
-    void AClass::CallBackMemberFunction()
-    {  endMove=true;  }
-    @endcode
-    */
-    template<typename T>;
-    void callbackSet(T *object, void (T::*CBmember)(void)) {
-        _callback = callback(object,CBmember);
-    }
-
-    /** Remove the Callback function that may have been attached previously. */
-    void callbackRemove();
+void Motor::setRotationPeriodSec(float Seconds_Per_Turn) {
+    // rescale to usec and pass on to the next handler. 
+    setStepTime_us(1000 * Seconds_Per_Turn / Steps_FullTurn) ;
+}
+void Motor::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(Status.TickIsAttached) { StopTick(); StartTick(); }
+ }
 
-    /** High Level: Run Motor for a number of Steps.
-    * 
-    * During Run: Uses ticker; State: first Motor_ON then Motor_RUN; cmd=MOTOR_run.
-    * Call Pause() or Stop() to pause or end the run prematurely.
-    * At the end: calls the Callback, stops ticker; State: Motor_OFF.
-    * @param[in] <Direction> Given Direction, can be: CLOCKWISE, or COUNTERCLOCKWISE.
-    * @param[in] <Nsteps> Number of steps to run for; must be >0 ; Nsteps<=0 will not be executed.    
-    */
-   void RunSteps (motorDir Direction, uint32_t Nsteps);
-
-    /** High Level: Run Motor for a given angle
-    
-    * Runs Motor for a given angle in given direction. 
-    * Call Pause() or Stop() to pause or end the Motor run prematurely.
-    * While run: Uses ticker; State: first Motor_ON then Motor_RUN; cmd=MOTOR_run.
-    * At end: calls attached Callback, stops ticker; State: Motor_OFF; cmd=MOTOR_stop then MOTOR_nop.
-    * @param[in] <Direction> Given Direction, can be: CLOCKWISE, or COUNTERCLOCKWISE.
-    * @param[in] <angle_deg> Angle>0 to rotate for, in degrees, Angles<=0 are not executed.     
-    */
-   void RunDegrees  (motorDir Direction, float angle_deg);
-
-    /** High Level: Run Motor "unlimited"
-    
-    * Runs Motor with out limit in given direction, precisely runs 4Billion Steps.
-    * While run: Uses ticker; State: first Motor_ON then Motor_RUN; cmd=MOTOR_run.
-    * Call Pause() or Stop() to pause or end the Motor run.
-    * @param[in] <Direction> Given Direction, can be: CLOCKWISE, or COUNTERCLOCKWISE.
-    */
-    void RunInfinite (motorDir Direction);
-
-    /** High Level: Pause a motor Run.
-     * Put Motor into Pause state, Run is suspended, but only effective if Status.cmd=MOTOR_run.
-     * Retains the number of steps that remain to be run if restarting run.
-     * While paused: still uses ticker; State: Motor_RUN; cmd=MOTOR_pause.
-     * Use RestartRun(); to continue. */
-    void PauseRun();
+void Motor::StartTick() {
+    if(!Status.TickIsAttached)   {
+        // Connect Interrupt routine in which the Motor and all the state machine is performed
+        MotorSysTick.attach_us(callback(this, &Motor::ProcessMotorStateMachine), StepTime_us);
+        // last=tuneTimings.read_us();
+        Status.TickIsAttached=true;
+        }
+}
 
-    /** High Level: Restart a Paused Run.
-     * Restart the Run that was launched before calling PuaseRun. 
-     * Only effective if Status.cmd=MOTOR_pause, otherwise no re/action.
-     * Status afterwards is same as afterRun commands. */
-    void RestartRun();
-
-    /** High Level: End any Run.
-    * Force stop of any ongoing run, but does not call the Callback function.
-    * Only effective if Status.cmd=MOTOR_run, otherwise no re/action.
-    * Emits first cmd=MOTOR_stop then cmd=MOTOR_nop.
-    * Aftewards: ticker is detached; State: Motor_OFF; */
-    void StopRun();
-    
-public: // All the ticker timing related parameters
+void Motor::ProcessMotorStateMachine()
+{
+    switch(Status.cmd) {
+        case MOTOR_run: {
+            switch(Status.state) {
+                case Motor_OFF:
+                    MotorON(); // First only turn on the Motor ..
+                    break;
+                case Motor_ZERO:
+                case Motor_ON:
+                        Status.state = Motor_RUN;
+                case Motor_RUN:
+                    if (Status.NSteps==0)  // No more steps to go
+                        { Status.cmd = MOTOR_stop; 
+                          if (_callback) _callback.call(); } 
+                      else // More steps to go
+                        { StepOnce(); 
+                        Status.NSteps--;}
+                    break;
+                } // switch(Status.state)
+            } // case MOTOR_run
+        case MOTOR_stop:
+            StopTick();
+            Status.cmd = MOTOR_nop; 
+            MotorOFF(); 
+            break;       
+        case MOTOR_nop:
+        case MOTOR_pause:
+        default: break;
+    } // switch(Status.cmd)
+}
 
-    /** MidLevel: Get Motor status
-    *
-    * Gets the Status of the different internal mechanisms. 
-    * See documentation of MotStatus Structure.
-    * \return MotStatus The structure of Motor status registers. */
-    MotStatus getStatus();
-
-    /** MidLevel: Get number of Steps per Full turn
-    
-    * Defaults to MOTOR_STEPS_FOR_A_TURN = 4096.
-    * Used by RunDegrees() to translate from angle in degrees to number of steps.
-    * Old Name was: getCalibration, but that was not a good explicit name. 
-    * \return uint32_t The structure of Motor status registers. */
-    uint32_t getStepsFullTurn();
-
-    /** MidLevel: Set number of Steps per Full turn.
-    
-    * Defaults is MOTOR_STEPS_FOR_A_TURN = 4096.
-    * Used by RunDegrees() to translate from degrees to number of steps.
-    * Old Name was: setCalibration, but not good explicit name.
-    * @param <StepsFullTurn> Number of steps needed to complete a full motor turn
-    */
-    void setStepsFullTurn(uint32_t StepsFullTurn);
+void Motor::StopTick() {
+    if(Status.TickIsAttached)   
+      { MotorSysTick.detach(); Status.TickIsAttached=false; }
+}
 
-    /** Mid Level: Get the Motor step time. 
-    
-    * Step time is time between two Motor steps, and is given in microseconds
-    * and is passed to the ticker as delay time. 
-    * So the larger the value the slower the motor speed.  
-    * Defaults to MOTOR_STEP_TIME_DEFAULT_US = 5000.
-    * \return uint32_t The structure of Motor status registers.
-    */
-    uint32_t getStepTime_us;
-    
-    /** Set the time in microseconds between two Motor steps.
-    *  Defaults to MOTOR_STEP_TIME_DEFAULT_US = 5000usec.
-    *  Filters values below Minimum Value = 700.
-    *  Passed to the ticker as delay time.
-    *  Can be called while ticker is running, and takes immediate effect. 
-    *  Was previously called setStepTime(), but was not clear which units.  
-    * @param <aStepTime_us> the time in microseconds between two Motor steps
-    */
-    void setStepTime_us(uint32_t aStepTime_us);
+/*******************************************************
+ **   all the low level direct Motor HW access
+ *******************************************************/
+ void Motor::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();
+}
 
-    /** Set the time in seconds to get one full turn, rotation of 360°.
-    * was previously called setSpeed().  
-    * @param <Seconds_Per_Turn> Period of Rotation, e.g. if =20.0 then Motor will do 360° in 20 seconds.
-    */
-    void setRotationPeriodSec(float Seconds_Per_Turn) ;
-
-private:
-    // all the  Ticker and Timing procedures, used to run the Motor for a duration
-    void StartTick();
-    void ProcessMotorStateMachine();
-    // The call back function pointer that is called when the Processor
-    // State Machine reaches its end.
-    Callback<void()> _callback;
-    void StopTick();
-    MotStatus Status;
-    timestamp_t StepTime_us;  // Time in µs for one Motor step
-    Ticker      MotorSysTick; // System Timer for Motor
-    uint32_t   Steps_FullTurn;// Number of step for a complete turn
-
-public: // all the low level direct Motor HW access, States are immediately reached
+ /** Turn off all Motor Phases, no more current flowing */
+void Motor::MotorOFF() 
+{   for (int ph=0; ph<4; ph++) *MPh[ph] = 0;  
+    Status.state=Motor_OFF;
+}
 
-    /** Low Level: Run one full turn clockwise then anticlockwise. 
-    * After: State: Motor_OFF.
-    * Blocking function, returns back only after end of full movement. 
-    */
-    void MotorTest();
+/** Turn on the Motor Phase, In the last used phase, memorized in StepPhases 
+*  Equivalent to what previously the function "void Start();" did */
+void Motor::MotorON()
+{   SetPhases(); // attention, does not change StepPhase!
+    if (StepPhase==0)  Status.state=Motor_ZERO;  
+        else Status.state=Motor_ON;
+} 
+
+/** Motor phases turned on and put to Zero Position*/    
+void Motor::MotorZero() {
+    StepPhase = 0;  // sets the phases to 0
+    SetPhases(); 
+    Status.state=Motor_ZERO;
+}
+
+void    Motor::StepOnce()     // Move the Motor in the used 'direction'
+{   if (Status.dir == CLOCKWISE) StepClkW(); else StepCCW();
+}
 
-    /** Low Level: turn off all Motor Phases
-    * No more current flows, reduces holding force.
-    * After: State: Motor_OFF.
-    * StepPhases memorizes the last used phase.
-    * Equivalent what previously the function "void Stop();" did .  */
-    void MotorOFF();
-    
-    /** Low Level: turn on the Motor Phases in the last used phase.
-    * The last used phase is held in StepPhases.
-    * After: State: Motor_ON, or Motor_ZERO if StepPhases==0
-    * Equivalent to what previously the function "void Start();" did.  */
-    void MotorON();
-    
-    /** Low Level: Advance Motor one step, rotates in direction of variable direction. */
-    void StepOnce();
-    
-    /** Low Level: Advance Motor one step, rotates CounterClockwise. */
-    void StepCCW();
-    
-    /** Low Level: Advance Motor one step, rotates Clockwise. */
-    void StepClkW();
-    
-    /** Low Level: turn on the Motor Phases in Zero Position. 
-     * After: State: Motor_ZERO, StepPhases==0
-     */
-    void MotorZero();
-    
-    
-private:
+void    Motor::StepClkW()    // Move the Motor one step Clockwise
+{   if (StepPhase<7) StepPhase++;  else  StepPhase = 0;
+    SetPhases();
+}
+void    Motor::StepCCW()    // Move the Motor one step Clockwise
+{   if (StepPhase>0) StepPhase--;  else  StepPhase = 7;
+    SetPhases();
+}
 
-    /** Low Level: Engage Motor Phases according to MotorIndex. */
-    void 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    Motor::SetPhases()    // Engage Motor Phases according to StepPhase
+{ for (int ph=0; ph<4; ph++) { *MPh[ph] = MotPh[ph][StepPhase]; }   }
 
-    DigitalOut *MPh[4];       // Digital outputs, one per phase
-    int        StepPhase;     // Motor Phase Variable, counts up and down with every step
-};
-
-#endif