AccelStepperI2C  v 0.1
I2C wrapper (and a bit more) for the AccelStepper Arduino library
firmware.ino
Go to the documentation of this file.
1 
21 #include <Arduino.h>
22 #include <Wire.h>
23 #include <AccelStepper.h>
24 #include <AccelStepperI2C.h>
25 #include <SimpleBuffer.h>
26 #include <EEPROM.h>
27 #if defined(__AVR__)
28 #include <avr/wdt.h>
29 #endif
30 
31 
32 
33 /************************************************************************/
34 /************* firmware configuration settings **************************/
35 /************************************************************************/
36 
37 /*
38  I2C address
39  Change this if you want to flash the firmware with a different I2C address.
40  Alternatively, you can use setI2Caddress() from the library to change it later
41 */
42 const uint8_t slaveDefaultAddress = 0x08; // default
43 
44 /*
45  uncomment and define max. number of servos
46  to enable servo support
47 */
48 #define SERVO_SUPPORT
49 const uint8_t maxServos = 4;
50 
51 
55 #define DEBUG
56 
57 
64 //#define DIAGNOSTICS
65 
66 
67 /************************************************************************/
68 /******* end of firmware configuration settings **************************/
69 /************************************************************************/
70 
71 /*
72  Servo stuff
73 */
74 
75 #if defined(SERVO_SUPPORT)
76 
77 #if defined(__AVR__) || defined(ESP8266)
78 #include <Servo.h>
79 #elif defined(ESP32)
80 #include <ESP32Servo.h> // ESP32 doesn't come with a native Servo.h
81 #endif // defined(__AVR__)
82 #include "ServoI2C.h"
83 
84 Servo servos[maxServos]; // ### really allocate all of them now?
85 uint8_t numServos = 0; // number of initialised servos.
86 
87 #endif // defined(SERVO_SUPPORT)
88 
89 /*
90  Debugging stuff
91 */
92 
93 #ifdef DEBUG
94 #define log(...) Serial.print(__VA_ARGS__)
95 volatile uint8_t writtenToBuffer = 0;
96 volatile uint8_t sentOnRequest = 0;
97 #else
98 #define log(...)
99 #endif // DEBUG
100 
101 #ifdef DEBUG
102 uint32_t now, then = millis();
103 bool reportNow = true;
104 uint32_t lastCycles = 0; // for simple cycles/reportPeriod diagnostics
105 const uint32_t reportPeriod = 2000; // ms between main loop simple diagnostics output
106 #endif // DEBUG
107 
108 
109 /*
110  Diagnostics stuff
111 */
112 
113 #ifdef DIAGNOSTICS
114 bool diagnosticsEnabled = false;
115 uint32_t thenMicros;
116 diagnosticsReport currentDiagnostics;
117 uint32_t previousLastReceiveTime; // used to delay receive times by one receive event
118 #endif // DIAGNOSTICS
119 
120 uint32_t cycles = 0; // keeps count of main loop iterations
121 
122 
123 /*
124  EEPROM stuff
125 */
126 
127 #define EEPROM_OFFSET_I2C_ADDRESS 0 // where in eeprom is the I2C address stored, if any? [1 CRC8 + 4 marker + 1 address = 6 bytes]
128 const uint32_t eepromI2CaddressMarker = 0x12C0ACCF; // arbitrary 32bit marker proving that next byte in EEPROM is in fact an I2C address
129 #if defined(ESP32) || defined(ESP8266)
130 const uint32_t eepromUsedSize = 6; // total bytes of EEPROM used by us
131 #endif // defined(ESP32) || defined(ESP8266)
132 
133 
134 /*
135  I2C stuff
136 */
137 
140 volatile uint8_t newMessage = 0; // signals main loop that a receiveEvent with valid new data occurred
141 
142 
147 {
148  SimpleBuffer b;
149  b.init(8);
150  // read 6 bytes from eeprom: [0]=CRC8; [1-4]=marker; [5]=address
151  //log("Reading from EEPROM: ");
152 #if defined(ESP32) || defined(ESP8266)
153  EEPROM.begin(256);
154 #endif // defined(ESP32) || defined(ESP8266)
155  for (byte i = 0; i < 6; i++) {
156  b.buffer[i] = EEPROM.read(EEPROM_OFFSET_I2C_ADDRESS + i);
157  // log(b.buffer[i]); log (" ");
158  }
159 #if defined(ESP32) || defined(ESP8266)
160  EEPROM.end();
161 #endif // defined(ESP32) || defined(ESP8266)
162  log("\n");
163  // we wrote directly to the buffer, so reader idx is still at 1
164  uint32_t markerTest; b.read(markerTest);
165  uint8_t storedAddress; b.read(storedAddress); // now idx will be at 6, so that CRC check below will work
166  if (b.checkCRC8() and (markerTest == eepromI2CaddressMarker)) {
167  return storedAddress;
168  } else {
169  return slaveDefaultAddress;
170  }
171 }
172 
173 
179 {
180  SimpleBuffer b;
181  b.init(8);
183  b.write(newAddress);
184  b.setCRC8();
185  log("Writing to EEPROM: ");
186 #if defined(ESP32) || defined(ESP8266)
187  EEPROM.begin(32);
188 #endif // defined(ESP32) || defined(ESP8266)
189  for (byte i = 0; i < 6; i++) { // [0]=CRC8; [1-4]=marker; [5]=address
190  EEPROM.write(EEPROM_OFFSET_I2C_ADDRESS + i, b.buffer[i]);
191  //EEPROM.put(EEPROM_OFFSET_I2C_ADDRESS + i, b.buffer[i]);
192  //delay(120);
193  log(b.buffer[i]); log (" ");
194  }
195 #if defined(ESP32) || defined(ESP8266)
196  EEPROM.end(); // end() will also commit()
197 #endif // defined(ESP32) || defined(ESP8266)
198  log("\n");
199 }
200 
201 
202 /*
203  Interrupt (to master) stuff
204  note: The interrupt pin is global, as there is only one shared by all steppers.
205  However, interrupts can be enabled for each stepper seperately.
206 */
207 
208 int8_t interruptPin = -1; // -1 for undefined
210 uint8_t interruptSource = 0xF;
212 
213 /*
214  Endstop stuff
215 */
216 
217 struct Endstop
218 {
219  uint8_t pin;
220  bool activeLow;
221  //bool internalPullup; // we don't need to store this, we can directly use it when adding the pin
222 };
223 const uint8_t maxEndstops = 2; // not sure if there are scenarios where more than two make sense, but why not be prepared and make this configurable?
224 const uint32_t endstopDebouncePeriod = 5; // millisecends to keep between triggering endstop interrupts; I measured a couple of switches, none bounced longer than 1 ms so this should be more than safe
225 
226 /*
227  Stepper stuff
228 */
229 
230 const uint8_t maxSteppers = 8;
231 uint8_t numSteppers = 0; // number of initialised steppers
232 
236 struct Stepper
237 {
238  AccelStepper* stepper;
239  uint8_t state = state_stopped;
241  uint8_t numEndstops = 0;
242  bool interruptsEnabled = false;
243  bool endstopsEnabled = false;
244  uint8_t prevEndstopState; // needed for detecting rising and falling flanks
245  uint32_t endstopDebounceEnd = 0; // used for debouncing, endstops are ignored after a new flank until this time is reached
246 };
248 
249 
250 /*
251  Reset stuff
252 */
253 
254 void resetFunc()
255 {
256 #if defined(__AVR__)
257  // The watchdog method supposedly is the preferred one, however it did not make my Nano restart normally,
258  // I think maybe it got stuck in the bootloader.
259  // cli();
260  // wdt_enable(WDTO_30MS);
261  // for (;;);
262  // so let's just go for the stupid version, it should work fine for the firmware:
263  asm volatile (" jmp 0");
264 #elif defined(ESP32) || defined(ESP8266)
265  ESP.restart();
266 #endif
267 }
268 
269 
270 /**************************************************************************/
275 /**************************************************************************/
276 void setup()
277 {
278 
279 #ifdef DEBUG
280  Serial.begin(115200);
281 #endif
282  log("\n\n\n=== AccelStepperI2C v");
284  log(" ===\n\n");
285 
286  uint8_t i2c_address = retrieveI2C_address();
287  Wire.begin(i2c_address);
288  log("I2C started with address "); log(i2c_address); log("\n\n");
289  Wire.onReceive(receiveEvent);
290  Wire.onRequest(requestEvent);
291 
294 
295 }
296 
297 /**************************************************************************/
304 /**************************************************************************/
305 int8_t addStepper(uint8_t interface = AccelStepper::FULL4WIRE,
306  uint8_t pin1 = 2,
307  uint8_t pin2 = 3,
308  uint8_t pin3 = 4,
309  uint8_t pin4 = 5,
310  bool enable = true)
311 {
312  if (numSteppers < maxSteppers) {
313  steppers[numSteppers].stepper = new AccelStepper(interface, pin1, pin2, pin3, pin4, enable);
315  log("Add stepper with internal myNum = "); log(numSteppers);
316  return numSteppers++;
317  } else {
318  log("-- Too many steppers, failed to add new one\n");
319  return -1;
320  }
321 }
322 
323 
324 /*
325  Read endstop(s) for stepper s.
326  Returns bit pattern, last added endstop is LSB; 0 for inactive, 1 for active (taking activeLow in account)
327  Returns 0x0 if no endstops set.
328 */
329 
330 uint8_t pollEndstops(uint8_t s)
331 {
332  uint8_t res = 0;
333  for (uint8_t i = 0; i < steppers[s].numEndstops; i++) {
334  //log(i); log(" p");
335  //log(digitalRead(steppers[s].endstops[i].pin)); log(" ");
336  res = (res << 1) | (digitalRead(steppers[s].endstops[i].pin) ^ steppers[s].endstops[i].activeLow); // xor
337  }
338  //log(" res="); log(res); log(" ");
339  return res;
340 }
341 
342 
343 /*
344  Interrupt master if interrupts are enabled for the source stepper.
345 */
346 
347 void triggerInterrupt(uint8_t source, uint8_t reason)
348 {
349  if (steppers[source].interruptsEnabled and interruptPin >= 0) {
350  log("~~ interrupt master with source = "); log(source);
351  log(" and reason = "); log(reason); log("\n");
352  digitalWrite(interruptPin, interruptActiveHigh ? HIGH : LOW);
353  interruptSource = source;
354  interruptReason = reason;
355  }
356 }
357 
359 {
360  if (interruptPin >= 0) {
361  digitalWrite(interruptPin, interruptActiveHigh ? LOW : HIGH);
362  }
363 }
364 
365 /**************************************************************************/
374 /**************************************************************************/
375 
376 void loop()
377 {
378 
379 #ifdef DEBUG
380  now = millis();
381  if (now > then) { // report cycles/reportPeriod statistics and state machine states for all defined steppers
382  reportNow = true;
383  log("\n > Cycles/s = "); log((cycles - lastCycles) * 1000 / reportPeriod);
384  //log("Cycles = "); log(cycles);
385  if (numSteppers > 0) {
386  log(" | [Steppers]:states =");
387  }
388  lastCycles = cycles;
389  then = now + reportPeriod;
390  }
391 #endif
392 
393  for (uint8_t i = 0; i < numSteppers; i++) { // cycle through all defined steppers
394 
395 #ifdef DEBUG
396  if (reportNow) {
397  log(" ["); log(i); log("]:"); log(steppers[i].state);
398  }
399 #endif
400 
401  bool timeToCheckTheEndstops = false; // ###todo: change polling to pinchange interrupt
402  // ### do we need this at all? Why not just poll each cycle? It doesn't take very long.
403  switch (steppers[i].state) {
404 
405  case state_run: // boolean AccelStepper::run
406  if (not steppers[i].stepper->run()) { // target reached?
409  }
410  timeToCheckTheEndstops = true; // we cannot tell if there was a step, so we'll have to check every time. (one more reason to do it with interrupts)
411  break;
412 
413  case state_runSpeed: // boolean AccelStepper::runSpeed
414  timeToCheckTheEndstops = steppers[i].stepper->runSpeed(); // true if stepped
415  break;
416 
417  case state_runSpeedToPosition: // boolean AccelStepper::runSpeedToPosition
418  timeToCheckTheEndstops = steppers[i].stepper->runSpeedToPosition(); // true if stepped
419  if (steppers[i].stepper->distanceToGo() == 0) {
420  // target reached, stop polling
423  }
424  break;
425 
426  case state_stopped: // do nothing
427  break;
428  } // switch
429 
430 #ifdef DEBUG
431  if (reportNow) {
432  log("\n\n");
433  reportNow = false;
434  }
435 #endif
436 
437  if (timeToCheckTheEndstops and steppers[i].endstopsEnabled) { // the stepper (potentially) stepped a step, so let's look at the endstops
438  uint8_t es = pollEndstops(i);
439  if (es != steppers[i].prevEndstopState) { // detect rising *or* falling flank
440  uint32_t ms = millis();
441  if (ms > steppers[i].endstopDebounceEnd) { // primitive debounce: ignore endstops for some ms after each new flank
442  log("** es: flank detected \n");
443  steppers[i].endstopDebounceEnd = ms + endstopDebouncePeriod; // set end of debounce period
444  steppers[i].prevEndstopState = es;
445  if (es != 0) { // this is a non-bounce, *rising* flank
446  log("** es: endstop detected!\n");
447  //steppers[i].stepper->stop();
448  steppers[i].stepper->setSpeed(0);
449  steppers[i].stepper->moveTo(steppers[i].stepper->currentPosition());
450  steppers[i].state = state_stopped; // endstop reached, stop polling
452  }
453  }
454  }
455  } // check endstops
456 
457  } // for
458 
459  // Check for new incoming messages from I2C interrupt
460  if (newMessage > 0) {
462  newMessage = 0;
463  }
464 
465 #ifdef DEBUG
466  if (sentOnRequest > 0) {
467  log("Output buffer sent ("); log(sentOnRequest); log(" bytes): ");
468  for (uint8_t j = 0; j < sentOnRequest; j++) {
469  log(bufferOut->buffer[j]); log(" ");
470  }
471  log("\n");
472  sentOnRequest = 0;
473  }
474 #endif
475 
476  cycles++;
477 }
478 
479 
480 
481 /**************************************************************************/
486 /**************************************************************************/
487 #if defined(ESP8266)
488 ICACHE_RAM_ATTR
489 #endif
490 void receiveEvent(int howMany)
491 {
492 
493 #ifdef DIAGNOSTICS
494  thenMicros = micros();
495 #endif // DIAGNOSTICS
496  log(howMany); log(" I\n");
497  if (newMessage == 0) { // Only accept message if an earlier one is fully processed.
498  bufferIn->reset();
499  for (uint8_t i = 0; i < howMany; i++) {
500  bufferIn->buffer[i] = Wire.read();
501  }
502  bufferIn->idx = howMany;
503  newMessage = howMany; // tell main loop that new data has arrived
504 
505  } else { // discard message ### how can we warn the master??? Use interrupt?
506  while (Wire.available()) { // discard data (### needed?)
507  Wire.read();
508  }
509  }
510 
511 #ifdef DIAGNOSTICS
512  // delay storing the executing time for one cycle, else diagnostics() would always return
513  // its own receive time, not the one of the previous command
514  currentDiagnostics.lastReceiveTime = previousLastReceiveTime;
515  previousLastReceiveTime = micros() - thenMicros;
516 #endif // DIAGNOSTICS
517 
518 }
519 
520 bool validStepper(int8_t s)
521 {
522  return (s >= 0) and (s < numSteppers);
523 }
524 
525 bool validServo(int8_t s)
526 {
527  return (s >= 0) and (s < numServos);
528 }
529 
530 /**************************************************************************/
548 /**************************************************************************/
549 void processMessage(uint8_t len)
550 {
551 
552 #ifdef DIAGNOSTICS
553  uint32_t thenMicrosP = micros();
554 #endif // DIAGNOSTICS
555 
556  log("New message with "); log(len); log(" bytes. ");
557  for (int j = 0; j < len; j++) { // I expect the compiler will optimize this away if debugging is off
558  log (bufferIn->buffer[j]); log(" ");
559  }
560  log("\n");
561 
562  if (bufferIn->checkCRC8()) { // ignore invalid message ### how to warn the master?
563 
564  bufferIn->reset(); // set reader to beginning of message
565  uint8_t cmd; bufferIn->read(cmd);
566  int8_t unit; bufferIn->read(unit); // stepper/servo/etc. addressed (if any, will not be used for general commands)
567  int8_t i = len - 3 ; // bytes received minus 3 header bytes = number of parameter bytes
568  log("CRC8 ok. Command = "); log(cmd); log(" for unit "); log(unit);
569  log(" with "); log(i); log(" parameter bytes --> ");
570  // if ((cmd >= generalCommandsStart) or ((unit >= 0) and
571  //#if defined(SERVO_SUPPORT) // unit can refer to steppers *or* servos
572  // (unit < (cmd >= servoCommandsStart ? numServos : numSteppers))
573  //#else // unit refers to steppers
574  // (unit < numSteppers)
575  //#endif //defined(SERVO_SUPPORT)
576  // )) {
577  bufferOut->reset(); // let's hope last result was already requested by master, as now it's gone
578 
579  switch (cmd) {
580 
581  case moveToCmd: { // void moveTo (long absolute)
582  if (validStepper(unit) and (i == 4)) { // 1 long parameter (not nice to have these constants hardcoded here, but what the heck)
583  long l = 0;
584  bufferIn->read(l);
585  steppers[unit].stepper->moveTo(l);
586  }
587  }
588  break;
589 
590  case moveCmd: { // void move (long relative)
591  if (validStepper(unit) and (i == 4)) { // 1 long parameter
592  long l = 0;
593  bufferIn->read(l);
594  steppers[unit].stepper->move(l);
595  }
596  }
597  break;
598 
599  // usually not to be called directly via I2C, use state machine instead
600  case runCmd: { // boolean run ()
601  if (validStepper(unit) and (i == 0)) { // no parameters
602  bool res = steppers[unit].stepper->run();
603  bufferOut->write(res);
604  }
605  }
606  break;
607 
608  // usually not to be called directly via I2C, use state machine instead
609  case runSpeedCmd: { // boolean runSpeed ()
610  if (validStepper(unit) and (i == 0)) { // no parameters
611  bool res = steppers[unit].stepper->runSpeed();
612  bufferOut->write(res);
613  }
614  }
615  break;
616 
617  case setMaxSpeedCmd: { // void setMaxSpeed (float speed)
618  if (validStepper(unit) and (i == 4)) { // 1 long parameter
619  float f = 0;
620  bufferIn->read(f);
621  steppers[unit].stepper->setMaxSpeed(f);
622  }
623  }
624  break;
625 
626  case maxSpeedCmd: { // float maxSpeed ()
627  if (validStepper(unit) and (i == 0)) { // no parameters
628  float f = steppers[unit].stepper->maxSpeed();
629  bufferOut->write(f);
630  }
631  }
632  break;
633 
634  case setAccelerationCmd: { // void setAcceleration (float acceleration)
635  if (validStepper(unit) and (i == 4)) { // 1 float parameter
636  float f = 0;
637  bufferIn->read(f);
638  steppers[unit].stepper->setAcceleration(f);
639  }
640  }
641  break;
642 
643  case setSpeedCmd: { // void setSpeed (float speed)
644  if (validStepper(unit) and (i == 4)) { // 1 float parameter
645  float f = 0;
646  bufferIn->read(f);
647  steppers[unit].stepper->setSpeed(f);
648  }
649  }
650  break;
651 
652  case speedCmd: { // float speed ()
653  if (validStepper(unit) and (i == 0)) { // no parameters
654  float f = steppers[unit].stepper->speed();
655  bufferOut->write(f);
656  }
657  }
658  break;
659 
660  case distanceToGoCmd: { // long distanceToGo ()
661  if (validStepper(unit) and (i == 0)) { // no parameters
662  long l = steppers[unit].stepper->distanceToGo();
663  bufferOut->write(l);
664  }
665  }
666  break;
667 
668  case targetPositionCmd: { // long targetPosition ()
669  if (validStepper(unit) and (i == 0)) { // no parameters
670  long l = steppers[unit].stepper->targetPosition();
671  bufferOut->write(l);
672  }
673  }
674  break;
675 
676  case currentPositionCmd: { // long currentPosition ()
677  if (validStepper(unit) and (i == 0)) { // no parameters
678  long l = steppers[unit].stepper->currentPosition();
679  bufferOut->write(l);
680  }
681  }
682  break;
683 
684  case setCurrentPositionCmd: { // void setCurrentPosition (long position)
685  if (validStepper(unit) and (i == 4)) { // 1 long parameter
686  long l = 0;
687  bufferIn->read(l);
688  steppers[unit].stepper->setCurrentPosition(l);
689  }
690  }
691  break;
692 
693  // blocking, not implemented (yet?)
694  case runToPositionCmd: // void runToPosition () {}
695  break;
696 
697  // usually not to be called directly via I2C, use state machine instead
698  case runSpeedToPositionCmd: { // boolean runSpeedToPosition ()
699  if (validStepper(unit) and (i == 0)) { // no parameters
700  bool res = steppers[unit].stepper->runSpeedToPosition();
701  bufferOut->write(res);
702  }
703  }
704  break;
705 
706  // blocking, not implemented (yet?)
707  case runToNewPositionCmd: // void runToNewPosition (long position) {}
708  break;
709 
710  case stopCmd: { // void stop ()
711  if (validStepper(unit) and (i == 0)) { // no parameters
712  steppers[unit].stepper->stop();
713  }
714  }
715  break;
716 
717  case disableOutputsCmd: { // virtual void disableOutputs ()
718  if (validStepper(unit) and (i == 0)) { // no parameters
719  steppers[unit].stepper->disableOutputs();
720  }
721  }
722  break;
723 
724  case enableOutputsCmd: { // virtual void enableOutputs ()
725  if (validStepper(unit) and (i == 0)) { // no parameters
726  steppers[unit].stepper->enableOutputs();
727  }
728  }
729  break;
730 
731  case setMinPulseWidthCmd: { // void setMinPulseWidth (unsigned int minWidth)
732  if (validStepper(unit) and (i == 2)) {
733  // 1 uint16
734  uint16_t minW = 0;
735  bufferIn->read(minW);
736  steppers[unit].stepper->setMinPulseWidth(minW);
737  }
738  }
739  break;
740 
741  case setEnablePinCmd: { // void setEnablePin (uint8_t enablePin=0xff)
742  if (validStepper(unit) and (i == 1)) {
743  // 1 uint8_t
744  uint8_t pin = 0;
745  bufferIn->read(pin);
746  steppers[unit].stepper->setEnablePin(pin);
747  }
748  }
749  break;
750 
751  case setPinsInverted1Cmd: { // void setPinsInverted (bool directionInvert=false, bool stepInvert=false, bool enableInvert=false)
752  if (validStepper(unit) and (i == 1)) {
753  // 8 bits
754  uint8_t b = 0;
755  bufferIn->read(b);
756  steppers[unit].stepper->setPinsInverted(
757  (b & 1 << 0) != 0, (b & 1 << 1) != 0, (b & 1 << 2) != 0);
758  }
759  }
760  break;
761 
762  case setPinsInverted2Cmd: { // void setPinsInverted (bool pin1Invert, bool pin2Invert, bool pin3Invert, bool pin4Invert, bool enableInvert)
763  if (validStepper(unit) and (i == 1)) {
764  // 8 bits
765  uint8_t b;
766  bufferIn->read(b);
767  steppers[unit].stepper->setPinsInverted(
768  (b & 1 << 0) != 0, (b & 1 << 1) != 0, (b & 1 << 2) != 0, (b & 1 << 3) != 0, (b & 1 << 4) != 0);
769  }
770  }
771  break;
772 
773  case isRunningCmd: { // bool isRunning ()
774  if (validStepper(unit) and (i == 0)) { // no parameters
775  bool b = steppers[unit].stepper->isRunning();
776  bufferOut->write(b);
777  }
778  }
779  break;
780 
781 
782  // AccelstepperI2C state machine commands
783 
784 
785  case setStateCmd: { //
786  if (validStepper(unit) and (i == 1)) { // 1 uint8_t
787  uint8_t newState;
788  bufferIn->read(newState);
789  steppers[unit].state = newState;
790  }
791  }
792  break;
793 
794 
795  case getStateCmd: { //
796  if (validStepper(unit) and (i == 0)) { // no parameters
797  bufferOut->write(steppers[unit].state);
798  }
799  }
800  break;
801 
802 
803  case setEndstopPinCmd: { //
804  if (validStepper(unit) and (i == 3) and (steppers[unit].numEndstops < maxEndstops)) {
805  int8_t pin; bufferIn->read(pin);
806  bool activeLow; bufferIn->read(activeLow);
807  bool internalPullup; bufferIn->read(internalPullup);
808  steppers[unit].endstops[steppers[unit].numEndstops].pin = pin;
809  steppers[unit].endstops[steppers[unit].numEndstops].activeLow = activeLow;
810  pinMode(pin, internalPullup ? INPUT_PULLUP : INPUT);
811  steppers[unit].numEndstops++;
812  }
813  }
814  break;
815 
816  case enableEndstopsCmd: {
817  if (validStepper(unit) and (i == 1)) { // 1 bool
818  bool en; bufferIn->read(en);
819  steppers[unit].endstopsEnabled = en;
820  if (en) { // prevent that an interrupt is triggered immediately in case an endstop happens to be active at the moment
821  steppers[unit].prevEndstopState = pollEndstops(unit);
822  }
823  }
824  }
825  break;
826 
827  case endstopsCmd: {
828  if (validStepper(unit) and (i == 0)) { // no parameters
829  uint8_t b = pollEndstops(unit);
830  bufferOut->write(b);
831  }
832  }
833  break;
834 
835  case attachCmd: { //
836  //log("addStepperCmd\n");
837  if (i == 6) { // 5 uint8_t + 1 bool
838  uint8_t interface; bufferIn->read(interface);
839  uint8_t pin1; bufferIn->read(pin1);
840  uint8_t pin2; bufferIn->read(pin2);
841  uint8_t pin3; bufferIn->read(pin3);
842  uint8_t pin4; bufferIn->read(pin4);
843  bool enable; bufferIn->read(enable);
844  int8_t num = addStepper(interface, pin1, pin2, pin3, pin4, enable);
845  bufferOut->write(num);
846  }
847  }
848  break;
849 
850  /*
851 
852  wrapperI2C commands
853 
854  */
855 
856  case resetCmd: {
857  if (i == 0) { // no parameters
858  log("\n\n---> Resetting\n\n");
859  for (uint8_t j = 0; j < numSteppers; j++) {
860  steppers[j].stepper->stop();
861  steppers[j].stepper->disableOutputs();
862  }
863  }
864 #ifdef DEBUG
865  Serial.flush();
866 #endif
867  resetFunc();
868  }
869  break;
870 
871  case changeI2CaddressCmd: {
872  if (i == 1) { // 1 uint8_t
873  uint8_t newAddress; bufferIn->read(newAddress);
874  log("Storing new Address "); log(newAddress); log("\n");
876  }
877  }
878  break;
879 
880 #ifdef DIAGNOSTICS
881 
882  case enableDiagnosticsCmd: {
883  if (i == 1) { // 1 bool
884  bufferIn->read(diagnosticsEnabled);
885  }
886  }
887  break;
888 
889  case diagnosticsCmd: {
890  if (i == 0) { // no parameters
891  currentDiagnostics.cycles = cycles;
892  bufferOut->write(currentDiagnostics);
893  cycles = 0;
894  }
895  }
896  break;
897 
898 #endif // DIAGNOSTICS
899 
900  case setInterruptPinCmd: {
901  if (i == 2) {
904  pinMode(interruptPin, OUTPUT);
905  clearInterrupt();
906  }
907  }
908  break;
909 
910  case clearInterruptCmd: {
911  if (i == 0) { // no parameters
912  clearInterrupt(); // reset pin state
914  interruptSource = 0xF;
915  interruptReason = 0xF;
916  }
917  }
918  break;
919 
920  case enableInterruptsCmd: { //
921  if (validStepper(unit) and (i == 1)) { // 1 bool
922  bufferIn->read(steppers[unit].interruptsEnabled);
923  }
924  }
925  break;
926 
927  case getVersionCmd: {
928  if (i == 0) { // no parameters
932  }
933  }
934  break;
935 
936 #if defined(SERVO_SUPPORT)
937  // servo implementation is dumb. It passes each command over I2C even if only some of the commands actually do something slave related. It would be much smarter to let the master do commands like attached() without I2C traffic. The dumb way however is easier and safer.
938 
939  case servoAttach1Cmd: {
940  if ((numServos < maxServos) and (i == 2)) { // 1 int
941  int p; bufferIn->read(p);
942  servos[numServos].attach(p);
944  log(numServos);
945  }
946  }
947  break;
948 
949  case servoAttach2Cmd: {
950  if ((numServos < maxServos) and (i == 6)) { // 3 int
951  int p; bufferIn->read(p);
952  int min; bufferIn->read(min);
953  int max; bufferIn->read(max);
954  servos[numServos].attach(p, min, max);
956  }
957  }
958  break;
959 
960  case servoDetachCmd: {
961  if (validServo(unit) and (i == 0)) { // no parameters
962  servos[unit].detach();
963  }
964  }
965  break;
966 
967  case servoWriteCmd: {
968  if (validServo(unit) and (i == 2)) { // 1 int
969  int value; bufferIn->read(value);
970  servos[unit].write(value);
971  }
972  }
973  break;
974 
976  if (validServo(unit) and (i == 2)) { // 1 int
977  int value; bufferIn->read(value);
978  servos[unit].writeMicroseconds(value);
979  }
980  }
981  break;
982 
983  case servoReadCmd: {
984  if (validServo(unit) and (i == 0)) { // no parameters
985  bufferOut->write(servos[unit].read());
986  }
987  }
988  break;
989 
991  if (validServo(unit) and (i == 0)) { // no parameters
992  bufferOut->write(servos[unit].readMicroseconds());
993  }
994  }
995  break;
996 
997  case servoAttachedCmd: {
998  if (validServo(unit) and (i == 0)) { // no parameters
999  bufferOut->write(servos[unit].attached());
1000  }
1001  }
1002  break;
1003 
1004 #endif // defined(SERVO_SUPPORT)
1005 
1006  default:
1007  log("No matching command found");
1008 
1009  } // switch
1010 
1011 #ifdef DEBUG
1012  if (bufferOut->idx > 1) {
1013  log("Output buffer after processing ("); log(bufferOut->idx); log(" bytes): ");
1014  for (uint8_t i = 0; i < bufferOut->idx; i++) {
1015  log(bufferOut->buffer[i]); log(" ");
1016  }
1017  log("\n");
1018  } // if (bufferOut->idx > 1)
1019 #endif
1020 
1021 #ifdef ESP32
1022  // ESP32 slave implementation needs the I2C-reply buffer prefilled. I.e. onRequest() will, without our own doing,
1023  // just send what's in the I2C buffer at the time of the request, and anything that's written during the request
1024  // will only be sent in the next request cycle. So let's prefill the buffer here.
1025  // ### what exactly is the role of slaveWrite() vs. Write(), here?
1026  // ### slaveWrite() is only for ESP32, not for it's poorer cousins ESP32-S2 and ESP32-C3. Need to fine tune the compiler directive, here?
1028 #endif // ESP32
1029 
1030  // } // if valid s
1031 
1032  } // if (bufferIn->checkCRC8())
1033  log("\n");
1034 
1035 
1036 #ifdef DIAGNOSTICS
1037  currentDiagnostics.lastProcessTime = micros() - thenMicrosP;
1038 #endif // DIAGNOSTICS
1039 
1040 }
1041 
1042 // If there is anything in the output buffer, write it out to I2C.
1043 // This is outdourced to a function, as, depending on the architecture,
1044 // it is called either directly from the interrupt (AVR) or from
1045 // message processing (ESP32).
1047 {
1048 
1049  if (bufferOut->idx > 1) {
1050 
1051  bufferOut->setCRC8();
1052 
1053 #ifdef ESP32
1054  Wire.slaveWrite(bufferOut->buffer, bufferOut->idx);
1055 #else
1056  Wire.write(bufferOut->buffer, bufferOut->idx);
1057 #endif // ESP32
1058 
1059 #ifdef DEBUG
1060  // for AVRs logging to Serial will happen in an interrupt - not recommenended but seems to work
1061  log("sent "); log(bufferOut->idx); log(" bytes: ");
1062  for (uint8_t i = 0; i < bufferOut->idx; i++) {
1063  log(bufferOut->buffer[i]); log(" ");
1064  }
1065  log("\n");
1066  writtenToBuffer = bufferOut->idx; // remember this (for ESP32) to signal main loop later that we sent buffer
1067 #endif
1068 
1069  bufferOut->reset(); // never send anything twice
1070 
1071  }
1072 }
1073 
1074 
1075 /**************************************************************************/
1082 /**************************************************************************/
1083 #if defined(ESP8266)
1084 ICACHE_RAM_ATTR
1085 #endif
1087 {
1088 
1089 #ifdef DIAGNOSTICS
1090  thenMicros = micros();
1091 #endif // DIAGNOSTICS
1092 
1093 #ifndef ESP32
1095 #endif // not ESP32
1096 
1097 #ifdef DEBUG
1098  sentOnRequest = writtenToBuffer; // signal main loop that we sent buffer contents
1099 #endif // DEBUG
1100 
1101 #ifdef DIAGNOSTICS
1102  currentDiagnostics.lastRequestTime = micros() - thenMicros;
1103 #endif // DIAGNOSTICS
1104 
1105 }
diagnosticsCmd
const uint8_t diagnosticsCmd
Definition: AccelStepperI2C.h:123
numSteppers
uint8_t numSteppers
Definition: firmware.ino:231
servoWriteMicrosecondsCmd
const uint8_t servoWriteMicrosecondsCmd
Definition: ServoI2C.h:42
newMessage
volatile uint8_t newMessage
Definition: firmware.ino:140
moveToCmd
const uint8_t moveToCmd
Definition: AccelStepperI2C.h:94
interruptReason_endstopHit
const uint8_t interruptReason_endstopHit
Definition: AccelStepperI2C.h:144
VersionMinor
const uint8_t VersionMinor
Definition: version.h:5
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
interruptReason_targetReachedByRunSpeedToPosition
const uint8_t interruptReason_targetReachedByRunSpeedToPosition
Definition: AccelStepperI2C.h:143
reportNow
bool reportNow
Definition: firmware.ino:103
runToNewPositionCmd
const uint8_t runToNewPositionCmd
Definition: AccelStepperI2C.h:109
lastCycles
uint32_t lastCycles
Definition: firmware.ino:104
Stepper
This struct comprises all stepper parameters needed for local slave management.
Definition: firmware.ino:236
eepromI2CaddressMarker
const uint32_t eepromI2CaddressMarker
Definition: firmware.ino:128
servoDetachCmd
const uint8_t servoDetachCmd
Definition: ServoI2C.h:40
writeOutputBuffer
void writeOutputBuffer()
Definition: firmware.ino:1046
speedCmd
const uint8_t speedCmd
Definition: AccelStepperI2C.h:102
isRunningCmd
const uint8_t isRunningCmd
Definition: AccelStepperI2C.h:117
SimpleBuffer::setCRC8
void setCRC8()
Calculate CRC8 checksum for the currently used buffer ([1]...[idx-1]) and store it in the first byte ...
Definition: SimpleBuffer.cpp:49
Stepper::stepper
AccelStepper * stepper
Definition: firmware.ino:238
runSpeedCmd
const uint8_t runSpeedCmd
Definition: AccelStepperI2C.h:97
stopCmd
const uint8_t stopCmd
Definition: AccelStepperI2C.h:110
diagnosticsReport::cycles
uint32_t cycles
Number of slave's main loop executions since the last reboot.
Definition: AccelStepperI2C.h:87
enableDiagnosticsCmd
const uint8_t enableDiagnosticsCmd
Definition: AccelStepperI2C.h:122
writtenToBuffer
volatile uint8_t writtenToBuffer
Definition: firmware.ino:95
disableOutputsCmd
const uint8_t disableOutputsCmd
Definition: AccelStepperI2C.h:111
SimpleBuffer::checkCRC8
bool checkCRC8()
Check for correct CRC8 checksum. First byte [0] holds the checksum, rest of the currently used buffer...
Definition: SimpleBuffer.cpp:54
now
uint32_t now
Definition: firmware.ino:102
SimpleBuffer
Definition: SimpleBuffer.h:30
setMaxSpeedCmd
const uint8_t setMaxSpeedCmd
Definition: AccelStepperI2C.h:98
enableInterruptsCmd
const uint8_t enableInterruptsCmd
Definition: AccelStepperI2C.h:124
servoReadMicrosecondsCmd
const uint8_t servoReadMicrosecondsCmd
Definition: ServoI2C.h:44
endstopDebouncePeriod
const uint32_t endstopDebouncePeriod
Definition: firmware.ino:224
AccelStepperI2CmaxBuf
const uint8_t AccelStepperI2CmaxBuf
Definition: AccelStepperI2C.h:75
currentPositionCmd
const uint8_t currentPositionCmd
Definition: AccelStepperI2C.h:105
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
steppers
Stepper steppers[maxSteppers]
Definition: firmware.ino:247
Endstop::pin
uint8_t pin
Definition: firmware.ino:219
servoReadCmd
const uint8_t servoReadCmd
Definition: ServoI2C.h:43
servoAttach2Cmd
const uint8_t servoAttach2Cmd
Definition: ServoI2C.h:39
servos
Servo servos[maxServos]
Uncomment this to enable time keeping diagnostics. You probably should disable debugging,...
Definition: firmware.ino:84
Stepper::numEndstops
uint8_t numEndstops
Definition: firmware.ino:241
changeI2CaddressCmd
const uint8_t changeI2CaddressCmd
Definition: I2Cwrapper.h:39
resetFunc
void resetFunc()
Definition: firmware.ino:254
AccelStepperI2C.h
setInterruptPinCmd
const uint8_t setInterruptPinCmd
Definition: I2Cwrapper.h:40
clearInterrupt
void clearInterrupt()
Definition: firmware.ino:358
diagnosticsReport::lastProcessTime
uint16_t lastProcessTime
microseconds the slave needed to process (interpret) most recently received command
Definition: AccelStepperI2C.h:88
cycles
uint32_t cycles
Definition: firmware.ino:120
SimpleBuffer::idx
uint8_t idx
The position pointer. Remember, [0] holds the CRC8 checksum, so for an empty buffer,...
Definition: SimpleBuffer.h:90
validStepper
bool validStepper(int8_t s)
Definition: firmware.ino:520
EEPROM_OFFSET_I2C_ADDRESS
#define EEPROM_OFFSET_I2C_ADDRESS
Definition: firmware.ino:127
setPinsInverted1Cmd
const uint8_t setPinsInverted1Cmd
Definition: AccelStepperI2C.h:115
interruptSource
uint8_t interruptSource
Definition: firmware.ino:210
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
getStateCmd
const uint8_t getStateCmd
Definition: AccelStepperI2C.h:126
numServos
uint8_t numServos
Definition: firmware.ino:85
maxEndstops
const uint8_t maxEndstops
Definition: firmware.ino:223
Stepper::interruptsEnabled
bool interruptsEnabled
Definition: firmware.ino:242
setEnablePinCmd
const uint8_t setEnablePinCmd
Definition: AccelStepperI2C.h:114
reportPeriod
const uint32_t reportPeriod
Definition: firmware.ino:105
storeI2C_address
void storeI2C_address(uint8_t newAddress)
Write I2C address to EEPROM.
Definition: firmware.ino:178
SimpleBuffer::init
void init(uint8_t buflen)
Allocate and reset buffer.
Definition: SimpleBuffer.cpp:16
attachCmd
const uint8_t attachCmd
Definition: AccelStepperI2C.h:121
interruptReason
uint8_t interruptReason
Definition: firmware.ino:211
slaveDefaultAddress
const uint8_t slaveDefaultAddress
Definition: firmware.ino:42
setAccelerationCmd
const uint8_t setAccelerationCmd
Definition: AccelStepperI2C.h:100
SimpleBuffer::reset
void reset()
Reset the position pointer to the start of the buffer (which is[1] as [0] is the CRC8 chcksum) withou...
Definition: SimpleBuffer.cpp:23
loop
void loop()
Main loop, implements the state machine. Will check for each stepper's state and do the appropriate p...
Definition: firmware.ino:376
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
maxServos
const uint8_t maxServos
Definition: firmware.ino:49
resetCmd
const uint8_t resetCmd
Definition: I2Cwrapper.h:38
log
#define log(...)
Definition: firmware.ino:94
then
uint32_t then
Definition: firmware.ino:102
Stepper::endstops
Endstop endstops[maxEndstops]
Definition: firmware.ino:240
processMessage
void processMessage(uint8_t len)
Definition: firmware.ino:549
interruptActiveHigh
bool interruptActiveHigh
Definition: firmware.ino:209
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
distanceToGoCmd
const uint8_t distanceToGoCmd
Definition: AccelStepperI2C.h:103
pollEndstops
uint8_t pollEndstops(uint8_t s)
Definition: firmware.ino:330
maxSpeedCmd
const uint8_t maxSpeedCmd
Definition: AccelStepperI2C.h:99
Stepper::endstopDebounceEnd
uint32_t endstopDebounceEnd
Definition: firmware.ino:245
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
Endstop
Definition: firmware.ino:217
receiveEvent
void receiveEvent(int howMany)
Handle I2C receive event. Just read the message and inform main loop.
Definition: firmware.ino:490
ServoI2C.h
Arduino library for I2C-control of servo motors connected to another Arduino which runs the associate...
Endstop::activeLow
bool activeLow
Definition: firmware.ino:220
targetPositionCmd
const uint8_t targetPositionCmd
Definition: AccelStepperI2C.h:104
SimpleBuffer.h
Simple and ugly serialization buffer for any data type. Template technique and CRC8 adapted from Nick...
interruptReason_none
const uint8_t interruptReason_none
You should not encounter this, as you don't want to be interrupted without a reason....
Definition: I2Cwrapper.h:53
enableOutputsCmd
const uint8_t enableOutputsCmd
Definition: AccelStepperI2C.h:112
bufferIn
SimpleBuffer * bufferIn
Definition: firmware.ino:138
maxSteppers
const uint8_t maxSteppers
Definition: firmware.ino:230
servoAttachedCmd
const uint8_t servoAttachedCmd
Definition: ServoI2C.h:45
validServo
bool validServo(int8_t s)
Definition: firmware.ino:525
newAddress
const uint8_t newAddress
Definition: Change_address.ino:17
runToPositionCmd
const uint8_t runToPositionCmd
Definition: AccelStepperI2C.h:107
triggerInterrupt
void triggerInterrupt(uint8_t source, uint8_t reason)
Definition: firmware.ino:347
SimpleBuffer::buffer
uint8_t * buffer
The allocated buffer.
Definition: SimpleBuffer.h:84
servoAttach1Cmd
const uint8_t servoAttach1Cmd
Definition: ServoI2C.h:38
setSpeedCmd
const uint8_t setSpeedCmd
Definition: AccelStepperI2C.h:101
requestEvent
void requestEvent()
Handle I2C request event. Will send results or information requested by the last command,...
Definition: firmware.ino:1086
setStateCmd
const uint8_t setStateCmd
Definition: AccelStepperI2C.h:125
enableEndstopsCmd
const uint8_t enableEndstopsCmd
Definition: AccelStepperI2C.h:128
getVersionCmd
const uint8_t getVersionCmd
Definition: I2Cwrapper.h:42
diagnosticsReport::lastRequestTime
uint16_t lastRequestTime
microseconds the slave spent in the most recent onRequest() interrupt
Definition: AccelStepperI2C.h:89
setup
void setup()
Setup system. Retrieve I2C address from EEPROM or default and initialize I2C slave.
Definition: firmware.ino:276
VersionPatch
const uint8_t VersionPatch
Definition: version.h:6
retrieveI2C_address
uint8_t retrieveI2C_address()
Read a stored I2C address from EEPROM. If there is none, use default.
Definition: firmware.ino:146
clearInterruptCmd
const uint8_t clearInterruptCmd
Definition: I2Cwrapper.h:41
interruptPin
int8_t interruptPin
Definition: firmware.ino:208
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
Stepper::prevEndstopState
uint8_t prevEndstopState
Definition: firmware.ino:244
interruptReason_targetReachedByRun
const uint8_t interruptReason_targetReachedByRun
Definition: AccelStepperI2C.h:142
sentOnRequest
volatile uint8_t sentOnRequest
Definition: firmware.ino:96
Stepper::state
uint8_t state
Definition: firmware.ino:239
runSpeedToPositionCmd
const uint8_t runSpeedToPositionCmd
Definition: AccelStepperI2C.h:108
bufferOut
SimpleBuffer * bufferOut
Definition: firmware.ino:139
servoWriteCmd
const uint8_t servoWriteCmd
Definition: ServoI2C.h:41
diagnosticsReport::lastReceiveTime
uint16_t lastReceiveTime
microseconds the slave spent in the most recent onReceive() interrupt
Definition: AccelStepperI2C.h:90
VersionMajor
const uint8_t VersionMajor
Definition: version.h:4
Stepper::endstopsEnabled
bool endstopsEnabled
Definition: firmware.ino:243
addStepper
int8_t addStepper(uint8_t interface=AccelStepper::FULL4WIRE, uint8_t pin1=2, uint8_t pin2=3, uint8_t pin3=4, uint8_t pin4=5, bool enable=true)
Assign and initialize new stepper. Calls the AccelStepper's[1/2] constructor
Definition: firmware.ino:305