Marco Newman / CartesianRobot

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependents:   CartesianRobot_Demo

Stepper.cpp@1:0b705e9f84ce, 2021-12-16 (annotated)

Committer:
marcoanewman
Date:
Thu Dec 16 04:33:27 2021 +0000
Revision:
1:0b705e9f84ce
Parent:
0:5af6295f4ec9 
consistency fixes

Who changed what in which revision?

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