Diff: Stepper.cpp

Revision:

1:a456aa3935ca

Parent:

0:f2a3b0be279d

diff -r f2a3b0be279d -r a456aa3935ca Stepper.cpp
--- 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
 }
+
+