AccelStepperI2C  v 0.1
I2C wrapper (and a bit more) for the AccelStepper Arduino library
Interrupt_Endstop.ino
Go to the documentation of this file.
1 /*
2  AccelStepperI2C interrupt & endstop demo
3  (c) juh 2022
4 
5  Uses end stops and interrupts to home a stepper, and then run
6  two tests: First, moves the stepper to random positions between,
7  and occasionaly beyond, two endstops. Then, repeatedley homes at
8  different speeds and reports reproducability of end stop positions.
9 
10  As it uses interrupts to detect target-reached or endstop-hit
11  conditions, it does not need to poll the slave at all.
12 
13  Assumes that master and slave both run on an Arduino Uno/Nano.
14  See config section below for hardware setup.
15 
16  Warning: Intentionally tries to move beyond the endstops, so set up
17  your test scenario according to the safety precautions in README.md.
18 
19 */
20 
21 #include <Arduino.h>
22 #include <AccelStepperI2C.h>
23 #include <Wire.h>
24 
25 
26 // --- Config. Change as needed for your hardware setup ---
27 
28 const uint8_t addr = 0x8; // i2c address of slave
29 const uint8_t stepPin = 8; // stepstick driver pin
30 const uint8_t dirPin = 7; // stepstick driver pin
31 const uint8_t enablePin = 2; // stepstick driver pin
32 const uint8_t endstopPin = 17; // Arduino Uno/Nano pin A3 (it's safer to use raw pin nr., since "A3" might mean sth. different if the master uses a different hardware platform)
33 const uint8_t interruptPinSlave = 9; // needs to be wired to interruptPinMaster
34 const uint8_t interruptPinMaster = 2; // wired to interruptPinSlave, needs to be a hardware interrupt pin (2 or 3 on Arduino Uno/Nano)
35 const float homingSpeed = 100.0; // in steps/second. Adapt this for your stepper/gear/microstepping setup
36 const float maxRunSpeed = 600.0; // in steps/second. Adapt this for your stepper/gear/microstepping setup
37 const float acceleration = maxRunSpeed / 4; // 4 seconds from 0 to max speed. Adapt this for your stepper/gear/microstepping setup
38 
39 // --- End of config ---
40 
41 
42 I2Cwrapper wrapper(addr); // each slave device is represented by a wrapper...
43 AccelStepperI2C stepper(&wrapper); // ...that the stepper uses to communicate with the slave
44 
45 volatile bool interruptFlag = false; // volatile for interrupt
47 long lower, upper;
48 
49 
50 /***********************************************************************************
51 
52  SETUP
53 
54  ************************************************************************************/
55 void setup()
56 {
57 
58  Serial.begin(115200);
59  Wire.begin();
60  // Wire.setClock(10000); // uncomment for ESP8266 slaves, to be on the safe side
61 
62 
63  /*
64  Prepare slave device
65  */
66 
67  if (!wrapper.ping()) {
68  Serial.println("Slave not found! Check connections and restart.");
69  while (true) {}
70  }
71  wrapper.reset(); // reset the slave device
72  delay(500); // and give it time to reboot
73 
74 
75  /*
76  Setup stepper/stepstick driver
77  */
78 
79  stepper.attach(AccelStepper::DRIVER, stepPin, dirPin);
80  if (stepper.myNum < 0) { // should not happen after a reset
81  Serial.println("Error: stepper could not be allocated");
82  while (true) {}
83  }
84  stepper.setEnablePin(enablePin);
85  stepper.setPinsInverted(false, false, true); // directionInvert, stepInvert, enableInvert
86  stepper.enableOutputs();
87  stepper.setMaxSpeed(maxRunSpeed);
88  stepper.setAcceleration(acceleration);
89 
90  /*
91  Setup endstops. Both are connected to the same pin, so we need to set the pin only once.
92  */
93 
94  stepper.setEndstopPin(endstopPin, /* activeLow */ true, /* internal pullup */ true); // activeLow since endstop switches pull the endstop pin to GND
95  stepper.enableEndstops(); // make the state machine check for the endstops
96  if (stepper.endstops() != 0) { // one of the endstops is currently activated
97  Serial.println("\nWarning: Please move stepper away from endstop,\n"
98  "because I don't know in which direction I have to move to get away from it.\n\nHALTING.");
99  stepper.disableOutputs(); // cut power, so that stepper can be moved away manually
100  while (true) {}
101  }
102 
103 
104  /*
105  Configure interrupts
106  */
107 
108  // First, make the slave send interrupts. The interrupt pin is shared by all units (steppers)
109  // which use interrupts, so we need the wrapper to set it up.
110  wrapper.setInterruptPin(interruptPinSlave, /* activeHigh */ true); // activeHigh -> master will have to look out for a RISING flank
111  stepper.enableInterrupts(); // make slave send out interrupts for this stepper at the pin set above
112 
113  // And now make the master listen for the interrupt
114  attachInterrupt(digitalPinToInterrupt(interruptPinMaster), interruptFromSlave, RISING);
115 
116 
117  /*
118  All set up and ready to go.
119  */
120 
121  findEndstops(homingSpeed); // find upperEndStopPos and lowerEndStopPos;
124 
125  stepper.moveTo(middlePos); // start in the middle
126  stepper.runState(); // go there with acceleration
127 
128  // do some random movements, causing endstop and target-reached interrupt conditions
129  randomWalk(25, 50); // 25 repetitions, 50% offshoot chance
130 
131  stepper.moveTo(middlePos); // go back to the middle before entering loop()
132  stepper.runState();
134 
135  // save endstop positions
138 
139 }
140 
141 
142 /***********************************************************************************
143 
144  LOOP: test reproducability of endstop positions
145 
146  ************************************************************************************/
147 long cycles = 0;
148 void loop()
149 {
150 
151  float sp = random(homingSpeed * 0.5, homingSpeed * 1.5); // vary speed a little
152  findEndstops(sp);
153  Serial.print("Deviations after "); Serial.print(cycles);
154  Serial.print(" cycles, measured with a speed of "); Serial.print(sp);
155  Serial.print(" steps/s: lower endstop = "); Serial.print(lowerEndStopPos - lower);
156  Serial.print(", upper endstop = "); Serial.println(upperEndStopPos - upper);
157  cycles++;
158 }
159 
160 
161 /***********************************************************************************
162 
163  REST OF THE SCHÜTZENFEST
164 
165  ************************************************************************************/
166 
167 void findEndstops(float sp)
168 {
169 
170  stepper.setSpeed(-sp); // start in negative direction, as we're looking for point zero first
171  stepper.runSpeedState(); // run at constant speed (= interrupt can only be triggered by endstop, not by target reached)
172  if (waitForInterrupt() != interruptReason_endstopHit) { // wait for endstop and check for correct interrupt cause
173  Serial.print("Error. Interrupt due to unexpected reason #");
174  while (true) {}
175  }
176 
177  lowerEndStopPos = stepper.currentPosition(); // save position (usually, it would make sense to simply set this to 0, but we don't for testing purposes)
178 
179  // note: we're still at the endstop. But the interrupt will only trigger on the next not-bouncing induced activated-flank, so no problem here
180  stepper.setSpeed(sp); // now let's look in the positive direction
181  stepper.runSpeedState(); // run at constant (slow) speed
182  if (waitForInterrupt() != interruptReason_endstopHit) { // and wait for other endstop
183  Serial.print("Error. Interrupt due to unexpected reason.");
184  while (true) {}
185  }
186  upperEndStopPos = stepper.currentPosition(); // save position
187 
188  Serial.print("Endstops found at positions 0 (lower endstop) and ");
189  Serial.print(upperEndStopPos); Serial.println(" (upper endstop).\n\n");
190 
191 }
192 
193 
194 
195 /*
196  wait and return reason for interrupt
197 */
199 {
200  while (!interruptFlag) {} // wait until interrupt will set the flag ### will probably need a yield() on ESPs
201  interruptFlag = false; // clear flag
202  uint8_t reasonAndUnit = wrapper.clearInterrupt();
203  if ((reasonAndUnit & 0xf) != stepper.myNum) { // stepper is the only unit in use, so there should be no other unit causing an interrupt
204  Serial.println("Error. Interrupt cause from unknown unit.");
205  while (true) {}
206  }
207  return reasonAndUnit >> 4; // dump unit and return reason in LSBs
208 }
209 
210 
211 
212 /*
213  This test routine will run the stepper repeatedly to random targets, some of
214  them intentionally invalid, i.e. outside of the endstop limits. If it hits
215  an endstop, it runs to the middle position, and starts over. If it arrives
216  at a valid target posistion, it just sets another target.
217 */
218 void randomWalk(int repetitions, long chance) // % chance that target will be off limits
219 {
220 
221  long offLimits = (range * chance / 100) / 2;
222 
223  for (int j = 0; j < repetitions; j++) {
224 
225  uint8_t reason = waitForInterrupt();
226 
227  switch (reason) {
229  Serial.print("Target reached as planned, setting new target to ");
230  long newPos = random(lowerEndStopPos - offLimits, upperEndStopPos + offLimits);
231  Serial.println(newPos);
232  stepper.moveTo(newPos);
233  stepper.runState(); // go there with acceleration
234  break;
235  }
237  Serial.println("Ran into endstop, returning to middle position.");
238  stepper.moveTo(middlePos);
239  stepper.runState(); // go there with acceleration
240  break;
241  }
242  default: {
243  Serial.print("Unexpected interrupt reason #");
244  Serial.println(reason);
245  while (true) {}
246  }
247  } // switch
248  }
249 } // void randomWalk
250 
251 
252 #if defined(ESP8266)
253 ICACHE_RAM_ATTR
254 #endif
256 {
257  interruptFlag = true; // just set a flag, leave interrupt as quickly as possible
258 }
I2Cwrapper
A helper class for the AccelStepperI2C (and ServoI2C) library.
Definition: I2Cwrapper.h:79
interruptReason_endstopHit
const uint8_t interruptReason_endstopHit
Definition: AccelStepperI2C.h:144
setup
void setup()
Definition: Interrupt_Endstop.ino:55
maxRunSpeed
const float maxRunSpeed
Definition: Interrupt_Endstop.ino:36
I2Cwrapper::clearInterrupt
uint8_t clearInterrupt()
Acknowledge a received interrupt to slave so that it can clear the interupt condition and return the ...
Definition: I2Cwrapper.cpp:137
addr
const uint8_t addr
Definition: Interrupt_Endstop.ino:28
wrapper
I2Cwrapper wrapper(addr)
Definition: Interrupt_Endstop.ino:43
AccelStepperI2C
An I2C wrapper class for the AccelStepper library.
Definition: AccelStepperI2C.h:163
AccelStepperI2C.h
acceleration
const float acceleration
Definition: Interrupt_Endstop.ino:37
randomWalk
void randomWalk(int repetitions, long chance)
Definition: Interrupt_Endstop.ino:218
upperEndStopPos
long upperEndStopPos
Definition: Interrupt_Endstop.ino:46
lower
long lower
Definition: Interrupt_Endstop.ino:47
interruptPinSlave
const uint8_t interruptPinSlave
Definition: Interrupt_Endstop.ino:33
stepPin
const uint8_t stepPin
Definition: Interrupt_Endstop.ino:29
interruptPinMaster
const uint8_t interruptPinMaster
Definition: Interrupt_Endstop.ino:34
endstopPin
const uint8_t endstopPin
Definition: Interrupt_Endstop.ino:32
I2Cwrapper::ping
bool ping()
Test if slave is listening.
Definition: I2Cwrapper.cpp:102
enablePin
const uint8_t enablePin
Definition: Interrupt_Endstop.ino:31
interruptFromSlave
void interruptFromSlave()
Definition: Interrupt_Endstop.ino:255
middlePos
long middlePos
Definition: Interrupt_Endstop.ino:46
upper
long upper
Definition: Interrupt_Endstop.ino:47
range
long range
Definition: Interrupt_Endstop.ino:46
cycles
long cycles
Definition: Interrupt_Endstop.ino:147
homingSpeed
const float homingSpeed
Definition: Interrupt_Endstop.ino:35
waitForInterrupt
uint8_t waitForInterrupt()
Definition: Interrupt_Endstop.ino:198
I2Cwrapper::reset
void reset()
Tells the slave to reset to it's default state. It is recommended to reset the slave every time the m...
Definition: I2Cwrapper.cpp:108
interruptReason_targetReachedByRun
const uint8_t interruptReason_targetReachedByRun
Definition: AccelStepperI2C.h:142
findEndstops
void findEndstops(float sp)
Definition: Interrupt_Endstop.ino:167
interruptFlag
volatile bool interruptFlag
Definition: Interrupt_Endstop.ino:45
lowerEndStopPos
long lowerEndStopPos
Definition: Interrupt_Endstop.ino:46
dirPin
const uint8_t dirPin
Definition: Interrupt_Endstop.ino:30
I2Cwrapper::setInterruptPin
void setInterruptPin(int8_t pin, bool activeHigh=true)
Define a global interrupt pin which can be used by device units (steppers, servos....
Definition: I2Cwrapper.cpp:128
loop
void loop()
Definition: Interrupt_Endstop.ino:148