Accessories
Arduino for motors and lights library.
LocoStepper.cpp
1 #include "Accessories.h"
2 
3 #ifndef NO_STEPPER
4 #include "LocoStepper.h"
5 
6 uint8_t step2default[4] =
7 {
8  B01,
9  B11,
10  B10,
11  B00,
12 };
13 
14 uint8_t step4default[4] =
15 {
16  B1010,
17  B0110,
18  B0101,
19  B1001,
20 };
21 LocoStepper::LocoStepper(uint8_t pin1, uint8_t pin2, uint8_t *pStep2) : LocoStepper(pin1, pin2, 255, 255)
22 {
23  if (pStep2 == NULL)
24  this->pSteps = step2default;
25  else
26  this->pSteps = pStep2;
27 }
28 
29 LocoStepper::LocoStepper(uint8_t pin1, uint8_t pin2, uint8_t pin3, uint8_t pin4, uint8_t *pStep4)
30 {
31  _currentPos = 0;
32  _targetPos = 0;
33  _speed = 0.0;
34  _stepInterval = 0;
35  _lastRunTime = 0;
36  _minPulseWidth = 1;
37  _lastStepTime = 0;
38  _pin1 = pin1;
39  _pin2 = pin2;
40  _pin3 = pin3;
41  _pin4 = pin4;
42  if (pStep4 == NULL)
43  this->pSteps = step4default;
44  else
45  this->pSteps = pStep4;
46  enableOutputs();
47 }
48 
49 void LocoStepper::moveTo(long absolute)
50 {
51  _targetPos = absolute;
52 
53  long distanceTo = distanceToGo();
54 
55  if (distanceTo == 0)
56  setSpeed(0.0f); // We're there
57  if (distanceTo > 0) // Clockwise
58  setSpeed((float) fabs(_speed));
59  else
60  setSpeed((float) -fabs(_speed));
61 }
62 
63 void LocoStepper::move(long relative)
64 {
65  moveTo(_currentPos + relative);
66 }
67 
68 // Run the motor to implement speed and acceleration in order to proceed to the target position
69 // You must call this at least once per step, preferably in your main loop
70 // If the motor is in the desired position, the cost is very small
71 // returns true if we are still running to position
73 {
74  if (_targetPos == _currentPos)
75  return false;
76 
77  unsigned long time = micros();
78 
79  if ((time >= (_lastStepTime + _stepInterval)) // okay if both current time and next step time wrap
80  || ((time < _lastRunTime) && (time >(0xFFFFFFFF - (_lastStepTime + _stepInterval))))) // check if only current time has wrapped
81  {
82  if (_speed > 0.0f)
83  {
84  // Clockwise
85  _currentPos += 1;
86  }
87  else
88  if (_speed < 0.0f)
89  {
90  // Anticlockwise
91  _currentPos -= 1;
92  }
93 
94  if (this->_pin3 == 255)
95  step2(_currentPos & 0x3); // Bottom 2 bits (same as mod 4, but works with + and - numbers)
96  else
97  step4(_currentPos & 0x3); // Bottom 2 bits (same as mod 4, but works with + and - numbers)
98 
99  _lastRunTime = time;
100  _lastStepTime = time;
101  }
102  else
103  _lastRunTime = time;
104 
105  return true;
106 }
107 
109 {
110  return _targetPos - _currentPos;
111 }
112 
114 {
115  return _targetPos;
116 }
117 
119 {
120  return _currentPos;
121 }
122 
123 // Useful during initializations or after initial positioning
125 {
126  _currentPos = position;
127 }
128 
129 void LocoStepper::setSpeed(float speed)
130 {
131  _speed = speed;
132  _stepInterval = (unsigned long) fabs(1000000.0 / _speed);
133 }
134 
136 {
137  return _speed;
138 }
139 
140 // 2 pin step function
141 // This is passed the current step number (0 to 3)
142 // Subclasses can override
143 void LocoStepper::step2(uint8_t step)
144 {
145  if ((this->pSteps[step] & B10) > 0)
146  digitalWrite(_pin1, HIGH);
147  else
148  digitalWrite(_pin1, LOW);
149 
150  if ((this->pSteps[step] & B01) > 0)
151  digitalWrite(_pin2, HIGH);
152  else
153  digitalWrite(_pin2, LOW);
154 }
155 
156 // 4 pin step function
157 // This is passed the current step number (0 to 3)
158 // Subclasses can override
159 void LocoStepper::step4(uint8_t step)
160 {
161  if ((this->pSteps[step] & B1000) > 0)
162  digitalWrite(_pin1, HIGH);
163  else
164  digitalWrite(_pin2, LOW);
165  if ((this->pSteps[step] & B0100) > 0)
166  digitalWrite(_pin2, HIGH);
167  else
168  digitalWrite(_pin3, LOW);
169  if ((this->pSteps[step] & B0010) > 0)
170  digitalWrite(_pin3, HIGH);
171  else
172  digitalWrite(_pin4, LOW);
173  if ((this->pSteps[step] & B0001) > 0)
174  digitalWrite(_pin4, HIGH);
175  else
176  digitalWrite(_pin1, LOW);
177 }
178 
179 
180 // Prevents power consumption on the outputs
182 {
183  digitalWrite(_pin1, LOW);
184  digitalWrite(_pin2, LOW);
185  if (_pin3 != 255)
186  digitalWrite(_pin3, LOW);
187  if (_pin4 != 255)
188  digitalWrite(_pin4, LOW);
189 }
190 
192 {
193  pinMode(_pin1, OUTPUT);
194  pinMode(_pin2, OUTPUT);
195  if (_pin3 != 255)
196  pinMode(_pin3, OUTPUT);
197  if (_pin4 != 255)
198  pinMode(_pin4, OUTPUT);
199 }
200 
201 void LocoStepper::setMinPulseWidth(unsigned int minWidth)
202 {
203  _minPulseWidth = minWidth;
204 }
205 #endif
void setMinPulseWidth(unsigned int minWidth)
void disableOutputs()
float speed()
void setCurrentPosition(long position)
long currentPosition()
long targetPosition()
void step2(uint8_t step)
boolean run()
Definition: LocoStepper.cpp:72
void move(long relative)
Definition: LocoStepper.cpp:63
void setSpeed(float speed)
long distanceToGo()
void moveTo(long absolute)
Definition: LocoStepper.cpp:49
void enableOutputs()
Support for stepper motors with acceleration etc.
Definition: LocoStepper.h:81
LocoStepper(uint8_t pin1, uint8_t pin2, uint8_t *inpStep2 = NULL)
Definition: LocoStepper.cpp:21
void step4(uint8_t step)