Vam GE / Stepper

update

Stepper.cpp@1:a456aa3935ca, 2014-09-22 (annotated)

Committer:
mederic
Date:
Mon Sep 22 14:18:31 2014 +0000
Revision:
1:a456aa3935ca
Parent:
0:f2a3b0be279d 
Correct Bug of 1st version

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