106 lines
2.5 KiB
C++
106 lines
2.5 KiB
C++
#include <AccelStepper.h>
|
|
|
|
// Define stepper motor connections and motor interface type. Motor interface type must be set to 1 when using a driver
|
|
#define motorInterfaceType 1
|
|
#define dirPin 2
|
|
#define stepPin 3
|
|
|
|
// Stepper motor speed configuration
|
|
#define microStepsFactor 4
|
|
#define maxStepperSpeed 1000 * microStepsFactor
|
|
#define inversionPause 20
|
|
|
|
// Create a new instance of the AccelStepper class:
|
|
AccelStepper stepper = AccelStepper(motorInterfaceType, stepPin, dirPin);
|
|
|
|
// Runtime data
|
|
int currentPosition[] = {-1, -1, -1};
|
|
int nextPosition[] = {0, 0, 0};
|
|
|
|
void setup() {
|
|
Serial.begin(115200);
|
|
|
|
// Wait for zeroing
|
|
Serial.println("Please rotate to zero position and press Enter");
|
|
while (Serial.available() == 0) {}
|
|
Serial.flush();
|
|
|
|
// Set the current position to 0:
|
|
stepper.setCurrentPosition(0);
|
|
// Set the maximum speed in steps per second:
|
|
stepper.setMaxSpeed(maxStepperSpeed);
|
|
|
|
// Rotate to 0,0,0
|
|
setPosition();
|
|
}
|
|
|
|
void loop() {
|
|
|
|
incrementCombination();
|
|
setPosition();
|
|
|
|
delay(inversionPause);
|
|
}
|
|
|
|
void incrementCombination() {
|
|
if (currentPosition[0] < 99) {
|
|
nextPosition[0] = currentPosition[0] + 1;
|
|
nextPosition[1] = currentPosition[1];
|
|
nextPosition[2] = currentPosition[2];
|
|
} else {
|
|
nextPosition[0] = 0;
|
|
if (currentPosition[1] < 99) {
|
|
nextPosition[1] = currentPosition[1] + 1;
|
|
nextPosition[2] = currentPosition[2];
|
|
} else {
|
|
nextPosition[1] = 0;
|
|
if (currentPosition[2] < 99) {
|
|
nextPosition[2] = currentPosition[2] + 1;
|
|
} else {
|
|
nextPosition[2] = 0;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
void setPosition() {
|
|
|
|
Serial.print("Testing ");
|
|
Serial.print(currentPosition[0]);
|
|
Serial.print(",");
|
|
Serial.print(currentPosition[1]);
|
|
Serial.print(",");
|
|
Serial.println(currentPosition[2]);
|
|
|
|
if (nextPosition[2] != nextPosition[2]) {
|
|
rotateCcwTo(nextPosition[2]);
|
|
rotateCcwTo(nextPosition[2]);
|
|
rotateCcwTo(nextPosition[2]);
|
|
}
|
|
|
|
if (nextPosition[1] != nextPosition[1]) {
|
|
rotateCwTo(nextPosition[1]);
|
|
rotateCwTo(nextPosition[1]);
|
|
}
|
|
|
|
if (nextPosition[0] != nextPosition[0]) {
|
|
rotateCcwTo(nextPosition[0]);
|
|
}
|
|
|
|
rotateCwTo(0);
|
|
rotateCwTo(10);
|
|
|
|
currentPosition[0] = nextPosition[0];
|
|
currentPosition[1] = nextPosition[1];
|
|
currentPosition[2] = nextPosition[2];
|
|
}
|
|
|
|
void rotateCwTo(int pos) {
|
|
stepper.move(1);
|
|
stepper.runToNewPosition(2 * pos * microStepsFactor);
|
|
}
|
|
|
|
void rotateCcwTo(int pos) {
|
|
stepper.move(-1);
|
|
stepper.runToNewPosition(-2 * pos * microStepsFactor);
|
|
}
|