Mederic Melard
For driving stepper motor driver with traditional (step/dir) control pins Based on "Generate stepper-motor speed profiles in real time" from "David AUSTIN - December 30, 2004"
Dependents: VAM_reliability VAM_REV2 VAM_DAX VAM_DAX_RELIABILITY ... more
Fork of
Stepper
by 
Mederic Melard
Revision 1:a456aa3935ca, committed 2014-09-22
- Comitter:
- mederic
- Date:
- Mon Sep 22 14:18:31 2014 +0000
- Parent:
- 0:f2a3b0be279d
- Commit message:
- Correct Bug of 1st version
Changed in this revision
| Stepper.cpp | Show annotated file Show diff for this revision Revisions of this file |
| Stepper.h | Show annotated file Show diff for this revision Revisions of this file |
--- a/Stepper.cpp Mon Apr 15 11:29:15 2013 +0000
+++ b/Stepper.cpp Mon Sep 22 14:18:31 2014 +0000
@@ -5,10 +5,12 @@
//***********************************/************************************
Stepper::Stepper(PinName clk, PinName dir): _clk(clk) , _dir(dir)
{
+ _clk = 1;
_state = STOP;
_pos = 0;
_steps = 0;
_spd = 400;
+ _dt0 = 0;
}
//***********************************/************************************
@@ -65,8 +67,9 @@
// Public Methods //
//***********************************/************************************
void Stepper::stop(void)
-{
- _timer.detach(); //detach step generator thread
+{
+ _clk = 1;
+ remove(); //stop timer
_state = STOP; //update state machine
_steps = 0; //reset total steps per move
}
@@ -76,7 +79,7 @@
if(!_spd)return; //spd must > 0
_dir = direction; //set output pin direction value
_steps = 0; //rotate until stop() by user
- run(); //start thread
+ handler(); //start thread
}
void Stepper::move(int steps)
@@ -92,7 +95,7 @@
_dir = CW; //set output pin direction value
_steps = steps; //total steps per move
}
- run();
+ handler(); //start thread
}
void Stepper::goesTo(int position)
@@ -103,61 +106,70 @@
//***********************************/************************************
// Protected Methods //
//***********************************/************************************
-void Stepper::run(void)
+void Stepper::handler(void)
{
- static float dtn; //delay for next step
- static unsigned int nStartDec; //steps to start decelerate
- static unsigned int n; //steps counter
-
+ static float i;
+
switch(_state)
{
case STOP:
- n = 0; //reset setp counter (motor stopped)
+ _n = 0; //reset setp counter (motor stopped)
if(_dt0 <= _dtmin || !_acc) //if first step faster than max speed step
{
- dtn = _dtmin; //delay = delaymin
+ _dtn = _dtmin; //delay = delaymin
_state = CRUISE; //no acceleration needed
}
else
{
- dtn = _dt0; //set first delay
+ _dtn = _dt0; //set first delay
_state = ACCEL; //acceleration phase
}
if(_steps) //if finite mov required
{
unsigned int nToSpeed = nTo(_spd,_acc); //Equation 16 How many steps to reach max speed
- nStartDec = (_steps * _dec) / (_dec + _acc); //Equation 19 after how many step we must start decelerate
- if(nStartDec > nToSpeed)nStartDec = _steps - ((nToSpeed*_acc)/_dec); //if speed can be reach Equation 17
- }
+ _nStartDec = (_steps * _dec) / (_dec + _acc); //Equation 19 after how many step we must start decelerate
+ if(_nStartDec > nToSpeed)_nStartDec = _steps - ((nToSpeed*_acc)/_dec); //if speed can be reach Equation 17
+ }
+ i = _dtn;
break;
case ACCEL:
- dtn -= (dtn*2) / ((n<<2)+1); //Equation 20 find next delay
- if((unsigned int)dtn <= _dtmin) //if max speed reached
+ //_dtn -= (_dtn*2.0) / ((_n<<2)+1); //Equation 20 find next delay
+ i-= i*2.0 / ((_n<<2)+1);
+ _dtn = i;
+
+ if((unsigned int)_dtn <= _dtmin) //if max speed reached
{
- dtn = _dtmin;
+ _dtn = _dtmin;
+ i = _dtn;
_state = CRUISE; //constant phase
}
- if(_steps && _dec && n >= nStartDec)_state = DECEL; //chech when must start decelerate
-
+ if(_steps && _dec && _n >= _nStartDec)_state = DECEL; //chech when must start decelerate
break;
case CRUISE:
- if(_steps && _dec && n >= nStartDec)_state = DECEL; //chech when must start decelerate
+ if(_steps && _dec && _n >= _nStartDec)_state = DECEL; //chech when must start decelerate
break;
case DECEL:
- dtn += (dtn*2) / (((_steps-n)<<2)+1); //Equation 20 find next delay
+ //_dtn += (_dtn*2) / (((_steps-_n)<<2)+1); //Equation 20 find next delay
+ i+= (i*2.0) / (((_steps-_n)<<2)+1);
+ _dtn = i;
break;
}
+
+ _clk=0;
+
+ if(!_n) insert(_dtn + us_ticker_read()); //start timer @ first delay
+ else insert(event.timestamp+(unsigned int)_dtn);
+
+ _n++; //increment step counter
+ _pos += (_dir<<1)-1; //set new position +1 if cw; -1 if ccw
+ _clk = 1; //toggle step out pin
- n++; //increment step counter
- _pos += (_dir<<1)-1; //set new position +1 if cw; -1 if ccw
- _clk = !_clk; //toggle step out pin
- _timer.attach_us(this,&Stepper::run,(unsigned int)dtn); //set next delay
- if(_steps && n >= _steps)stop(); //check for motor stop
+ if(_steps && _n >= _steps)stop(); //check for motor stop
}
unsigned int Stepper::nTo(float speed,float acc)
@@ -167,3 +179,5 @@
return (!acc || !speed) ? 0 : (speed * speed) / (2 * acc); //Equation 16 step number n as a function of speed & acceleration
}
+
+
--- a/Stepper.h Mon Apr 15 11:29:15 2013 +0000
+++ b/Stepper.h Mon Sep 22 14:18:31 2014 +0000
@@ -34,9 +34,9 @@
* }
* @endcode
*/
-class Stepper
+class Stepper : public TimerEvent
{
-public:
+public:
/** Create Stepper instance connected to pin clk & dir
* @param clk pin to connect at clk/step output
* @param dir pin to connect at dir output
@@ -112,21 +112,24 @@
typedef enum {CW=1,CCW=0} direction;
protected:
- void run(void); //step generator thread
unsigned int nTo(float speed,float acc);
-
+
private:
- float _acc; //Acceleration [step/s²]
- float _dec; //Decceleration [step/s²]
- float _spd; //Speed [step/s]
- unsigned int _steps; //nbr total of steps per mov
- DigitalOut _clk; //output clock pin
- DigitalOut _dir; //output dir pin
- Timeout _timer; //step timer
+ float _acc; //Acceleration [step/s²]
+ float _dec; //Decceleration [step/s²]
+ float _spd; //Speed [step/s]
+ unsigned int _steps; //nbr total of steps per mov
+ DigitalOut _clk; //output clock pin
+ DigitalOut _dir; //output dir pin
enum {STOP,ACCEL,CRUISE,DECEL}_state; //Motor state
- unsigned int _dt0; //initial delay [µs]
- unsigned int _dtmin; //delay minimum [µs]
- int _pos; //motor position
+ unsigned int _dt0; //initial delay [µs]
+ unsigned int _dtmin; //delay minimum [µs]
+ unsigned int _dtn; //current delay
+ int _pos; //motor position
+ unsigned int _n; //steps counters
+ unsigned int _nStartDec; //steps to decelerate
+ virtual void handler();
+
};
#endif
\ No newline at end of file