Stepper.cpp

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