AccelStepperI2C  v 0.1
I2C wrapper (and a bit more) for the AccelStepper Arduino library
AccelStepperI2C.cpp
Go to the documentation of this file.
1 
12 #include <Wire.h>
13 #include <AccelStepperI2C.h>
14 #include "I2Cwrapper.h"
15 
16 
17 
18 
19 /*
20  *
21  * AccelStepper wrapper functions start here
22  *
23  */
24 
25 // Constructor
27 {
28  wrapper = w;
29 }
30 
31 
32 void AccelStepperI2C::attach(uint8_t interface,
33  uint8_t pin1, uint8_t pin2, uint8_t pin3, uint8_t pin4,
34  bool enable)
35 {
36  wrapper->prepareCommand(attachCmd); // myNum not know yet
37  wrapper->buf.write(interface); // parameters
38  wrapper->buf.write(pin1);
39  wrapper->buf.write(pin2);
40  wrapper->buf.write(pin3);
41  wrapper->buf.write(pin4);
42  wrapper->buf.write(enable);
43  if (wrapper->sendCommand() and wrapper->readResult(attachResult)) {
44  wrapper->buf.read(myNum);
45  } // else leave myNum at -1 (= failed)
46 }
47 
48 
49 void AccelStepperI2C::moveTo(long absolute)
50 {
51  wrapper->prepareCommand(moveToCmd, myNum);
52  wrapper->buf.write(absolute);
53  wrapper->sendCommand();
54 }
55 
56 
57 void AccelStepperI2C::move(long relative)
58 {
59  wrapper->prepareCommand(moveCmd, myNum);
60  wrapper->buf.write(relative);
61  wrapper->sendCommand();
62 }
63 
64 
65 // don't use this, use state machine instead
67 {
68  wrapper->prepareCommand(runCmd, myNum);
69  boolean res = false; // will be returned on transmission error
70  if (wrapper->sendCommand() and wrapper->readResult(runResult)) {
71  wrapper->buf.read(res); // else return result of function call
72  }
73  return res;
74 }
75 
76 
77 // don't use this, use state machine instead
79 {
80  wrapper->prepareCommand(runSpeedCmd, myNum);
81  boolean res = false; // will be returned on transmission error
82  if (wrapper->sendCommand() and wrapper->readResult(runSpeedResult)) {
83  wrapper->buf.read(res); // else return result of function call
84  }
85  return res;
86 }
87 
88 
89 // don't use this, use state machine instead
91 {
93  boolean res = false; // will be returned on transmission error
94  if (wrapper->sendCommand() and wrapper->readResult(runSpeedToPositionResult)) {
95  wrapper->buf.read(res); // else return result of function call
96  }
97  return res;
98 }
99 
100 
102 {
104  long res = resError; // funny value returned on error
105  if (wrapper->sendCommand() and wrapper->readResult(distanceToGoResult)) {
106  wrapper->buf.read(res); // else return result of function call
107  }
108  return res;
109 }
110 
111 
113 {
115  long res = resError; // funny value returned on error
116  if (wrapper->sendCommand() and wrapper->readResult(targetPositionResult)) {
117  wrapper->buf.read(res); // else return result of function call
118  }
119  return res;
120 }
121 
122 
124 {
126  long res = resError; // funny value returned on error
127  if (wrapper->sendCommand() and wrapper->readResult(currentPositionResult)) {
128  wrapper->buf.read(res); // else return result of function call
129  }
130  return res;
131 }
132 
133 
135 {
137  wrapper->buf.write(position);
138  wrapper->sendCommand();
139 }
140 
141 
143 {
145  wrapper->buf.write(speed);
146  wrapper->sendCommand();
147 }
148 
149 
151 {
152  wrapper->prepareCommand(maxSpeedCmd, myNum);
153  float res = resError; // funny value returned on error
154  if (wrapper->sendCommand() and wrapper->readResult(maxSpeedResult)) {
155  wrapper->buf.read(res);
156  }
157  return res;
158 }
159 
160 
162 {
164  wrapper->buf.write(acceleration);
165  wrapper->sendCommand();
166 }
167 
168 
169 void AccelStepperI2C::setSpeed(float speed)
170 {
171  wrapper->prepareCommand(setSpeedCmd, myNum);
172  wrapper->buf.write(speed);
173  wrapper->sendCommand();
174 }
175 
176 
178 {
179  wrapper->prepareCommand(speedCmd, myNum);
180  float res = resError; // funny value returned on error
181  if (wrapper->sendCommand() and wrapper->readResult(speedResult)) {
182  wrapper->buf.read(res);
183  }
184  return res;
185 }
186 
187 
189 {
191  wrapper->sendCommand();
192 }
193 
194 
196 {
198  wrapper->sendCommand();
199 }
200 
201 
202 void AccelStepperI2C::setMinPulseWidth(unsigned int minWidth)
203 {
205  wrapper->buf.write(minWidth);
206  wrapper->sendCommand();
207 }
208 
209 
211 {
213  wrapper->buf.write(enablePin);
214  wrapper->sendCommand();
215 }
216 
217 
218 void AccelStepperI2C::setPinsInverted(bool directionInvert, bool stepInvert, bool enableInvert)
219 {
221  uint8_t bits = (uint8_t)directionInvert << 0 | (uint8_t)stepInvert << 1 | (uint8_t)enableInvert << 2;
222  wrapper->buf.write(bits);
223  wrapper->sendCommand();
224 }
225 
226 
227 void AccelStepperI2C::setPinsInverted(bool pin1Invert, bool pin2Invert, bool pin3Invert, bool pin4Invert, bool enableInvert)
228 {
230  uint8_t bits = (uint8_t)pin1Invert << 0 | (uint8_t)pin2Invert << 1 | (uint8_t)pin3Invert << 2 | (uint8_t)pin4Invert << 3 | (uint8_t)enableInvert << 4;
231  wrapper->buf.write(bits);
232  wrapper->sendCommand();
233 }
234 
235 // blocking, implemented with state machine
237 {
238  runState(); // start state machine with currently set target
239  while (isRunning()) {
240  delay(100); // this is a bit arbitrary, but given usual motor applications, a precision of 1/10 second should be ok in most situations
241  }
242 }
243 
244 // blocking
246 {
247  moveTo(position);
248  runToPosition();
249 }
250 
251 
253 {
254  wrapper->prepareCommand(stopCmd, myNum);
255  wrapper->sendCommand();
256 }
257 
258 
260 {
261  wrapper->prepareCommand(isRunningCmd, myNum);
262  bool res = false;
263  if (wrapper->sendCommand() and wrapper->readResult(isRunningResult)) {
264  wrapper->buf.read(res);
265  }
266  return res;
267 }
268 
269 
270 
271 /*
272  *
273  * Methods specific to AccelStepperI2C start here
274  *
275  */
276 
277 
278 
280 {
282  wrapper->buf.write(enable);
283  wrapper->sendCommand();
284 }
285 
286 
288 {
290  if (wrapper->sendCommand() and wrapper->readResult(diagnosticsResult)) {
291  wrapper->buf.read(*report); // dereference. I *love* how many uses c++ found for the asterisk...
292  }
293 }
294 
295 
296 
297 void AccelStepperI2C::setState(uint8_t newState)
298 {
299  wrapper->prepareCommand(setStateCmd, myNum);
300  wrapper->buf.write(newState);
301  wrapper->sendCommand();
302 }
303 
304 
306 {
307  wrapper->prepareCommand(getStateCmd, myNum);
308  uint8_t state = -1;
309  if (wrapper->sendCommand() and wrapper->readResult(getStateResult)) {
310  wrapper->buf.read(state);
311  }
312  return state;
313 }
314 
315 
317 {
319 }
320 
321 
323 {
325 }
326 
327 
329 {
331 }
332 
333 
335 {
337 }
338 
339 
341  bool activeLow,
342  bool internalPullup)
343 {
345  wrapper->buf.write(pin);
346  wrapper->buf.write(activeLow);
347  wrapper->buf.write(internalPullup);
348  wrapper->sendCommand();
349 }
350 
351 
353 {
355  wrapper->buf.write(enable);
356  wrapper->sendCommand();
357 }
358 
359 
360 uint8_t AccelStepperI2C::endstops() // returns endstop(s) states in bits 0 and 1
361 {
362  wrapper->prepareCommand(endstopsCmd, myNum);
363  uint8_t res = 0xff; // send "all endstops hit" on transmission error
364  if (wrapper->sendCommand() and wrapper->readResult(endstopsResult)) {
365  wrapper->buf.read(res); // else send real result
366  }
367  return res;
368 }
369 
370 
372 {
374  wrapper->buf.write(enable);
375  wrapper->sendCommand();
376 }
377 
diagnosticsCmd
const uint8_t diagnosticsCmd
Definition: AccelStepperI2C.h:123
resError
const long resError
Definition: AccelStepperI2C.h:79
I2Cwrapper
A helper class for the AccelStepperI2C (and ServoI2C) library.
Definition: I2Cwrapper.h:79
moveToCmd
const uint8_t moveToCmd
Definition: AccelStepperI2C.h:94
AccelStepperI2C::runSpeedToPosition
boolean runSpeedToPosition()
Don't use this, use state machine instead with runSpeedToPositionState().
Definition: AccelStepperI2C.cpp:90
diagnosticsResult
const uint8_t diagnosticsResult
Definition: AccelStepperI2C.h:123
AccelStepperI2C::setPinsInverted
void setPinsInverted(bool directionInvert=false, bool stepInvert=false, bool enableInvert=false)
Definition: AccelStepperI2C.cpp:218
AccelStepperI2C::setState
void setState(uint8_t newState)
Set the state machine's state manually.
Definition: AccelStepperI2C.cpp:297
state_runSpeed
const uint8_t state_runSpeed
corresponds to AccelStepper::runSpeed(), will remain active until stopped by user or endstop
Definition: AccelStepperI2C.h:135
setMinPulseWidthCmd
const uint8_t setMinPulseWidthCmd
Definition: AccelStepperI2C.h:113
AccelStepperI2C::moveTo
void moveTo(long absolute)
Definition: AccelStepperI2C.cpp:49
speedResult
const uint8_t speedResult
Definition: AccelStepperI2C.h:102
AccelStepperI2C::setEnablePin
void setEnablePin(uint8_t enablePin=0xff)
Definition: AccelStepperI2C.cpp:210
AccelStepperI2C::enableOutputs
void enableOutputs()
Definition: AccelStepperI2C.cpp:195
AccelStepperI2C::targetPosition
long targetPosition()
Definition: AccelStepperI2C.cpp:112
currentPositionResult
const uint8_t currentPositionResult
Definition: AccelStepperI2C.h:105
AccelStepperI2C::diagnostics
void diagnostics(diagnosticsReport *report)
Get most recent diagnostics data. Needs diagnostics enabled and a slave which was compiled with the D...
Definition: AccelStepperI2C.cpp:287
speedCmd
const uint8_t speedCmd
Definition: AccelStepperI2C.h:102
isRunningCmd
const uint8_t isRunningCmd
Definition: AccelStepperI2C.h:117
AccelStepperI2C::stopState
void stopState()
Will stop any of the above states, i.e. stop polling. It does nothing else, so the master is solely i...
Definition: AccelStepperI2C.cpp:316
I2Cwrapper::prepareCommand
void prepareCommand(uint8_t cmd, uint8_t unit=-1)
Definition: I2Cwrapper.cpp:34
AccelStepperI2C::runSpeedState
void runSpeedState()
Will poll runSpeed(), i.e. run at constant speed until told otherwise (see AccelStepperI2C::stopState...
Definition: AccelStepperI2C.cpp:328
runSpeedCmd
const uint8_t runSpeedCmd
Definition: AccelStepperI2C.h:97
stopCmd
const uint8_t stopCmd
Definition: AccelStepperI2C.h:110
enableDiagnosticsCmd
const uint8_t enableDiagnosticsCmd
Definition: AccelStepperI2C.h:122
disableOutputsCmd
const uint8_t disableOutputsCmd
Definition: AccelStepperI2C.h:111
AccelStepperI2C::distanceToGo
long distanceToGo()
Definition: AccelStepperI2C.cpp:101
setMaxSpeedCmd
const uint8_t setMaxSpeedCmd
Definition: AccelStepperI2C.h:98
AccelStepperI2C::myNum
int8_t myNum
Stepper number with myNum >= 0 for successfully added steppers.
Definition: AccelStepperI2C.h:383
enableInterruptsCmd
const uint8_t enableInterruptsCmd
Definition: AccelStepperI2C.h:124
AccelStepperI2C::disableOutputs
void disableOutputs()
Definition: AccelStepperI2C.cpp:188
targetPositionResult
const uint8_t targetPositionResult
Definition: AccelStepperI2C.h:104
getStateResult
const uint8_t getStateResult
Definition: AccelStepperI2C.h:126
I2Cwrapper::buf
SimpleBuffer buf
Definition: I2Cwrapper.h:190
AccelStepperI2C::endstops
uint8_t endstops()
Read current state of endstops.
Definition: AccelStepperI2C.cpp:360
currentPositionCmd
const uint8_t currentPositionCmd
Definition: AccelStepperI2C.h:105
AccelStepperI2C::isRunning
bool isRunning()
Definition: AccelStepperI2C.cpp:259
SimpleBuffer::read
void read(T &value)
Read any basic data type from the buffer from the current position and increment the position pointer...
Definition: SimpleBuffer.h:112
endstopsResult
const uint8_t endstopsResult
Definition: AccelStepperI2C.h:129
AccelStepperI2C.h
AccelStepperI2C::setMaxSpeed
void setMaxSpeed(float speed)
Definition: AccelStepperI2C.cpp:142
acceleration
const float acceleration
Definition: Interrupt_Endstop.ino:37
AccelStepperI2C::run
boolean run()
Don't use this, use state machine instead with runState().
Definition: AccelStepperI2C.cpp:66
setPinsInverted1Cmd
const uint8_t setPinsInverted1Cmd
Definition: AccelStepperI2C.h:115
I2Cwrapper.h
A helper class for the AccelStepperI2C (and ServoI2C) library.
runSpeedResult
const uint8_t runSpeedResult
Definition: AccelStepperI2C.h:97
state_run
const uint8_t state_run
corresponds to AccelStepper::run(), will fall back to state_stopped if target reached or endstop hit
Definition: AccelStepperI2C.h:134
AccelStepperI2C::AccelStepperI2C
AccelStepperI2C(I2Cwrapper *w)
Constructor.
Definition: AccelStepperI2C.cpp:26
I2Cwrapper::readResult
bool readResult(uint8_t numBytes)
Definition: I2Cwrapper.cpp:70
getStateCmd
const uint8_t getStateCmd
Definition: AccelStepperI2C.h:126
AccelStepperI2C::enableDiagnostics
void enableDiagnostics(bool enable)
Turn on/off diagnostic speed logging.
Definition: AccelStepperI2C.cpp:279
AccelStepperI2C::runSpeedToPositionState
void runSpeedToPositionState()
Will poll runSpeedToPosition(), i.e. run at constant speed until target has been reached.
Definition: AccelStepperI2C.cpp:334
setEnablePinCmd
const uint8_t setEnablePinCmd
Definition: AccelStepperI2C.h:114
AccelStepperI2C::runToNewPosition
void runToNewPosition(long position)
This is a blocking function in the original, it will only return after the new target has been reache...
Definition: AccelStepperI2C.cpp:245
runSpeedToPositionResult
const uint8_t runSpeedToPositionResult
Definition: AccelStepperI2C.h:108
attachCmd
const uint8_t attachCmd
Definition: AccelStepperI2C.h:121
AccelStepperI2C::currentPosition
long currentPosition()
Definition: AccelStepperI2C.cpp:123
AccelStepperI2C::runState
void runState()
Will poll run(), i.e. run to the target with acceleration and stop the state machine upon reaching it...
Definition: AccelStepperI2C.cpp:322
AccelStepperI2C::setAcceleration
void setAcceleration(float acceleration)
Definition: AccelStepperI2C.cpp:161
setAccelerationCmd
const uint8_t setAccelerationCmd
Definition: AccelStepperI2C.h:100
moveCmd
const uint8_t moveCmd
Definition: AccelStepperI2C.h:95
runCmd
const uint8_t runCmd
Definition: AccelStepperI2C.h:96
setPinsInverted2Cmd
const uint8_t setPinsInverted2Cmd
Definition: AccelStepperI2C.h:116
setCurrentPositionCmd
const uint8_t setCurrentPositionCmd
Definition: AccelStepperI2C.h:106
state_stopped
const uint8_t state_stopped
stepper state machine states
Definition: AccelStepperI2C.h:133
attachResult
const uint8_t attachResult
Definition: AccelStepperI2C.h:121
AccelStepperI2C::runSpeed
boolean runSpeed()
Don't use this, use state machine instead with runSpeedState().
Definition: AccelStepperI2C.cpp:78
diagnosticsReport
Used to transmit diagnostic info with AccelStepperI2C::diagnostics().
Definition: AccelStepperI2C.h:85
endstopsCmd
const uint8_t endstopsCmd
Definition: AccelStepperI2C.h:129
setEndstopPinCmd
const uint8_t setEndstopPinCmd
Definition: AccelStepperI2C.h:127
AccelStepperI2C::setSpeed
void setSpeed(float speed)
Definition: AccelStepperI2C.cpp:169
distanceToGoCmd
const uint8_t distanceToGoCmd
Definition: AccelStepperI2C.h:103
maxSpeedCmd
const uint8_t maxSpeedCmd
Definition: AccelStepperI2C.h:99
state_runSpeedToPosition
const uint8_t state_runSpeedToPosition
corresponds to AccelStepper::state_runSpeedToPosition(), will fall back to state_stopped if target po...
Definition: AccelStepperI2C.h:136
AccelStepperI2C::speed
float speed()
Definition: AccelStepperI2C.cpp:177
isRunningResult
const uint8_t isRunningResult
Definition: AccelStepperI2C.h:117
AccelStepperI2C::maxSpeed
float maxSpeed()
Definition: AccelStepperI2C.cpp:150
targetPositionCmd
const uint8_t targetPositionCmd
Definition: AccelStepperI2C.h:104
enablePin
const uint8_t enablePin
Definition: Interrupt_Endstop.ino:31
AccelStepperI2C::attach
void attach(uint8_t interface=AccelStepper::FULL4WIRE, uint8_t pin1=2, uint8_t pin2=3, uint8_t pin3=4, uint8_t pin4=5, bool enable=true)
Replaces the AccelStepper constructor and takes the same arguments. Will allocate an AccelStepper obj...
Definition: AccelStepperI2C.cpp:32
enableOutputsCmd
const uint8_t enableOutputsCmd
Definition: AccelStepperI2C.h:112
AccelStepperI2C::setCurrentPosition
void setCurrentPosition(long position)
Definition: AccelStepperI2C.cpp:134
AccelStepperI2C::enableInterrupts
void enableInterrupts(bool enable=true)
Start or stop sending interrupts to master for this stepper. An interrupt will be sent whenever a sta...
Definition: AccelStepperI2C.cpp:371
AccelStepperI2C::stop
void stop()
Definition: AccelStepperI2C.cpp:252
AccelStepperI2C::move
void move(long relative)
Definition: AccelStepperI2C.cpp:57
AccelStepperI2C::setEndstopPin
void setEndstopPin(int8_t pin, bool activeLow, bool internalPullup)
Define a new endstop pin. Each stepper can have up to two, so don't call this more than twice per ste...
Definition: AccelStepperI2C.cpp:340
AccelStepperI2C::runToPosition
void runToPosition()
This is a blocking function in the original, it will only return after the target has been reached....
Definition: AccelStepperI2C.cpp:236
setSpeedCmd
const uint8_t setSpeedCmd
Definition: AccelStepperI2C.h:101
AccelStepperI2C::getState
uint8_t getState()
Read the state machine's state (it may have been changed by endstop or target reached condition).
Definition: AccelStepperI2C.cpp:305
setStateCmd
const uint8_t setStateCmd
Definition: AccelStepperI2C.h:125
enableEndstopsCmd
const uint8_t enableEndstopsCmd
Definition: AccelStepperI2C.h:128
runResult
const uint8_t runResult
Definition: AccelStepperI2C.h:96
AccelStepperI2C::setMinPulseWidth
void setMinPulseWidth(unsigned int minWidth)
Definition: AccelStepperI2C.cpp:202
AccelStepperI2C::enableEndstops
void enableEndstops(bool enable=true)
Tell the state machine to check the endstops regularly. If any of the (one or two) stepper's endstops...
Definition: AccelStepperI2C.cpp:352
SimpleBuffer::write
void write(const T &value)
Write any basic data type to the buffer at the current position and increment the position pointer ac...
Definition: SimpleBuffer.h:103
runSpeedToPositionCmd
const uint8_t runSpeedToPositionCmd
Definition: AccelStepperI2C.h:108
maxSpeedResult
const uint8_t maxSpeedResult
Definition: AccelStepperI2C.h:99
I2Cwrapper::sendCommand
bool sendCommand()
Definition: I2Cwrapper.cpp:46
distanceToGoResult
const uint8_t distanceToGoResult
Definition: AccelStepperI2C.h:103