Gdzie tu jest blad? #include "FastAccelStepper.h" // Biblioteka do sterowania silnikami…
- User
Gdzie tu jest blad?
#include "FastAccelStepper.h" // Biblioteka do sterowania silnikami krokowymi #include <Preferences.h> // Biblioteka do przechowywania ustawień #include <Arduino.h>
#define NUM_STEPPERS 3 // Liczba silników krokowych
const int limitSwitchPin1 = 7; // krańcówki const int limitSwitchPin2 = 6; const int limitSwitchPin3 = 5;
const long CALIBRATION_TOLERANCE = 10; volatile bool calibrationInProgress = false;
// Definicja struktury silnika krokowego struct StepperMotor { FastAccelStepper* stepper; int stepPin; int dirPin; int calibrationPin; int alarmPin; int stepperResolution; // Kroki na jednostkę (stopnie lub milimetry) int maxSpeed; int stepsMin; int stepsMax; long zeroOffset; long calibrationPosition; String name; bool dirHigh; bool calibrationDirectionForward; // Kierunek bazowania String speedPrefKey; String accelerationPrefKey; String posPrefKey; String zeroOffsetPrefKey; String emergencyPosPrefKey; int speed; int acceleration; bool speedModifiedForEncoder; int prevSpeed; int prevAcceleration; int calibrationSpeedFactor; int calibrationMoveAwaySteps; bool calibrationRunInitialDirectionForward; bool calibrationSecondMoveDirectionForward; int calibrationLimitSwitchActiveState; int calibrationDelayBetweenMoves; int calibrationLoopDelay; int encoderSpeed; int encoderAcceleration; long targetPosition; unsigned long previousMillis; unsigned long previousMillisNewZero;
// Wędka - prędkości enum SpeedMode { VERY_FAST, FAST, MEDIUM_FAST, MEDIUM, SLOW, VERY_SLOW };
SpeedMode currentSpeedMode; // Aktualny tryb prędkości };
Preferences preferences; StepperMotor steppers[NUM_STEPPERS]; FastAccelStepperEngine engine = FastAccelStepperEngine();
// Definicje pinów // const int safetyButton = 19; //Wylaczone chwilowo const int encoderPinA = 42; const int encoderPinB = 41; const int XaxisPin = 38; const int YaxisPin = 37; const int ZaxisPin = 36; const int multiplier1Pin = 35; const int multiplier10Pin = 45;
// Zmienne globalne int pinAstateCurrent = LOW; int pinAStateLast = LOW;
const unsigned long interval = 100; const unsigned long intervalNewZero = 100;
bool processEncoderActive = false; // bool safetyLocked = false; // Flaga blokady bezpieczeństwa //wylaczone chwilowo
// Flaga trybu LIVE volatile bool liveModeActive = false;
int liveDelayOffset = 5; // delay po wyświetleniu P10 int liveDelayAbsolute = 5; // delay po wyświetleniu P1
void checkLimitSwitchConnectivity(); void stopStepperMotor(StepperMotor& motor); void stopAllSteppers(); void checkStopInLoop();
void checkLimitSwitchConnectivity() { if (calibrationInProgress) return;
const int indices[] = { 0, 2 }; // silniki M (0) i L (2) for (int idx : indices) { StepperMotor& m = steppers[idx]; long pos = m.stepper->getCurrentPosition(); int pinState = digitalRead(m.calibrationPin);
// jeśli jesteśmy dalej niż tolerancja od pozycji kalibracyjnej... if (abs(pos - m.calibrationPosition) > CALIBRATION_TOLERANCE) { // ...i pin wskazuje na aktywację (LOW) => rozłączenie/przerwanie if (pinState == m.calibrationLimitSwitchActiveState) { Serial.print("Krańcówka silnika "); Serial.print(m.name); Serial.println(" ROZŁĄCZONA!"); } }} }
void startLive(float degrees) {
if (liveModeActive) return;
long steps = long(degrees * steppers[0].stepperResolution); long currentPos = steppers[0].stepper->getCurrentPosition(); long targetPos = currentPos + steps;
if (targetPos < steppers[0].stepsMin || targetPos > steppers[0].stepsMax) { Serial.println("LIVE: ruch poza zakresem—nie wykonano."); return; }
liveModeActive = true; steppers[0].stepper->move(steps);
while (steppers[0].stepper->isRunning()) { checkStopInLoop(); enforceLimits(); safetyCheck(); { long stepsOff = steppers[0].stepper->getCurrentPosition() - steppers[0].zeroOffset; float degOff = float(stepsOff) / steppers[0].stepperResolution; Serial.print("P10: "); Serial.println(degOff, 3); delay(liveDelayOffset); } { long stepsAbs = steppers[0].stepper->getCurrentPosition(); float degAbs = float(stepsAbs) / steppers[0].stepperResolution; Serial.print("P1: "); Serial.println(degAbs, 3); delay(liveDelayAbsolute); } } liveModeActive = false; }
void setup() { Serial.begin(460800); setupPins(); engine.init(); preferences.begin("stepper", false); setupSteppers(); readPreferences(); Serial.println("Setup zakończony."); }
void loop() { checkStopInLoop(); if (liveModeActive) { return; } /* if (!safetyLocked) { enforceLimits(); safetyCheck(); checkLimitSwitchConnectivity(); if (processEncoderActive) { processEncoderAndMoveSteppers(); } if (Serial.available() > 0) { processSerialCommand(); } } else { if (Serial.available() > 0) { String command = Serial.readStringUntil('\n'); command.trim(); if (command == "Odblokuj") { unlockSafety(); } else if (command == "reset") { resetESP32(); } else { Serial.println("System zablokowany. Użyj komendy 'Odblokuj' lub 'reset'."); } } } } */ //Obsługa enkodera i komend niewyłączona if (processEncoderActive) { processEncoderAndMoveSteppers(); } if (Serial.available() > 0) { processSerialCommand(); } }
// Funkcje konfigurujące void setupPins() { // pinMode(safetyButton, INPUT_PULLUP); //czasowo wylaczone pinMode(encoderPinA, INPUT); pinMode(encoderPinB, INPUT); pinMode(XaxisPin, INPUT_PULLDOWN); pinMode(YaxisPin, INPUT_PULLDOWN); pinMode(ZaxisPin, INPUT_PULLDOWN); pinMode(multiplier1Pin, INPUT_PULLDOWN); pinMode(multiplier10Pin, INPUT_PULLDOWN); pinMode(limitSwitchPin1, INPUT_PULLUP); pinMode(limitSwitchPin2, INPUT_PULLUP); pinMode(limitSwitchPin3, INPUT_PULLUP);
/* for (int i = 0; i < NUM_STEPPERS; ++i) { pinMode(steppers[i].alarmPin, INPUT_PULLDOWN); } */ //zakomentowane alarmy }
void setupSteppers() { steppers[0] = { .stepper = nullptr, .stepPin = 3, .dirPin = 8, .calibrationPin = 7, .alarmPin = 46, .stepperResolution = 320, .maxSpeed = 64000, .stepsMin = -33024, .stepsMax = 38400, .zeroOffset = 0, .calibrationPosition = -33024, .name = "M", .dirHigh = false, .calibrationDirectionForward = false, .speedPrefKey = "speed1", .accelerationPrefKey = "acceleration1", .posPrefKey = "pos1", .zeroOffsetPrefKey = "zeroOffset1", .emergencyPosPrefKey = "emergencyPos1", .speed = 0, .acceleration = 0, .calibrationSpeedFactor = 50, .calibrationMoveAwaySteps = 1000, .calibrationRunInitialDirectionForward = false, .calibrationSecondMoveDirectionForward = false, .calibrationLimitSwitchActiveState = LOW, .calibrationDelayBetweenMoves = 500, .calibrationLoopDelay = 0, .encoderSpeed = 32000, .encoderAcceleration = 16000, .targetPosition = 0, .previousMillis = 0, .previousMillisNewZero = 0 };
steppers[1] = { .stepper = nullptr, .stepPin = 16, .dirPin = 15, .calibrationPin = 6, .alarmPin = 9, .stepperResolution = 320, .maxSpeed = 32000, .stepsMin = -32000, .stepsMax = 0, .zeroOffset = 0, .calibrationPosition = 0, .name = "N", .dirHigh = false, .calibrationDirectionForward = true, .speedPrefKey = "speed2", .accelerationPrefKey = "acceleration2", .posPrefKey = "pos2", .zeroOffsetPrefKey = "zeroOffset2", .emergencyPosPrefKey = "emergencyPos2", .speed = 0, .acceleration = 0, .calibrationSpeedFactor = 25, .calibrationMoveAwaySteps = -1600, .calibrationRunInitialDirectionForward = true, .calibrationSecondMoveDirectionForward = true, .calibrationLimitSwitchActiveState = HIGH, .calibrationDelayBetweenMoves = 500, .calibrationLoopDelay = 0, .encoderSpeed = 32000, .encoderAcceleration = 32000, .targetPosition = 0, .previousMillis = 0, .previousMillisNewZero = 0 };
steppers[2] = { .stepper = nullptr, .stepPin = 18, .dirPin = 17, .calibrationPin = 5, .alarmPin = 10, .stepperResolution = 260, .maxSpeed = 26000, .stepsMin = -31200, .stepsMax = 31200, .zeroOffset = 0, .calibrationPosition = -31200, .name = "L", .dirHigh = true, .calibrationDirectionForward = false, .speedPrefKey = "speed3", .accelerationPrefKey = "acceleration3", .posPrefKey = "pos3", .zeroOffsetPrefKey = "zeroOffset3", .emergencyPosPrefKey = "emergencyPos3", .speed = 0, .acceleration = 0, .calibrationSpeedFactor = 20, .calibrationMoveAwaySteps = 2600, .calibrationRunInitialDirectionForward = false, .calibrationSecondMoveDirectionForward = false, .calibrationLimitSwitchActiveState = HIGH, .calibrationDelayBetweenMoves = 500, .calibrationLoopDelay = 500, .encoderSpeed = 32000, .encoderAcceleration = 32000, .targetPosition = 0, .previousMillis = 0, .previousMillisNewZero = 0 };
for (int i = 0; i < NUM_STEPPERS; ++i) { steppers[i].stepper = engine.stepperConnectToPin(steppers[i].stepPin); if (steppers[i].stepper) { steppers[i].stepper->setDirectionPin(steppers[i].dirPin, steppers[i].dirHigh); steppers[i].stepper->setAcceleration(steppers[i].acceleration); steppers[i].stepper->setSpeedInHz(steppers[i].speed); steppers[i].stepper->setCurrentPosition(steppers[i].targetPosition); steppers[i].targetPosition = steppers[i].stepper->getCurrentPosition(); steppers[i].currentSpeedMode = StepperMotor::MEDIUM; Serial.print("Silnik "); Serial.print(steppers[i].name); Serial.println(" zainicjalizowany."); } else { Serial.print("Błąd inicjalizacji silnika "); Serial.println(steppers[i].name); } } }
void readPreferences() { for (int i = 0; i < NUM_STEPPERS; ++i) { StepperMotor& motor = steppers[i]; motor.speed = preferences.getInt(motor.speedPrefKey.c_str(), motor.maxSpeed / 10); motor.acceleration = preferences.getInt(motor.accelerationPrefKey.c_str(), motor.speed / 2); motor.prevSpeed = motor.speed; motor.prevAcceleration = motor.acceleration; motor.speedModifiedForEncoder = false; long pos = preferences.getLong(motor.posPrefKey.c_str(), 0); motor.zeroOffset = preferences.getLong(motor.zeroOffsetPrefKey.c_str(), 0);
if (motor.stepper) { motor.stepper->setSpeedInHz(motor.speed); motor.stepper->setAcceleration(motor.acceleration); motor.stepper->setCurrentPosition(pos); motor.targetPosition = pos; Serial.print("Preferencje odczytane dla silnika "); Serial.println(motor.name); }} }
void enforceLimits() { for (int i = 0; i < NUM_STEPPERS; ++i) { StepperMotor& motor = steppers[i]; long pos = motor.stepper->getCurrentPosition(); if (pos < motor.stepsMin || pos > motor.stepsMax) { motor.stepper->forceStop(); if (pos < motor.stepsMin) { motor.stepper->setCurrentPosition(motor.stepsMin); motor.targetPosition = motor.stepsMin; Serial.print("Silnik "); Serial.print(motor.name); Serial.println(" przekroczył minimalny zakres i został ustawiony na stepsMin."); } else { motor.stepper->setCurrentPosition(motor.stepsMax); motor.targetPosition = motor.stepsMax; Serial.print("Silnik "); Serial.print(motor.name); Serial.println(" przekroczył maksymalny zakres i został ustawiony na stepsMax."); } Serial.print("Silnik "); Serial.print(motor.name); Serial.println(" został zatrzymany z powodu przekroczenia granic."); } } }
void safetyCheck() { /* bool safetyTriggered = false; if (digitalRead(safetyButton) == LOW) { safetyTriggered = true; } for (int i = 0; i < NUM_STEPPERS; ++i) { if (digitalRead(steppers[i].alarmPin) == LOW) { safetyTriggered = true; } } if (safetyTriggered) { for (int i = 0; i < NUM_STEPPERS; ++i) { steppers[i].stepper->forceStop(); long pos = steppers[i].stepper->getCurrentPosition(); preferences.putLong(steppers[i].posPrefKey.c_str(), pos); preferences.putLong(steppers[i].emergencyPosPrefKey.c_str(), pos); steppers[i].targetPosition = pos; } Serial.println("Silniki zatrzymane z powodu alarmu bezpieczeństwa. Proszę odblokować lub zresetować system."); safetyLocked = true; } */ }
void unlockSafety() { /* if (safetyLocked) { safetyLocked = false; Serial.println("System odblokowany. Możesz kontynuować pracę."); } else { Serial.println("System nie jest zablokowany."); } */ }
void resetESP32() { Serial.println("Restartowanie ESP32..."); delay(1000); ESP.restart(); }
void processEncoderAndMoveSteppers() { checkStopInLoop(); pinAstateCurrent = digitalRead(encoderPinA); static long stepsForSteppers[NUM_STEPPERS] = { 0 }; static unsigned long lastEncoderEventTime = 0; static unsigned long prevEncoderEventTime = 0; static unsigned long deltaT = 1000;
if (pinAStateLast == LOW && pinAstateCurrent == HIGH) { int stepIncrement = (digitalRead(encoderPinB) == HIGH) ? -1 : 1; prevEncoderEventTime = lastEncoderEventTime; lastEncoderEventTime = millis(); if (prevEncoderEventTime != 0) { deltaT = lastEncoderEventTime - prevEncoderEventTime; } for (int i = 0; i < NUM_STEPPERS; ++i) { stepsForSteppers[i] += stepIncrement * steppers[i].stepperResolution; } } pinAStateLast = pinAstateCurrent;
for (int i = 0; i < NUM_STEPPERS; ++i) { if (stepsForSteppers[i] != 0) { int axisPin = (i == 0 ? XaxisPin : i == 1 ? YaxisPin : ZaxisPin); if (digitalRead(axisPin) == HIGH) { float multiplier = 1.0; if (digitalRead(multiplier1Pin) == HIGH) multiplier = 0.1; else if (digitalRead(multiplier10Pin) == HIGH) multiplier = 0.01;
long movement = stepsForSteppers[i] * multiplier; long newTargetPosition = steppers[i].targetPosition + movement; if (newTargetPosition >= steppers[i].stepsMin && newTargetPosition <= steppers[i].stepsMax) { // histereza... StepperMotor::SpeedMode newMode = steppers[i].currentSpeedMode; const unsigned long veryFastThreshold = 30; const unsigned long fastThreshold = 50; const unsigned long mediumFastThreshold = 100; const unsigned long mediumThreshold = 150; const unsigned long slowThreshold = 200; switch (steppers[i].currentSpeedMode) { case StepperMotor::VERY_FAST: if (deltaT > veryFastThreshold) newMode = StepperMotor::FAST; break; case StepperMotor::FAST: if (deltaT > fastThreshold) newMode = StepperMotor::MEDIUM_FAST; else if (deltaT <= veryFastThreshold) newMode = StepperMotor::VERY_FAST; break; case StepperMotor::MEDIUM_FAST: if (deltaT > mediumFastThreshold) newMode = StepperMotor::MEDIUM; else if (deltaT <= fastThreshold) newMode = StepperMotor::FAST; break; case StepperMotor::MEDIUM: if (deltaT > mediumThreshold) newMode = StepperMotor::SLOW; else if (deltaT <= mediumFastThreshold) newMode = StepperMotor::MEDIUM_FAST; break; case StepperMotor::SLOW: if (deltaT > slowThreshold) newMode = StepperMotor::VERY_SLOW; else if (deltaT <= mediumThreshold) newMode = StepperMotor::MEDIUM; break; case StepperMotor::VERY_SLOW: if (deltaT <= slowThreshold) newMode = StepperMotor::SLOW; break; default: newMode = StepperMotor::MEDIUM; } if (newMode != steppers[i].currentSpeedMode) { steppers[i].currentSpeedMode = newMode; steppers[i].speedModifiedForEncoder = true; } int newSpeed, newAcceleration; switch (steppers[i].currentSpeedMode) { case StepperMotor::VERY_FAST: newSpeed = steppers[i].encoderSpeed; newAcceleration = steppers[i].encoderAcceleration; break; case StepperMotor::FAST: newSpeed = steppers[i].encoderSpeed / 2; newAcceleration = steppers[i].encoderAcceleration / 2; break; case StepperMotor::MEDIUM_FAST: newSpeed = steppers[i].encoderSpeed / 6; newAcceleration = steppers[i].encoderAcceleration / 4; break; case StepperMotor::MEDIUM: newSpeed = steppers[i].encoderSpeed / 8; newAcceleration = steppers[i].encoderAcceleration / 6; break; case StepperMotor::SLOW: newSpeed = steppers[i].encoderSpeed / 10; newAcceleration = steppers[i].encoderAcceleration / 8; break; case StepperMotor::VERY_SLOW: newSpeed = steppers[i].encoderSpeed / 12; newAcceleration = steppers[i].encoderAcceleration / 10; break; default: newSpeed = steppers[i].encoderSpeed; newAcceleration = steppers[i].encoderAcceleration; } if (steppers[i].speedModifiedForEncoder) { steppers[i].prevSpeed = steppers[i].speed; steppers[i].prevAcceleration = steppers[i].acceleration; steppers[i].speedModifiedForEncoder = false; } if (steppers[i].speed != newSpeed || steppers[i].acceleration != newAcceleration) { steppers[i].speed = newSpeed; steppers[i].acceleration = newAcceleration; steppers[i].stepper->setSpeedInHz(newSpeed); steppers[i].stepper->setAcceleration(newAcceleration); } steppers[i].targetPosition = newTargetPosition; steppers[i].stepper->move(movement); } stepsForSteppers[i] = 0; } } // przywracanie oryginalnych prędkości po zakończeniu ruchu if (steppers[i].speedModifiedForEncoder && !steppers[i].stepper->isRunning()) { steppers[i].speed = steppers[i].prevSpeed; steppers[i].acceleration = steppers[i].prevAcceleration; steppers[i].stepper->setSpeedInHz(steppers[i].speed); steppers[i].stepper->setAcceleration(steppers[i].acceleration); steppers[i].speedModifiedForEncoder = false; }} }
void powP10(int times = 500) { for (int i = 0; i < times; ++i) { steppers[0].previousMillisNewZero = 0; Serial.print("P10: "); printPositionZero(steppers[0]); delay(intervalNewZero); } }
void printLimits(StepperMotor& motor) { float minPos = (float)motor.stepsMin / motor.stepperResolution; float maxPos = (float)motor.stepsMax / motor.stepperResolution; Serial.print("Silnik "); Serial.print(motor.name); Serial.print(" — min: "); Serial.print(minPos, 3); Serial.print(" max: "); Serial.println(maxPos, 3); }
void limitsAll() { Serial.println("=== Limity wszystkich silników ==="); printLimits(steppers[0]); printLimits(steppers[1]); printLimits(steppers[2]); Serial.println("=================================="); }
void processSerialCommand() { String command = Serial.readStringUntil('\n'); command.trim(); if (command == "STOP1") { stopStepperMotor(steppers[0]); return; } else if (command == "STOP2") { stopStepperMotor(steppers[1]); return; } else if (command == "STOP3") { stopStepperMotor(steppers[2]); return; } else if (command == "STOPALL") { stopAllSteppers(); return; } else if (command.startsWith("speed1")) { Serial.print("speed1: "); float speed = command.substring(6).toFloat(); changeSpeed(steppers[0], speed); } else if (command.startsWith("speed2")) { Serial.print("speed2: "); float speed = command.substring(6).toFloat(); changeSpeed(steppers[1], speed); } else if (command.startsWith("speed3")) { Serial.print("speed3: "); float speed = command.substring(6).toFloat(); changeSpeed(steppers[2], speed); } else if (command.startsWith("acceleration1")) { Serial.print("acceleration1: "); int acc = command.substring(13).toInt(); changeAcceleration(steppers[0], acc); } else if (command.startsWith("acceleration2")) { Serial.print("acceleration2: "); int acc = command.substring(13).toInt(); changeAcceleration(steppers[1], acc); } else if (command.startsWith("acceleration3")) { Serial.print("acceleration3: "); int acc = command.substring(13).toInt(); changeAcceleration(steppers[2], acc); } else if (command == "H") { Serial.print("H: "); calibration(steppers[0]); } else if (command == "J") { Serial.print("J: "); calibration(steppers[1]); } else if (command == "G") { Serial.print("G: "); calibration(steppers[2]); } else if (command == "H1") { Serial.print("H1: "); returnToZero(steppers[0]); } else if (command == "J1") { Serial.print("J1: "); returnToZero(steppers[1]); } else if (command == "G1") { Serial.print("G1: "); returnToZero(steppers[2]); } else if (command == "H2") { Serial.print("H2: "); returnToOffsetZero(steppers[0]); } else if (command == "J2") { Serial.print("J2: "); returnToOffsetZero(steppers[1]); } else if (command == "G2") { Serial.print("G2: "); returnToOffsetZero(steppers[2]); } else if (command == "H3") { Serial.print("H3: "); returnToEmergencyPosition(steppers[0]); } else if (command == "J3") { Serial.print("J3: "); returnToEmergencyPosition(steppers[1]); } else if (command == "G3") { Serial.print("G3: "); returnToEmergencyPosition(steppers[2]); } else if (command == "C") { Serial.print("C: "); calibrationAll(); } else if (command == "R") { Serial.print("R: "); returnToZeroAll(); } else if (command == "T") { Serial.print("T: "); returnToOffsetZeroAll(); } else if (command == "Y") { Serial.print("Y: "); returnToEmergencyPositionAll(); } else if (command == "A") { Serial.print("A: "); setNewZero(steppers[0]); } else if (command == "S") { Serial.print("S: "); setNewZero(steppers[1]); } else if (command == "D") { Serial.print("D: "); setNewZero(steppers[2]); } else if (command == "P") { positionAll(); } else if (command == "P1") { Serial.print("P1: "); printPosition(steppers[0]); } else if (command == "P2") { Serial.print("P2: "); printPosition(steppers[1]); } else if (command == "P3") { Serial.print("P3: "); printPosition(steppers[2]); } else if (command == "P0") { positionAllZero(); } else if (command == "P10") { Serial.print("P10: "); printPositionZero(steppers[0]); } else if (command == "P20") { Serial.print("P20: "); printPositionZero(steppers[1]); } else if (command == "P30") { Serial.print("P30: "); printPositionZero(steppers[2]); } else if (command == "EP") { positionAllEmergency(); } else if (command == "EP1") { Serial.print("EP1: "); printEmergencyPosition(steppers[0]); } else if (command == "EP2") { Serial.print("EP2: "); printEmergencyPosition(steppers[1]); } else if (command == "EP3") { Serial.print("EP3: "); printEmergencyPosition(steppers[2]); } else if (command == "L") { limitsAll(); } else if (command == "L1") { Serial.print("L1: "); printLimits(steppers[0]); } else if (command == "L2") { Serial.print("L2: "); printLimits(steppers[1]); } else if (command == "L3") { Serial.print("L3: "); printLimits(steppers[2]);
else if (command == "F") { Serial.print("F: "); processEncoderActive = true; Serial.println("Jog activate"); } else if (command == "F1") { Serial.print("F1: "); processEncoderActive = false; Serial.println("Jog deactivate"); } else if (command == "X") { Serial.print("X: "); checkSafety(); } else if (command.equalsIgnoreCase("Odblokuj")) { unlockSafety(); } else if (command.equalsIgnoreCase("reset")) { resetESP32(); } else if (command == "baza1") { setCurrentPositionToCalibration(steppers[0]); } else if (command == "baza2") { setCurrentPositionToCalibration(steppers[1]); } else if (command == "baza3") { setCurrentPositionToCalibration(steppers[2]); } else if (command.startsWith("getSpeed1")) { Serial.print("getSpeed1: "); getSpeed(steppers[0]); } else if (command.startsWith("getSpeed2")) { Serial.print("getSpeed2: "); getSpeed(steppers[1]); } else if (command.startsWith("getSpeed3")) { Serial.print("getSpeed3: "); getSpeed(steppers[2]); } else if (command.startsWith("getMaxSpeed1")) { Serial.print("maxSpeedStepper1: "); getMaxSpeed(steppers[0]); } else if (command.startsWith("getMaxSpeed2")) { Serial.print("maxSpeedStepper2: "); getMaxSpeed(steppers[1]); } else if (command.startsWith("getMaxSpeed3")) { Serial.print("maxSpeedStepper3: "); getMaxSpeed(steppers[2]); } else if (command.startsWith("move1Steps")) { int steps = command.substring(11).toInt(); moveStepperSteps(steppers[0], steps); } else if (command.startsWith("move2Steps")) { int steps = command.substring(11).toInt(); moveStepperSteps(steppers[1], steps); } else if (command.startsWith("move3Steps")) { int steps = command.substring(11).toInt(); moveStepperSteps(steppers[2], steps); } else if (command.startsWith("LIVE")) { float deg = command.substring(4).toFloat(); Serial.print("Starting LIVE for "); Serial.print(deg, 2); Serial.println(" degrees"); startLive(deg); } else if (command == "P10R") { powP10(500); } else if (command.startsWith("liveDelayOffset")) { int v = command.substring(16).toInt(); liveDelayOffset = v; Serial.print("Ustawiono liveDelayOffset = "); Serial.println(liveDelayOffset); } else if (command.startsWith("liveDelayAbsolute")) { int v = command.substring(18).toInt(); liveDelayAbsolute = v; Serial.print("Ustawiono liveDelayAbsolute = "); Serial.println(liveDelayAbsolute); } else { controlStepperMotor(command); }}
void controlStepperMotor(String command) { if (command.length() < 3) { Serial.println("Niepoprawny format komendy."); return; } checkStopInLoop(); char motorId = command.charAt(0); float value = command.substring(2).toFloat(); Serial.println(String(motorId) + ": " + String(value)); for (int i = 0; i < NUM_STEPPERS; ++i) { checkStopInLoop(); if (steppers[i].name.charAt(0) == motorId) { long steps = value * steppers[i].stepperResolution; long newTargetPosition = steppers[i].targetPosition + steps; if (newTargetPosition >= steppers[i].stepsMin && newTargetPosition <= steppers[i].stepsMax) { steppers[i].targetPosition = newTargetPosition; steppers[i].stepper->move(steps); while (steppers[i].stepper->isRunning()) { checkStopInLoop(); // <-- tu zatrzymaj silnik, jeśli padnie STOP delay(1); // krótki oddech, żeby nie zablokować całkowicie CPU } } else { Serial.print("Ruch poza zakresem dla silnika "); Serial.println(steppers[i].name); } break; } } }
void changeSpeed(StepperMotor & motor, float newSpeed) { int speedInHz = motor.stepperResolution * newSpeed; if (speedInHz > motor.maxSpeed) { speedInHz = motor.maxSpeed; Serial.println("maxSpeedStepper: " + String(motor.maxSpeed / motor.stepperResolution)); } else { Serial.println(newSpeed); } motor.speed = speedInHz; motor.acceleration = speedInHz / 2; motor.stepper->setSpeedInHz(motor.speed); motor.stepper->setAcceleration(motor.acceleration); preferences.putInt(motor.speedPrefKey.c_str(), motor.speed); preferences.putInt(motor.accelerationPrefKey.c_str(), motor.acceleration); }
void changeAcceleration(StepperMotor & motor, int newAcceleration) { motor.acceleration = newAcceleration; motor.stepper->setAcceleration(motor.acceleration); preferences.putInt(motor.accelerationPrefKey.c_str(), motor.acceleration); Serial.println(newAcceleration); }
void calibration(StepperMotor & motor) { calibrationInProgress = true; int calibrationSpeed = motor.stepperResolution * motor.calibrationSpeedFactor; int calibrationAcceleration = calibrationSpeed / 2; motor.stepper->setSpeedInHz(calibrationSpeed); motor.stepper->setAcceleration(calibrationAcceleration); motor.stepper->applySpeedAcceleration(); /* if (digitalRead(safetyButton) == LOW || digitalRead(motor.alarmPin) == LOW || digitalRead(steppers[0].alarmPin) == LOW || digitalRead(steppers[1].alarmPin) == LOW || digitalRead(steppers[2].alarmPin) == LOW) { safetyCheck(); return; } */ motor.stepper->moveTo(motor.calibrationPosition); while (motor.stepper->isRunning()) { checkStopInLoop();
/* if (digitalRead(safetyButton) == LOW || digitalRead(motor.alarmPin) == LOW || digitalRead(steppers[0].alarmPin) == LOW || digitalRead(steppers[1].alarmPin) == LOW || digitalRead(steppers[2].alarmPin) == LOW) { safetyCheck(); return; } */ if (digitalRead(motor.calibrationPin) == motor.calibrationLimitSwitchActiveState) { motor.stepper->forceStop(); break; } delay(motor.calibrationLoopDelay); } motor.stepper->forceStopAndNewPosition(motor.calibrationPosition); motor.targetPosition = motor.calibrationPosition; delay(motor.calibrationDelayBetweenMoves); motor.stepper->move(motor.calibrationMoveAwaySteps); while (motor.stepper->isRunning()) { checkStopInLoop(); /* if (digitalRead(safetyButton) == LOW || digitalRead(motor.alarmPin) == LOW || digitalRead(steppers[0].alarmPin) == LOW || digitalRead(steppers[1].alarmPin) == LOW || digitalRead(steppers[2].alarmPin) == LOW) { safetyCheck(); return; } */ delay(motor.calibrationLoopDelay); } motor.targetPosition += motor.calibrationMoveAwaySteps; motor.stepper->setSpeedInHz(calibrationSpeed / 2); motor.stepper->setAcceleration(calibrationAcceleration / 4); motor.stepper->applySpeedAcceleration(); if (motor.calibrationSecondMoveDirectionForward) { motor.stepper->runForward(); } else { motor.stepper->runBackward(); } while (digitalRead(motor.calibrationPin) != motor.calibrationLimitSwitchActiveState) { checkStopInLoop(); /* if (digitalRead(safetyButton) == LOW || digitalRead(motor.alarmPin) == LOW || digitalRead(steppers[0].alarmPin) == LOW || digitalRead(steppers[1].alarmPin) == LOW || digitalRead(steppers[2].alarmPin) == LOW) { safetyCheck(); return; } */ delay(motor.calibrationLoopDelay); } motor.stepper->forceStopAndNewPosition(motor.calibrationPosition); motor.targetPosition = motor.calibrationPosition; motor.stepper->setSpeedInHz(motor.speed); motor.stepper->setAcceleration(motor.acceleration); motor.stepper->applySpeedAcceleration(); printPosition(motor); calibrationInProgress = false;}
void calibrationAll() { calibrationInProgress = true; for (int i = 0; i < NUM_STEPPERS; ++i) { StepperMotor& motor = steppers[i]; int calibrationSpeed = motor.stepperResolution * motor.calibrationSpeedFactor; int calibrationAcceleration = calibrationSpeed / 2; motor.stepper->setSpeedInHz(calibrationSpeed); motor.stepper->setAcceleration(calibrationAcceleration); motor.stepper->applySpeedAcceleration(); } /* if (digitalRead(safetyButton) == LOW) { safetyCheck(); return; } for (int i = 0; i < NUM_STEPPERS; ++i) { if (digitalRead(steppers[i].alarmPin) == LOW) { safetyCheck(); return; } } / for (int i = 0; i < NUM_STEPPERS; ++i) { steppers[i].stepper->moveTo(steppers[i].calibrationPosition); } bool allRunning; do { checkStopInLoop(); allRunning = false; for (int i = 0; i < NUM_STEPPERS; ++i) { if (steppers[i].stepper->isRunning()) { allRunning = true; / if (digitalRead(safetyButton) == LOW || digitalRead(steppers[i].alarmPin) == LOW) { safetyCheck(); return; } / if (digitalRead(steppers[i].calibrationPin) == steppers[i].calibrationLimitSwitchActiveState) { steppers[i].stepper->forceStop(); } } } for (int i = 0; i < NUM_STEPPERS; ++i) { if (steppers[i].stepper->isRunning()) { delay(steppers[i].calibrationLoopDelay); } } } while (allRunning); { checkStopInLoop(); } for (int i = 0; i < NUM_STEPPERS; ++i) { steppers[i].stepper->forceStopAndNewPosition(steppers[i].calibrationPosition); steppers[i].targetPosition = steppers[i].calibrationPosition; } delay(steppers[0].calibrationDelayBetweenMoves); for (int i = 0; i < NUM_STEPPERS; ++i) { steppers[i].stepper->move(steppers[i].calibrationMoveAwaySteps); steppers[i].targetPosition += steppers[i].calibrationMoveAwaySteps; } bool anyRunning; do { checkStopInLoop(); anyRunning = false; for (int i = 0; i < NUM_STEPPERS; ++i) { if (steppers[i].stepper->isRunning()) { / anyRunning = true; if (digitalRead(safetyButton) == LOW || digitalRead(steppers[i].alarmPin) == LOW) { safetyCheck(); return; } / } } for (int i = 0; i < NUM_STEPPERS; ++i) { if (steppers[i].stepper->isRunning()) { delay(steppers[i].calibrationLoopDelay); } } } while (anyRunning); { checkStopInLoop(); } for (int i = 0; i < NUM_STEPPERS; ++i) { steppers[i].stepper->setSpeedInHz(steppers[i].calibrationSpeedFactor * steppers[i].stepperResolution / 2); steppers[i].stepper->setAcceleration(steppers[i].calibrationSpeedFactor * steppers[i].stepperResolution / 8); steppers[i].stepper->applySpeedAcceleration(); } for (int i = 0; i < NUM_STEPPERS; ++i) { if (steppers[i].calibrationSecondMoveDirectionForward) { steppers[i].stepper->runForward(); } else { steppers[i].stepper->runBackward(); } } bool calibrationComplete[NUM_STEPPERS] = { false }; while (true) { checkStopInLoop(); bool allComplete = true; for (int i = 0; i < NUM_STEPPERS; ++i) { if (!calibrationComplete[i]) { if (digitalRead(steppers[i].calibrationPin) == steppers[i].calibrationLimitSwitchActiveState) { steppers[i].stepper->forceStopAndNewPosition(steppers[i].calibrationPosition); steppers[i].targetPosition = steppers[i].calibrationPosition; calibrationComplete[i] = true; } / if (digitalRead(safetyButton) == LOW || digitalRead(steppers[i].alarmPin) == LOW) { safetyCheck(); return; } */ if (!calibrationComplete[i]) { allComplete = false; delay(steppers[i].calibrationLoopDelay); } } } if (allComplete) break; } for (int i = 0; i < NUM_STEPPERS; ++i) { steppers[i].stepper->setSpeedInHz(steppers[i].speed); steppers[i].stepper->setAcceleration(steppers[i].acceleration); steppers[i].stepper->applySpeedAcceleration(); } positionAll(); calibrationInProgress = false; }
void returnToZero(StepperMotor & motor) { /* if (digitalRead(safetyButton) == LOW || digitalRead(motor.alarmPin) == LOW) { safetyCheck(); return; } */ motor.stepper->moveTo(0); motor.targetPosition = 0; }
void returnToZeroAll() { for (int i = 0; i < NUM_STEPPERS; ++i) { /* if (digitalRead(safetyButton) == LOW || digitalRead(steppers[i].alarmPin) == LOW) { safetyCheck(); return; } / } for (int i = 0; i < NUM_STEPPERS; ++i) { steppers[i].stepper->moveTo(0); steppers[i].targetPosition = 0; } while (steppers[0].stepper->isRunning() || steppers[1].stepper->isRunning() || steppers[2].stepper->isRunning()) { checkStopInLoop(); for (int i = 0; i < NUM_STEPPERS; ++i) { / if (digitalRead(safetyButton) == LOW || digitalRead(steppers[i].alarmPin) == LOW) { safetyCheck(); return; } */ } } delay(500); for (int i = 0; i < NUM_STEPPERS; ++i) { printPosition(steppers[i]); } }
void returnToOffsetZero(StepperMotor & motor) { checkStopInLoop(); /* if (digitalRead(safetyButton) == LOW || digitalRead(motor.alarmPin) == LOW) { safetyCheck(); return; } */ motor.stepper->moveTo(motor.zeroOffset); motor.targetPosition = motor.zeroOffset; }
void returnToOffsetZeroAll() { for (int i = 0; i < NUM_STEPPERS; ++i) { /* if (digitalRead(safetyButton) == LOW || digitalRead(steppers[i].alarmPin) == LOW) { safetyCheck(); return; } / } for (int i = 0; i < NUM_STEPPERS; ++i) { steppers[i].stepper->moveTo(steppers[i].zeroOffset); steppers[i].targetPosition = steppers[i].zeroOffset; } while (steppers[0].stepper->isRunning() || steppers[1].stepper->isRunning() || steppers[2].stepper->isRunning()) { checkStopInLoop(); for (int i = 0; i < NUM_STEPPERS; ++i) { / if (digitalRead(safetyButton) == LOW || digitalRead(steppers[i].alarmPin) == LOW) { safetyCheck(); return; } */ } } delay(500); for (int i = 0; i < NUM_STEPPERS; ++i) { printPositionZero(steppers[i]); } }
void returnToEmergencyPosition(StepperMotor & motor) { checkStopInLoop(); /* if (digitalRead(safetyButton) == LOW || digitalRead(motor.alarmPin) == LOW) { safetyCheck(); return; } */ long emergencyPos = preferences.getLong(motor.emergencyPosPrefKey.c_str(), 0); motor.stepper->moveTo(emergencyPos); motor.targetPosition = emergencyPos; Serial.print("Silnik "); Serial.print(motor.name); Serial.print(" porusza się do pozycji awaryjnej: "); Serial.println(emergencyPos); }
void returnToEmergencyPositionAll() { for (int i = 0; i < NUM_STEPPERS; ++i) { /* if (digitalRead(safetyButton) == LOW || digitalRead(steppers[i].alarmPin) == LOW) { safetyCheck(); return; } / } for (int i = 0; i < NUM_STEPPERS; ++i) { long emergencyPos = preferences.getLong(steppers[i].emergencyPosPrefKey.c_str(), 0); steppers[i].stepper->moveTo(emergencyPos); steppers[i].targetPosition = emergencyPos; Serial.print("Silnik "); Serial.print(steppers[i].name); Serial.print(" porusza się do pozycji awaryjnej: "); Serial.println(emergencyPos); } while (steppers[0].stepper->isRunning() || steppers[1].stepper->isRunning() || steppers[2].stepper->isRunning()) { checkStopInLoop(); for (int i = 0; i < NUM_STEPPERS; ++i) { / if (digitalRead(safetyButton) == LOW || digitalRead(steppers[i].alarmPin) == LOW) { safetyCheck(); return; } */ } } delay(500); for (int i = 0; i < NUM_STEPPERS; ++i) { printEmergencyPosition(steppers[i]); } }
void setNewZero(StepperMotor & motor) { motor.zeroOffset = motor.stepper->getCurrentPosition(); preferences.putLong(motor.zeroOffsetPrefKey.c_str(), motor.zeroOffset); Serial.print("Ustawiono nowe zero dla silnika "); Serial.println(motor.name); }
void positionAll() { Serial.print("P1: "); printPosition(steppers[0]); Serial.print("P2: "); printPosition(steppers[1]); Serial.print("P3: "); printPosition(steppers[2]); }
void printPosition(StepperMotor & motor) { unsigned long currentMillis = millis(); if (currentMillis - motor.previousMillis >= interval) { motor.previousMillis = currentMillis; float position = (float)motor.stepper->getCurrentPosition() / motor.stepperResolution; Serial.println(position, 3); preferences.putLong(motor.posPrefKey.c_str(), motor.stepper->getCurrentPosition()); } }
void positionAllZero() { Serial.print("P10: "); printPositionZero(steppers[0]); Serial.print("P20: "); printPositionZero(steppers[1]); Serial.print("P30: "); printPositionZero(steppers[2]); }
void printPositionZero(StepperMotor & motor) { unsigned long currentMillis = millis(); if (currentMillis - motor.previousMillisNewZero >= intervalNewZero) { motor.previousMillisNewZero = currentMillis; float position = (float)(motor.stepper->getCurrentPosition() - motor.zeroOffset) / motor.stepperResolution; Serial.println(position, 3); } }
void positionAllEmergency() { Serial.print("EP1: "); printEmergencyPosition(steppers[0]); Serial.print("EP2: "); printEmergencyPosition(steppers[1]); Serial.print("EP3: "); printEmergencyPosition(steppers[2]); }
void printEmergencyPosition(StepperMotor & motor) { unsigned long currentMillis = millis(); if (currentMillis - motor.previousMillis >= interval) { motor.previousMillis = currentMillis; long emergencyPos = preferences.getLong(motor.emergencyPosPrefKey.c_str(), 0); float position = (float)emergencyPos / motor.stepperResolution; Serial.println(position, 3); } }
void getSpeed(StepperMotor & motor) { float speed = (float)motor.speed / motor.stepperResolution; Serial.println(speed); }
void getMaxSpeed(StepperMotor & motor) { float maxSpeed = (float)motor.maxSpeed / motor.stepperResolution; Serial.println(maxSpeed); }
void moveStepperSteps(StepperMotor & motor, int steps) { motor.stepper->move(steps); motor.targetPosition += steps; Serial.print("Silnik "); Serial.print(motor.name); Serial.print(" porusza się o "); Serial.print(steps); Serial.println(" kroków."); }
void setCurrentPositionToCalibration(StepperMotor & motor) { motor.stepper->forceStopAndNewPosition(motor.calibrationPosition); motor.targetPosition = motor.calibrationPosition; Serial.print("Pozycja silnika "); Serial.print(motor.name); Serial.print(" została ustawiona na pozycję kalibracyjną: "); Serial.println(motor.calibrationPosition); preferences.putLong(motor.posPrefKey.c_str(), motor.calibrationPosition); }
void checkSafety() { /* for (int i = 0; i < NUM_STEPPERS; ++i) { int alarmState = digitalRead(steppers[i].alarmPin); Serial.print("Stan alarmPin"); Serial.print(i + 1); Serial.print(": "); Serial.println(alarmState == LOW ? "LOW" : "HIGH"); } int safetyButtonState = digitalRead(safetyButton); Serial.print("Stan safetyButton: "); Serial.println(safetyButtonState == LOW ? "LOW" : "HIGH"); */ }
void updateTargetPositions() { for (int i = 0; i < NUM_STEPPERS; ++i) { if (!steppers[i].stepper->isRunning()) { steppers[i].targetPosition = steppers[i].stepper->getCurrentPosition(); Serial.print("Stepper "); Serial.print(steppers[i].name); Serial.print(" targetPosition updated to: "); Serial.println(steppers[i].targetPosition); } } }
void stopStepperMotor(StepperMotor & motor) { if (motor.stepper) { motor.stepper->forceStop(); motor.targetPosition = motor.stepper->getCurrentPosition(); Serial.print("Silnik "); Serial.print(motor.name); Serial.println(" zatrzymany."); } }
void stopAllSteppers() { for (int i = 0; i < NUM_STEPPERS; ++i) { if (steppers[i].stepper) { steppers[i].stepper->forceStop(); steppers[i].targetPosition = steppers[i].stepper->getCurrentPosition(); } } Serial.println("Wszystkie silniki zatrzymane."); }
void checkStopInLoop() { if (Serial.available() > 0) { String cmd = Serial.readStringUntil('\n'); cmd.trim(); if (cmd == "STOP1") { stopStepperMotor(steppers[0]); return; } else if (cmd == "STOP2") { stopStepperMotor(steppers[1]); return; } else if (cmd == "STOP3") { stopStepperMotor(steppers[2]); return; } else if (cmd == "STOPALL") { stopAllSteppers(); return; } // jeśli to nie STOP, oddajemy komendę do normalnego parsera processSerialCommand(); } checkLimitSwitchConnectivity(); }
- Assistant
Twoja analiza kodu jest bardzo szczegółowa, ale skoro pytasz o "błąd", to przyjrzę się temu dokładnie. Kod wygląda ogólnie dobrze, ale istnieje jeden poważny błąd strukturalny, który może powodować crash lub nieprzewidywalne zachowanie.
🔴 Błąd: Zła lokalizacja definicji funkcji
controlStepperMotorMiejsce błędu:
void processSerialCommand() { ... else if (command.startsWith("liveDelayAbsolute")) { ... } else { controlStepperMotor(command); } } void controlStepperMotor(String command) { ... }Co się dzieje?
W C++ wszystkie funkcje muszą być zadeklarowane lub zdefiniowane przed ich użyciem. W Twoim przypadku:
- Funkcja
controlStepperMotor()jest zdefiniowana wewnątrz blokuelsewprocessSerialCommand(). - To jest niedozwolone w standardowym C++ – nie można definiować nowych funkcji w innych funkcjach.
- Ten kod skompiluje się tylko dzięki specyficznemu trybowi kompilatora Arduino, ale może powodować nieprzewidywalne efekty lub nie działać w niektórych IDE czy wersjach kompilatora.
✅ Rozwiązanie: Przenieś definicję
controlStepperMotor()pozaprocessSerialCommand()Krok po kroku:
-
Znajdź funkcję
controlStepperMotor(...)– kończy się tuż za:} while (allComplete); -
Wytnij tę całą funkcję od:
void controlStepperMotor(String command) { ... } -
Wklej ją przed funkcją
setup(), najlepiej zaraz po deklaracjach prototypów funkcji, np.:void checkLimitSwitchConnectivity(); void stopStepperMotor(StepperMotor& motor); void stopAllSteppers(); void checkStopInLoop(); // Dodaj tutaj: void controlStepperMotor(String command); // Deklaracja #endif -
A potem umieść pełną definicję tej funkcji gdzieś pod
#include'ami i przedsetup():void controlStepperMotor(String command) { ... } -
Usuń tę funkcję z wnętrza
processSerialCommand()– tam już jej nie wolno mieć.
🛠️ Dlaczego to ważne?
- W standardowym języku C++ funkcje nie mogą być zagnieżdżone.
- Niektóre kompilatory Arduino pozwalają na to jako rozszerzenie GCC, ale to nieportowalne i ryzykowne.
- Może prowadzić do błędów typu:
undefined reference- Crash'y przy uruchomieniu
- Błędy przy aktualizacji bibliotek lub środowiska
✅ Inne możliwe problemy (opcjonalne poprawki):
-
Brak
pinMode()dla alarmPinówpinMode(steppers[i].alarmPin, INPUT_PULLDOWN);→ zakomentowane, ale jeśli są używane, trzeba je odkomentować.
-
Brak obsługi timeout'u w
calibration()→ Dobrze byłoby dodać maksymalny czas oczekiwania na krańcówkę, żeby uniknąć zawieszenia. -
Zmienne globalne
pinAStateLast,pinAstateCurrentitp. → Sprawdź, czy są poprawnie synchronizowane w przerwaniach, jeśli enkoder ma przerwanie.
✅ Podsumowanie
Typ błędu Opis 🔴 Krytyczny błąd składniowy controlStepperMotor()zdefiniowana w środku innej funkcji✅ Poprawka Wyjmij controlStepperMotor()i umieść ją globalnie⚠️ Opcjonalne Sprawdź obsługę pinów, timeout'y i synchronizację Jeśli chcesz, mogę też pomóc Ci automatycznie przesunąć tę funkcję we właściwe miejsce i dać gotowy fragment kodu.
Czy chcesz taką naprawioną wersję?
- Funkcja
Other models, same or similar prompt
Responses were produced in different, uncontrolled contexts (system prompts, settings, prior turns). This is not a benchmark.