Stepper.cpp

00001 #include "Stepper.h"
00002 
00003 //***********************************/************************************
00004 //                         Constructors                                 //
00005 //***********************************/************************************
00006 Stepper::Stepper(PinName clk, PinName dir): _clk(clk) , _dir(dir)
00007 {
00008     _state = STOP;
00009     _pos = 0;
00010     _steps = 0;
00011     _spd = 400;
00012 }
00013 
00014 //***********************************/************************************
00015 //                                Get Set                               //
00016 //***********************************/************************************
00017 void Stepper::setSpeed(float speed)
00018 {
00019     _spd = (speed<0) ? -speed : speed;  //speed must be unsigned
00020     if(_spd)_dtmin = 1000000/_spd;      //fin min delay (max spd)
00021 }
00022 
00023 float Stepper::getSpeed(void)
00024 {
00025     return _spd;
00026 }
00027 
00028 void Stepper::setAcceleration(float acc)
00029 {
00030     _acc = (acc<0) ? -acc : acc;            //acceleration must be unsigned
00031     if(_acc)_dt0 = 676000 * sqrt(2.0/_acc); //Equation 15 [µs] instead Equation 7
00032 }
00033 
00034 float Stepper::getAcceleration(void)
00035 {
00036     return _acc;
00037 }
00038 
00039 void Stepper::setDeceleration(float dec)
00040 {
00041     _dec = (dec<0) ? -dec : dec;        //deceleration must be unsigned
00042 }
00043 
00044 float Stepper::getDeceleration(void)
00045 {
00046     return _dec;
00047 }
00048 
00049 void Stepper::setPositionZero(void)
00050 {
00051     _pos = 0;
00052 }
00053 
00054 int Stepper::getPosition(void)
00055 {
00056     return _pos;
00057 }
00058 
00059 bool Stepper::stopped(void)
00060 {
00061     return (_state == STOP) ? true : false;
00062 }
00063 
00064 //***********************************/************************************
00065 //                             Public Methods                           //
00066 //***********************************/************************************
00067 void Stepper::stop(void)
00068 {
00069     _timer.detach();    //detach step generator thread
00070     _state = STOP;      //update state machine 
00071     _steps = 0;         //reset total steps per move
00072 }
00073 
00074 void Stepper::rotate(bool direction)
00075 {
00076     if(!_spd)return;    //spd must > 0
00077     _dir = direction;   //set output pin direction value
00078     _steps = 0;         //rotate until stop() by user
00079     run();              //start thread
00080 }
00081 
00082 void Stepper::move(int steps)
00083 {
00084     if(!steps || !_spd) return;
00085     if(steps<0) //fin direction
00086     {
00087         _dir = CCW;         //set output pin direction value
00088         _steps = -steps;    //total steps per move must be unsigned
00089     }
00090     else
00091     {
00092         _dir = CW;          //set output pin direction value
00093         _steps = steps;     //total steps per move
00094     }
00095     run();
00096 }
00097 
00098 void Stepper::goesTo(int position)
00099 {
00100     move(position-_pos);    //absolute to relative transformation   
00101 }
00102 
00103 //***********************************/************************************
00104 //                          Protected Methods                           //
00105 //***********************************/************************************
00106 void Stepper::run(void)
00107 {
00108     static float dtn;                           //delay for next step
00109     static unsigned int nStartDec;              //steps to start decelerate
00110     static unsigned int n;                      //steps counter
00111   
00112     switch(_state)
00113     {
00114         case STOP:
00115             n = 0;                      //reset setp counter (motor stopped)
00116   
00117             if(_dt0 <= _dtmin || !_acc) //if first step faster than max speed step
00118             {
00119                 dtn = _dtmin;       //delay = delaymin
00120                 _state = CRUISE;    //no acceleration needed
00121             }
00122             else
00123             {
00124                 dtn = _dt0;         //set first delay
00125                 _state = ACCEL;     //acceleration phase
00126             }
00127 
00128             if(_steps)  //if finite mov required
00129             {
00130                 unsigned int nToSpeed = nTo(_spd,_acc);      //Equation 16 How many steps to reach max speed 
00131                 nStartDec = (_steps * _dec) / (_dec + _acc);   //Equation 19 after how many step we must start decelerate  
00132                 if(nStartDec > nToSpeed)nStartDec = _steps - ((nToSpeed*_acc)/_dec);  //if speed can be reach Equation 17                
00133             }    
00134         break;
00135         
00136         case ACCEL:
00137             dtn -=  (dtn*2) / ((n<<2)+1);   //Equation 20 find next delay
00138             if((unsigned int)dtn <= _dtmin) //if max speed reached
00139             {
00140                  dtn = _dtmin;
00141                 _state = CRUISE;    //constant phase
00142             }
00143             if(_steps && _dec && n >= nStartDec)_state = DECEL; //chech when must start decelerate
00144             
00145         break;
00146         
00147         case CRUISE:
00148             if(_steps && _dec && n >= nStartDec)_state = DECEL; //chech when must start decelerate
00149         break;
00150         
00151         case DECEL:
00152             dtn +=  (dtn*2) / (((_steps-n)<<2)+1);  //Equation 20 find next delay
00153         break;    
00154     }
00155 
00156     n++;                    //increment step counter
00157     _pos += (_dir<<1)-1;    //set new position +1 if cw; -1 if ccw
00158     _clk = !_clk;           //toggle step out pin
00159     _timer.attach_us(this,&Stepper::run,(unsigned int)dtn); //set next delay
00160     if(_steps && n >= _steps)stop();    //check for motor stop
00161 }
00162 
00163 unsigned int Stepper::nTo(float speed,float acc)
00164 {
00165     if(speed<0)speed = -speed;
00166     if(acc<0)acc = -acc;
00167     
00168     return (!acc || !speed) ? 0 : (speed * speed) / (2 * acc); //Equation 16 step number n as a function of speed & acceleration
00169 }