poprawki
This commit is contained in:
@@ -0,0 +1,192 @@
|
||||
#include <AccelStepper.h>
|
||||
|
||||
// ── Pole robocze i Ruch (Zgodnie z plikiem config.h) ────────────────────
|
||||
constexpr float STEPS_PER_MM_X = 80.0f; //
|
||||
constexpr float STEPS_PER_MM_Y = 80.0f; //
|
||||
constexpr float TEST_SPEED_MM_S = 150.0f; // Wolna prędkość testowa z config.h
|
||||
constexpr float ACCEL_MM_S2 = 200.0f; //100.0f; // Łagodne przyspieszenie
|
||||
|
||||
// ── Piny GPIO (Zaktualizowane na podstawie pliku config.h) ─────────────────────
|
||||
constexpr uint8_t PIN_X_STEP = 26; //
|
||||
constexpr uint8_t PIN_X_DIR = 27; //
|
||||
constexpr uint8_t PIN_Y_STEP = 14; //
|
||||
constexpr uint8_t PIN_Y_DIR = 15; //
|
||||
constexpr uint8_t PIN_STEPPER_EN = 13; //
|
||||
|
||||
// Wyłączniki krańcowe (Zaktualizowane piny z config.h)
|
||||
constexpr uint8_t PIN_LIMIT_X_MIN = 36; // LEWA STRONA (DOM X)
|
||||
constexpr uint8_t PIN_LIMIT_X_MAX = 34; // PRAWA STRONA (KONIEC X)
|
||||
constexpr uint8_t PIN_LIMIT_Y_MIN = 39; //
|
||||
constexpr uint8_t PIN_LIMIT_Y_MAX = 35; //
|
||||
|
||||
// Serwo i Przyciski (Zgodnie z config.h)
|
||||
constexpr uint8_t PIN_SERVO = 25; //
|
||||
constexpr int SERVO_ANGLE_UP = 90; //
|
||||
constexpr int SERVO_ANGLE_DOWN = 50; //
|
||||
constexpr uint8_t PIN_BTN_UP = 32; //
|
||||
constexpr uint8_t PIN_BTN_DOWN = 33; //
|
||||
constexpr uint8_t PIN_BTN_OK = 4; //
|
||||
|
||||
// ── Konfiguracja Timera Oszczędzania Energii ─────────────────────────────────
|
||||
constexpr uint32_t STEPPER_TIMEOUT_MS = 2000; // 2 sekundy bezczynności przed odłączeniem prądu
|
||||
|
||||
// Inicjalizacja obiektów dla silników CoreXY / H-Bot
|
||||
AccelStepper motorA(AccelStepper::DRIVER, PIN_X_STEP, PIN_X_DIR);
|
||||
AccelStepper motorB(AccelStepper::DRIVER, PIN_Y_STEP, PIN_Y_DIR);
|
||||
|
||||
// Zmienne pomocnicze
|
||||
bool isPenDown = false;
|
||||
bool lastBtnOkState = HIGH;
|
||||
uint32_t lastActivityTime = 0;
|
||||
bool steppersEnabled = false;
|
||||
|
||||
// Funkcja włączająca prąd w silnikach
|
||||
void enableSteppers() {
|
||||
if (!steppersEnabled) {
|
||||
digitalWrite(PIN_STEPPER_EN, LOW); // LOW = Włączone
|
||||
steppersEnabled = true;
|
||||
Serial.println("[ZASILANIE] Silniki włączone");
|
||||
}
|
||||
lastActivityTime = millis(); // Resetowanie licznika czasu aktywności
|
||||
}
|
||||
|
||||
// Funkcja odcinająca prąd w silnikach (ochrona przed grzaniem)
|
||||
void disableSteppers() {
|
||||
if (steppersEnabled) {
|
||||
motorA.stop();
|
||||
motorB.stop();
|
||||
digitalWrite(PIN_STEPPER_EN, HIGH); // HIGH = Odłączone
|
||||
steppersEnabled = false;
|
||||
Serial.println("[ZASILANIE] Bezczynność 4s -> Odcięcie prądu (Silniki chłodne)");
|
||||
}
|
||||
}
|
||||
|
||||
// Sterowanie serwem (natywne PWM dla ESP32)
|
||||
void setServoAngle(uint8_t pin, int angle) {
|
||||
int dutyCycle = map(angle, 0, 180, 13, 26);
|
||||
analogWrite(pin, dutyCycle);
|
||||
}
|
||||
|
||||
// Sprawdzenie stanu krańcówki (LOW = wciśnięta)
|
||||
bool isLimitTriggered(uint8_t pin) {
|
||||
return digitalRead(pin) == LOW;
|
||||
}
|
||||
|
||||
void setup() {
|
||||
Serial.begin(115200);
|
||||
//while (!Serial) delay(10);
|
||||
delay(500);
|
||||
|
||||
// Konfiguracja wejść
|
||||
pinMode(PIN_LIMIT_X_MIN, INPUT);
|
||||
pinMode(PIN_LIMIT_X_MAX, INPUT);
|
||||
pinMode(PIN_LIMIT_Y_MIN, INPUT);
|
||||
pinMode(PIN_LIMIT_Y_MAX, INPUT);
|
||||
|
||||
pinMode(PIN_BTN_UP, INPUT_PULLUP);
|
||||
pinMode(PIN_BTN_DOWN, INPUT_PULLUP);
|
||||
pinMode(PIN_BTN_OK, INPUT_PULLUP);
|
||||
|
||||
// Konfiguracja pinu Enable silników
|
||||
pinMode(PIN_STEPPER_EN, OUTPUT);
|
||||
steppersEnabled = false;
|
||||
enableSteppers();
|
||||
|
||||
// Konfiguracja PWM dla Serwa (50Hz)
|
||||
analogWriteFrequency(PIN_SERVO, 50);
|
||||
setServoAngle(PIN_SERVO, SERVO_ANGLE_UP); // Pisak w górze na starcie
|
||||
|
||||
// Maksymalne prędkości silników
|
||||
motorA.setMaxSpeed(TEST_SPEED_MM_S * STEPS_PER_MM_X);
|
||||
motorB.setMaxSpeed(TEST_SPEED_MM_S * STEPS_PER_MM_Y);
|
||||
}
|
||||
|
||||
void loop() {
|
||||
uint32_t currentTime = millis();
|
||||
|
||||
// ─── 1. OBSŁUGA SERWA (PRZYCISK OK) ──────────────────────────────────────
|
||||
// bool currentBtnOkState = digitalRead(PIN_BTN_OK);
|
||||
|
||||
// if (currentBtnOkState == LOW && lastBtnOkState == HIGH) {
|
||||
// delay(50); // Debounce
|
||||
// if (digitalRead(PIN_BTN_OK) == LOW) {
|
||||
// enableSteppers(); // Aktywność użytkownika wybudza silniki
|
||||
// isPenDown = !isPenDown;
|
||||
|
||||
// if (isPenDown) {
|
||||
// setServoAngle(PIN_SERVO, SERVO_ANGLE_DOWN); //
|
||||
// Serial.println("[SERWO] Pisak opuszczony (DOWN)");
|
||||
// } else {
|
||||
// setServoAngle(PIN_SERVO, SERVO_ANGLE_UP); //
|
||||
// Serial.println("[SERWO] Pisak podniesiony (UP)");
|
||||
// }
|
||||
// }
|
||||
// }
|
||||
// lastBtnOkState = currentBtnOkState;
|
||||
|
||||
// ─── 2. OBSŁUGA RUCHU OSI X (PRZYCISKI UP / DOWN) ────────────────────────
|
||||
bool upPressed = (digitalRead(PIN_BTN_UP) == LOW);
|
||||
bool downPressed = (digitalRead(PIN_BTN_DOWN) == LOW);
|
||||
|
||||
// if (isLimitTriggered(PIN_LIMIT_X_MAX)){
|
||||
// Serial.println("Krańcówka X_MAX (PRAWA) aktywna!");
|
||||
// }
|
||||
|
||||
// if (isLimitTriggered(PIN_LIMIT_X_MIN)){
|
||||
// Serial.println("Krańcówka X_MIN (LEWA) aktywna!");
|
||||
// }
|
||||
|
||||
// if (isLimitTriggered(PIN_LIMIT_Y_MIN)){
|
||||
// Serial.println("Krańcówka Y_MIN (GORNA) aktywna!");
|
||||
// }
|
||||
|
||||
// if (isLimitTriggered(PIN_LIMIT_Y_MAX)){
|
||||
// Serial.println("Krańcówka X_MAX (DOLNA) aktywna!");
|
||||
// }
|
||||
|
||||
|
||||
if (upPressed && !downPressed) { // Wciśnięty UP
|
||||
enableSteppers();
|
||||
|
||||
// --- RUCH W PRAWO ---
|
||||
// Sprawdzamy, czy nie została osiągnięta prawa krańcówka (X_MAX)
|
||||
if (isLimitTriggered(PIN_LIMIT_X_MAX)) {
|
||||
Serial.println("[STOP] Krańcówka X_MAX (PRAWA) aktywna! Ruch zablokowany.");
|
||||
motorA.stop();
|
||||
motorB.stop();
|
||||
} else {
|
||||
// Oba silniki kręcą się w tę samą stronę (np. wartości dodatnie)
|
||||
motorA.setSpeed(TEST_SPEED_MM_S * STEPS_PER_MM_X);
|
||||
motorB.setSpeed(TEST_SPEED_MM_S * STEPS_PER_MM_Y);
|
||||
motorA.runSpeed();
|
||||
motorB.runSpeed();
|
||||
}
|
||||
}
|
||||
else if (downPressed && !upPressed) { // Przycisk DOWN
|
||||
enableSteppers();
|
||||
|
||||
// --- RUCH W LEWO ---
|
||||
// Sprawdzamy, czy nie została osiągnięta lewa krańcówka (X_MIN)
|
||||
if (isLimitTriggered(PIN_LIMIT_X_MIN)) {
|
||||
Serial.println("[STOP] Krańcówka X_MIN (LEWA) aktywna! Ruch zablokowany.");
|
||||
motorA.stop();
|
||||
motorB.stop();
|
||||
} else {
|
||||
// Oba silniki kręcą się w tę samą stronę, ale w przeciwnym kierunku (ujemne prędkości)
|
||||
motorA.setSpeed(-(TEST_SPEED_MM_S * STEPS_PER_MM_X));
|
||||
motorB.setSpeed(-(TEST_SPEED_MM_S * STEPS_PER_MM_Y));
|
||||
motorA.runSpeed();
|
||||
motorB.runSpeed();
|
||||
}
|
||||
}
|
||||
// else {
|
||||
// // Gdy żaden przycisk nie jest wciśnięty -> wyhamuj silniki
|
||||
// motorA.stop();
|
||||
// motorB.stop();
|
||||
// }
|
||||
|
||||
// ─── 3. AUTOMATYCZNY TIMER USYPIANIA SILNIKÓW ────────────────────────────
|
||||
if (steppersEnabled && (currentTime - lastActivityTime >= STEPPER_TIMEOUT_MS)) {
|
||||
disableSteppers();
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user