Und hier das ganze Sketch:
#include <AccelStepper.h>
#include <AS5600.h>
#define STEPpin 14
#define DIRpin 12
#define Microstepping 16 // Stepperdriver is set to 1/n microsteps (here 1/4)
#define StepperStepsPerRevolution 200 //one revolution of the stepper is 200 steps
int StepsPerRevolution = (StepperStepsPerRevolution * Microstepping); // results in 800 steps per one revolution of the stepper
int Pos[4] = {115, 1129, 2153, 3177}; // Der Winkelsensor hat Werte für 0-360° Sensorradposition von 0-4095. Diese vier Werte in dem Array markieren im Wertebereich 0-4095 die Positionen der vier Mulden im Sensorrad
int RelativeStep[4];
int StartupPos;
int EncoderPos;
int StepperPos;
int StepperOffset;
int ErrorThreshold = 30; //Fehlerschwelle Stepper
int PosErrorCount; //Fehlerzähler Stepper
int MaxAllowedError = 10; //maximale Fehlerzahl
int StepperShakeCount;
int TurnCount;
int n;
unsigned long currentMillis;
bool AlternateLoop = true;
unsigned long previousMillis = 0;
unsigned long stepperMillis = 0;
const long PosMeasureInterval = 50; //alle 100 ms wird die Position des Sensorrads gemessen
AS5600 encoder;
AccelStepper stepper(AccelStepper::DRIVER, STEPpin, DIRpin);
void StepperShake() {
StepperShakeCount ++;
Serial.print("Shake: ");
Serial.println(StepperShakeCount);
stepper.setCurrentPosition(0);
stepper.setMaxSpeed(100000);
stepper.setAcceleration(100000);
stepper.runToNewPosition(-10);
yield();
stepper.runToNewPosition(100);
yield();
stepper.runToNewPosition(-100);
yield();
stepper.runToNewPosition(100);
yield();
stepper.runToNewPosition(-100);
yield();
stepper.runToNewPosition(100);
yield();
stepper.setMaxSpeed(150000);
stepper.setAcceleration(150000);
stepper.runToNewPosition(-10);
yield();
stepper.runToNewPosition(100);
yield();
stepper.runToNewPosition(-100);
yield();
stepper.runToNewPosition(100);
yield();
stepper.runToNewPosition(-100);
yield();
stepper.runToNewPosition(100);
yield();
stepper.setMaxSpeed(250000);
stepper.setAcceleration(250000);
stepper.runToNewPosition(-10);
yield();
stepper.runToNewPosition(100);
yield();
stepper.runToNewPosition(-100);
yield();
stepper.runToNewPosition(100);
yield();
stepper.runToNewPosition(-100);
yield();
stepper.runToNewPosition(100);
yield();
stepper.setMaxSpeed(350000);
stepper.setAcceleration(350000);
stepper.runToNewPosition(-10);
yield();
stepper.runToNewPosition(100);
yield();
stepper.runToNewPosition(-100);
yield();
stepper.runToNewPosition(100);
yield();
stepper.runToNewPosition(-100);
yield();
stepper.runToNewPosition(100);
yield();
stepper.setMaxSpeed(500000);
stepper.setAcceleration(500000);
stepper.runToNewPosition(-10);
yield();
stepper.runToNewPosition(100);
yield();
stepper.runToNewPosition(-100);
yield();
stepper.runToNewPosition(100);
yield();
stepper.runToNewPosition(-100);
yield();
stepper.runToNewPosition(100);
yield();
}
void StepperFindOrigin() {
stepper.setCurrentPosition(0);
stepper.setMaxSpeed(1000); // 10000 ist wunderbar
stepper.setAcceleration(1000); // 1000 scheint hier ein relalistischer Wert zu sein, 100000 ist schön schnell, 200 ist schön langsam
RelativeStep[0] = (map(Pos[1], 0, 4095, 1, StepsPerRevolution)) - (map(Pos[0], 0, 4095, 1, StepsPerRevolution));
RelativeStep[1] = (map(Pos[2], 0, 4095, 1, StepsPerRevolution)) - (map(Pos[1], 0, 4095, 1, StepsPerRevolution));
RelativeStep[2] = (map(Pos[3], 0, 4095, 1, StepsPerRevolution)) - (map(Pos[2], 0, 4095, 1, StepsPerRevolution));
RelativeStep[3] = StepsPerRevolution - (map(Pos[3], 0, 4095, 1, StepsPerRevolution)) + (map(Pos[0], 0, 4095, 1, StepsPerRevolution));
StartupPos = encoder.getPosition();
if ( (Pos[0] != StartupPos) && (Pos[1] != StartupPos) && (Pos[2] != StartupPos) && (Pos[3] != StartupPos) ) {
if (Pos[0] - StartupPos > 0) {
stepper.runToNewPosition(map(Pos[0] - StartupPos, 0, 4095, 1, StepsPerRevolution));
}
else if (Pos[1] - StartupPos > 0) {
stepper.runToNewPosition(map(Pos[1] - StartupPos, 0, 4095, 1, StepsPerRevolution));
}
else if (Pos[2] - StartupPos > 0) {
stepper.runToNewPosition(map(Pos[2] - StartupPos, 0, 4095, 1, StepsPerRevolution));
}
else if (Pos[3] - StartupPos > 0) {
stepper.runToNewPosition(map(Pos[3] - StartupPos, 0, 4095, 1, StepsPerRevolution));
}
else {
stepper.runToNewPosition(map(4095 - StartupPos + Pos[0], 0, 4095, 1, StepsPerRevolution));
}
}
EncoderPos = encoder.getPosition(); // get the absolute position of the encoder
StepperPos = stepper.currentPosition(); // get the position of the stepper
StepperOffset = EncoderPos - map(StepperPos, 1, StepsPerRevolution, 0, 4095);
stepper.setMaxSpeed(10000); // 10000 ist wunderbar
stepper.setAcceleration(100000); // 1000 scheint hier ein relalistischer Wert zu sein, 100000 ist schön schnell, 200 ist schön langsam
}
void setup()
{
Wire.setClock(400000L); //setzt die I²C Taktfrequenz auf 400kHz
Serial.begin(9600);
delay(100);
Serial.println("");
StepperFindOrigin();
}
void loop() {
currentMillis = millis();
if (AlternateLoop) // mit diesem Trick kann man pro Loop Durchlauf entweder einen Zweig oder einen anderen Zweig durchlaufen lassen, hier ist der 1. Zweig aktiv
{
if (stepper.distanceToGo() == 0)
{
stepperMillis = currentMillis;
n ++;
TurnCount ++;
Serial.print("Turn: ");
Serial.println(TurnCount);
if (n > 3) {
n = n - 4;
};
AlternateLoop = false;
}
} else { // hier ist der 2. Zweig aktiv
if (currentMillis - stepperMillis >= 500) // 500ms in einer der 4 Positionen warten, hier wird später die Farbe gemessen
{
stepper.move(RelativeStep[n]);
AlternateLoop = true;
}
}
stepper.run();
//*************** Prozedur Positionsabgleich begin ***************
if (currentMillis - previousMillis >= PosMeasureInterval) {
previousMillis = currentMillis;
EncoderPos = encoder.getPosition(); // get the absolute position of the encoder
StepperPos = stepper.currentPosition(); // get the position of the stepper
//Serial.print("SOLL: ");
//Serial.print((StepperOffset + map(StepperPos, 1, StepsPerRevolution, 0, 4095)) % 4095);
//Serial.print(" IST: ");
//Serial.print(EncoderPos);
//Serial.print(" DIFFERENZ: ");
//Serial.print((StepperOffset + map(StepperPos, 1, StepsPerRevolution, 0, 4095)) % 4095 - EncoderPos);
if (((StepperOffset + map(StepperPos, 1, StepsPerRevolution, 0, 4095)) % 4095 - EncoderPos < -ErrorThreshold ) || ((StepperOffset + map(StepperPos, 1, StepsPerRevolution, 0, 4095)) % 4095 - EncoderPos > ErrorThreshold )) {
PosErrorCount++; //Wenn die Positionsabweichung ausserhalb der erlaubten Toleranz ist, den Fehlerzähler um 1 erhöhen
}
//Serial.print(" PosErrorCount: ");
//Serial.println(PosErrorCount);
if (((StepperOffset + map(StepperPos, 1, StepsPerRevolution, 0, 4095)) % 4095 - EncoderPos > -ErrorThreshold ) && ((StepperOffset + map(StepperPos, 1, StepsPerRevolution, 0, 4095)) % 4095 - EncoderPos < ErrorThreshold )) {
PosErrorCount = 0; //Wenn die Positionsabweichung innerhalb der erlaubten Toleranz ist, den Fehlerzähler auf 0 setzen
}
if (PosErrorCount > MaxAllowedError) { //Wenn die Fehlerzähler mehr als 5 ist, dann Fehlerprozedur aufrufen
StepperShake();
StepperFindOrigin();
PosErrorCount = 0;
};
}
//*************** Prozedur Positionsabgleich end ***************
}