Vam GE / Stepper

update

Stepper.cpp@0:f2a3b0be279d, 2013-04-15 (annotated)

Committer:
mederic
Date:
Mon Apr 15 11:29:15 2013 +0000
Revision:
0:f2a3b0be279d
Child:
1:a456aa3935ca
Published as library

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mederic 0:f2a3b0be279d 1 #include "Stepper.h"
mederic 0:f2a3b0be279d 2
mederic 0:f2a3b0be279d 3 //***********************************/************************************
mederic 0:f2a3b0be279d 4 // Constructors //
mederic 0:f2a3b0be279d 5 //***********************************/************************************
mederic 0:f2a3b0be279d 6 Stepper::Stepper(PinName clk, PinName dir): _clk(clk) , _dir(dir)
mederic 0:f2a3b0be279d 7 {
mederic 0:f2a3b0be279d 8 _state = STOP;
mederic 0:f2a3b0be279d 9 _pos = 0;
mederic 0:f2a3b0be279d 10 _steps = 0;
mederic 0:f2a3b0be279d 11 _spd = 400;
mederic 0:f2a3b0be279d 12 }
mederic 0:f2a3b0be279d 13
mederic 0:f2a3b0be279d 14 //***********************************/************************************
mederic 0:f2a3b0be279d 15 // Get Set //
mederic 0:f2a3b0be279d 16 //***********************************/************************************
mederic 0:f2a3b0be279d 17 void Stepper::setSpeed(float speed)
mederic 0:f2a3b0be279d 18 {
mederic 0:f2a3b0be279d 19 _spd = (speed<0) ? -speed : speed; //speed must be unsigned
mederic 0:f2a3b0be279d 20 if(_spd)_dtmin = 1000000/_spd; //fin min delay (max spd)
mederic 0:f2a3b0be279d 21 }
mederic 0:f2a3b0be279d 22
mederic 0:f2a3b0be279d 23 float Stepper::getSpeed(void)
mederic 0:f2a3b0be279d 24 {
mederic 0:f2a3b0be279d 25 return _spd;
mederic 0:f2a3b0be279d 26 }
mederic 0:f2a3b0be279d 27
mederic 0:f2a3b0be279d 28 void Stepper::setAcceleration(float acc)
mederic 0:f2a3b0be279d 29 {
mederic 0:f2a3b0be279d 30 _acc = (acc<0) ? -acc : acc; //acceleration must be unsigned
mederic 0:f2a3b0be279d 31 if(_acc)_dt0 = 676000 * sqrt(2.0/_acc); //Equation 15 [µs] instead Equation 7
mederic 0:f2a3b0be279d 32 }
mederic 0:f2a3b0be279d 33
mederic 0:f2a3b0be279d 34 float Stepper::getAcceleration(void)
mederic 0:f2a3b0be279d 35 {
mederic 0:f2a3b0be279d 36 return _acc;
mederic 0:f2a3b0be279d 37 }
mederic 0:f2a3b0be279d 38
mederic 0:f2a3b0be279d 39 void Stepper::setDeceleration(float dec)
mederic 0:f2a3b0be279d 40 {
mederic 0:f2a3b0be279d 41 _dec = (dec<0) ? -dec : dec; //deceleration must be unsigned
mederic 0:f2a3b0be279d 42 }
mederic 0:f2a3b0be279d 43
mederic 0:f2a3b0be279d 44 float Stepper::getDeceleration(void)
mederic 0:f2a3b0be279d 45 {
mederic 0:f2a3b0be279d 46 return _dec;
mederic 0:f2a3b0be279d 47 }
mederic 0:f2a3b0be279d 48
mederic 0:f2a3b0be279d 49 void Stepper::setPositionZero(void)
mederic 0:f2a3b0be279d 50 {
mederic 0:f2a3b0be279d 51 _pos = 0;
mederic 0:f2a3b0be279d 52 }
mederic 0:f2a3b0be279d 53
mederic 0:f2a3b0be279d 54 int Stepper::getPosition(void)
mederic 0:f2a3b0be279d 55 {
mederic 0:f2a3b0be279d 56 return _pos;
mederic 0:f2a3b0be279d 57 }
mederic 0:f2a3b0be279d 58
mederic 0:f2a3b0be279d 59 bool Stepper::stopped(void)
mederic 0:f2a3b0be279d 60 {
mederic 0:f2a3b0be279d 61 return (_state == STOP) ? true : false;
mederic 0:f2a3b0be279d 62 }
mederic 0:f2a3b0be279d 63
mederic 0:f2a3b0be279d 64 //***********************************/************************************
mederic 0:f2a3b0be279d 65 // Public Methods //
mederic 0:f2a3b0be279d 66 //***********************************/************************************
mederic 0:f2a3b0be279d 67 void Stepper::stop(void)
mederic 0:f2a3b0be279d 68 {
mederic 0:f2a3b0be279d 69 _timer.detach(); //detach step generator thread
mederic 0:f2a3b0be279d 70 _state = STOP; //update state machine
mederic 0:f2a3b0be279d 71 _steps = 0; //reset total steps per move
mederic 0:f2a3b0be279d 72 }
mederic 0:f2a3b0be279d 73
mederic 0:f2a3b0be279d 74 void Stepper::rotate(bool direction)
mederic 0:f2a3b0be279d 75 {
mederic 0:f2a3b0be279d 76 if(!_spd)return; //spd must > 0
mederic 0:f2a3b0be279d 77 _dir = direction; //set output pin direction value
mederic 0:f2a3b0be279d 78 _steps = 0; //rotate until stop() by user
mederic 0:f2a3b0be279d 79 run(); //start thread
mederic 0:f2a3b0be279d 80 }
mederic 0:f2a3b0be279d 81
mederic 0:f2a3b0be279d 82 void Stepper::move(int steps)
mederic 0:f2a3b0be279d 83 {
mederic 0:f2a3b0be279d 84 if(!steps || !_spd) return;
mederic 0:f2a3b0be279d 85 if(steps<0) //fin direction
mederic 0:f2a3b0be279d 86 {
mederic 0:f2a3b0be279d 87 _dir = CCW; //set output pin direction value
mederic 0:f2a3b0be279d 88 _steps = -steps; //total steps per move must be unsigned
mederic 0:f2a3b0be279d 89 }
mederic 0:f2a3b0be279d 90 else
mederic 0:f2a3b0be279d 91 {
mederic 0:f2a3b0be279d 92 _dir = CW; //set output pin direction value
mederic 0:f2a3b0be279d 93 _steps = steps; //total steps per move
mederic 0:f2a3b0be279d 94 }
mederic 0:f2a3b0be279d 95 run();
mederic 0:f2a3b0be279d 96 }
mederic 0:f2a3b0be279d 97
mederic 0:f2a3b0be279d 98 void Stepper::goesTo(int position)
mederic 0:f2a3b0be279d 99 {
mederic 0:f2a3b0be279d 100 move(position-_pos); //absolute to relative transformation
mederic 0:f2a3b0be279d 101 }
mederic 0:f2a3b0be279d 102
mederic 0:f2a3b0be279d 103 //***********************************/************************************
mederic 0:f2a3b0be279d 104 // Protected Methods //
mederic 0:f2a3b0be279d 105 //***********************************/************************************
mederic 0:f2a3b0be279d 106 void Stepper::run(void)
mederic 0:f2a3b0be279d 107 {
mederic 0:f2a3b0be279d 108 static float dtn; //delay for next step
mederic 0:f2a3b0be279d 109 static unsigned int nStartDec; //steps to start decelerate
mederic 0:f2a3b0be279d 110 static unsigned int n; //steps counter
mederic 0:f2a3b0be279d 111
mederic 0:f2a3b0be279d 112 switch(_state)
mederic 0:f2a3b0be279d 113 {
mederic 0:f2a3b0be279d 114 case STOP:
mederic 0:f2a3b0be279d 115 n = 0; //reset setp counter (motor stopped)
mederic 0:f2a3b0be279d 116
mederic 0:f2a3b0be279d 117 if(_dt0 <= _dtmin || !_acc) //if first step faster than max speed step
mederic 0:f2a3b0be279d 118 {
mederic 0:f2a3b0be279d 119 dtn = _dtmin; //delay = delaymin
mederic 0:f2a3b0be279d 120 _state = CRUISE; //no acceleration needed
mederic 0:f2a3b0be279d 121 }
mederic 0:f2a3b0be279d 122 else
mederic 0:f2a3b0be279d 123 {
mederic 0:f2a3b0be279d 124 dtn = _dt0; //set first delay
mederic 0:f2a3b0be279d 125 _state = ACCEL; //acceleration phase
mederic 0:f2a3b0be279d 126 }
mederic 0:f2a3b0be279d 127
mederic 0:f2a3b0be279d 128 if(_steps) //if finite mov required
mederic 0:f2a3b0be279d 129 {
mederic 0:f2a3b0be279d 130 unsigned int nToSpeed = nTo(_spd,_acc); //Equation 16 How many steps to reach max speed
mederic 0:f2a3b0be279d 131 nStartDec = (_steps * _dec) / (_dec + _acc); //Equation 19 after how many step we must start decelerate
mederic 0:f2a3b0be279d 132 if(nStartDec > nToSpeed)nStartDec = _steps - ((nToSpeed*_acc)/_dec); //if speed can be reach Equation 17
mederic 0:f2a3b0be279d 133 }
mederic 0:f2a3b0be279d 134 break;
mederic 0:f2a3b0be279d 135
mederic 0:f2a3b0be279d 136 case ACCEL:
mederic 0:f2a3b0be279d 137 dtn -= (dtn*2) / ((n<<2)+1); //Equation 20 find next delay
mederic 0:f2a3b0be279d 138 if((unsigned int)dtn <= _dtmin) //if max speed reached
mederic 0:f2a3b0be279d 139 {
mederic 0:f2a3b0be279d 140 dtn = _dtmin;
mederic 0:f2a3b0be279d 141 _state = CRUISE; //constant phase
mederic 0:f2a3b0be279d 142 }
mederic 0:f2a3b0be279d 143 if(_steps && _dec && n >= nStartDec)_state = DECEL; //chech when must start decelerate
mederic 0:f2a3b0be279d 144
mederic 0:f2a3b0be279d 145 break;
mederic 0:f2a3b0be279d 146
mederic 0:f2a3b0be279d 147 case CRUISE:
mederic 0:f2a3b0be279d 148 if(_steps && _dec && n >= nStartDec)_state = DECEL; //chech when must start decelerate
mederic 0:f2a3b0be279d 149 break;
mederic 0:f2a3b0be279d 150
mederic 0:f2a3b0be279d 151 case DECEL:
mederic 0:f2a3b0be279d 152 dtn += (dtn*2) / (((_steps-n)<<2)+1); //Equation 20 find next delay
mederic 0:f2a3b0be279d 153 break;
mederic 0:f2a3b0be279d 154 }
mederic 0:f2a3b0be279d 155
mederic 0:f2a3b0be279d 156 n++; //increment step counter
mederic 0:f2a3b0be279d 157 _pos += (_dir<<1)-1; //set new position +1 if cw; -1 if ccw
mederic 0:f2a3b0be279d 158 _clk = !_clk; //toggle step out pin
mederic 0:f2a3b0be279d 159 _timer.attach_us(this,&Stepper::run,(unsigned int)dtn); //set next delay
mederic 0:f2a3b0be279d 160 if(_steps && n >= _steps)stop(); //check for motor stop
mederic 0:f2a3b0be279d 161 }
mederic 0:f2a3b0be279d 162
mederic 0:f2a3b0be279d 163 unsigned int Stepper::nTo(float speed,float acc)
mederic 0:f2a3b0be279d 164 {
mederic 0:f2a3b0be279d 165 if(speed<0)speed = -speed;
mederic 0:f2a3b0be279d 166 if(acc<0)acc = -acc;
mederic 0:f2a3b0be279d 167
mederic 0:f2a3b0be279d 168 return (!acc || !speed) ? 0 : (speed * speed) / (2 * acc); //Equation 16 step number n as a function of speed & acceleration
mederic 0:f2a3b0be279d 169 }