#include "Arduino.h" // Pin definitions const int stepPin = 2; // YELLOW (Set back to 2 for the Mega) const int dirPin = 3; // GREEN const int homeSwitchMin = 18; // (was 4) const int homeSwitchMax = 19; // (was 5) const int stepPin2 = 14; // BLUE const int dirPin2 = 15; // VIOLET const int homeSwitchMin2 = 4; // (was 18) const int homeSwitchMax2 = 5; // (was 19) const int startStop = 21; const int buttonLight = 29; // System state flags volatile bool startButtonReady = false; // Start in waiting state volatile bool motorRunning = false; const int TIME = 100; // Delay for homing cycle; was 1000 const int SPEED = 200; // Motor speed (stalls at 125) int travelDistance = 0; bool homeSwitchesPresent = true; volatile unsigned long lastInterruptTime = 0; // For debounce in ISR // ---------------------------------------------------------------- // INTERRUPT: Start/Stop Button ISR // When the button is pressed while motors are running, it stops them. // ---------------------------------------------------------------- void startStopISR() { static unsigned long lastIntTime = 0; unsigned long currentTime = millis(); // 200ms debounce in ISR if (currentTime - lastIntTime < 200) return; lastIntTime = currentTime; // Only act if motors are running if (motorRunning) { motorRunning = false; // Stop motors immediately Serial.println("🚨 INTERRUPT: Motor stopped!"); } // Do nothing if motors are not running } // ---------------------------------------------------------------- // SETUP // ---------------------------------------------------------------- void setup() { Serial.begin(115200); // Set pin modes for Motor 1 pinMode(stepPin, OUTPUT); pinMode(dirPin, OUTPUT); pinMode(homeSwitchMin, INPUT_PULLUP); pinMode(homeSwitchMax, INPUT_PULLUP); // Set pin modes for Motor 2 pinMode(stepPin2, OUTPUT); pinMode(dirPin2, OUTPUT); pinMode(homeSwitchMin2, INPUT_PULLUP); pinMode(homeSwitchMax2, INPUT_PULLUP); // Set pin modes for start/stop and button light pinMode(startStop, INPUT_PULLUP); pinMode(buttonLight, OUTPUT); // Attach interrupt to startStop (FALLING edge) attachInterrupt(digitalPinToInterrupt(startStop), startStopISR, FALLING); // Start in waiting state startButtonReady = false; motorRunning = false; Serial.println("System Ready - Waiting for Start Button!"); } void slowDistanceDec(int speed, int motor, int shortDistance) { for (int a = 0; a < 5; a++) { backyForthyDirection(1200, speed, 1, motor, 10, 1); if (!motorRunning) break; backyForthyDirection(1200, speed, 1, motor, 10, 0); if (!motorRunning) break; } for (int a = 0; a < 5; a++) { backyForthyDirection(800, speed, 1, motor, 10, 1); if (!motorRunning) break; backyForthyDirection(800, speed, 1, motor, 10, 0); if (!motorRunning) break; } for (int a = 0; a < 5; a++) { backyForthyDirection(300, speed, 1, motor, 10, 1); if (!motorRunning) break; backyForthyDirection(300, speed, 1, motor, 10, 0); if (!motorRunning) break; } for (int a = 0; a < shortDistance; a++) { backyForthyDirection(100, speed, 1, motor, 10, 1); if (!motorRunning) break; backyForthyDirection(100, speed, 1, motor, 10, 0); if (!motorRunning) break; } } void slowDistanceInc(int speed, int motor, int shortDistance) { for (int a = 0; a < 5; a++) { backyForthyDirection(100, speed, 1, motor, 10, 1); if (!motorRunning) break; backyForthyDirection(100, speed, 1, motor, 10, 0); if (!motorRunning) break; } for (int a = 0; a < 5; a++) { backyForthyDirection(300, speed, 1, motor, 10, 1); if (!motorRunning) break; backyForthyDirection(300, speed, 1, motor, 10, 0); if (!motorRunning) break; } for (int a = 0; a < 5; a++) { backyForthyDirection(800, speed, 1, motor, 10, 1); if (!motorRunning) break; backyForthyDirection(800, speed, 1, motor, 10, 0); if (!motorRunning) break; } for (int a = 0; a < shortDistance; a++) { backyForthyDirection(1200, speed, 1, motor, 10, 1); if (!motorRunning) break; backyForthyDirection(1200, speed, 1, motor, 10, 0); if (!motorRunning) break; } } // ---------------------------------------------------------------- // MAIN LOOP // ---------------------------------------------------------------- void loop() { // READY STATE: Only start when a new deliberate button press occurs. if (!motorRunning) { digitalWrite(buttonLight, HIGH); Serial.println("Waiting for Start Button..."); // Ensure button is released (avoid using a stale press) while (digitalRead(startStop) == LOW) { delay(10); } // Now wait for a new press while (digitalRead(startStop) == HIGH) { delay(10); } // Wait for the button to be released after the press while (digitalRead(startStop) == LOW) { delay(10); } Serial.println("✅ Button Pressed! System Starting..."); startButtonReady = true; // Mark ready motorRunning = true; // Start motors digitalWrite(buttonLight, LOW); } // RUNNING STATE: Execute homing and movement routines if motors are running. if (motorRunning) { Serial.println("Starting homing..."); travelDistance = homeMotor(); if (!motorRunning) { // If stopped during homing, exit. Serial.println("🚨 Stopped during homing. Waiting for new start."); startButtonReady = false; return; } Serial.println("Homing complete!"); middle(travelDistance); travelDistance = homeMotor2(); if (!motorRunning) { // Check again after second homing Serial.println("🚨 Stopped during second homing. Waiting for new start."); startButtonReady = false; return; } Serial.println("Homing complete!"); middle2(travelDistance); // (int steps, int speed, bool roundTrip, int motor, int wait, bool dir) Serial.println("MOVING..."); slowDistanceDec(200, 2, 10); // fast shake front back Serial.println("a"); for (int a = 0; a < 15; a++) { backyForthyDirection(50, 100, 1, 1, 10, 1); if (!motorRunning) break; backyForthyDirection(50, 100, 1, 1, 10, 0); if (!motorRunning) break; } Serial.println("b"); slowDistanceInc(175, 2, 15); Serial.println("c"); slowDistanceDec(150, 2, 25); // slowDistanceInc(125, 2, 35); for (int a = 0; a < 100; a++) { backyForthyDirection(100, 100, 1, 2, 10, 1); if (!motorRunning) break; backyForthyDirection(100, 100, 1, 2, 10, 0); if (!motorRunning) break; } Serial.println("d"); for (int a = 0; a < 10; a++) { backyForthyDirection(100, 100, 1, 1, 10, 1); if (!motorRunning) break; backyForthyDirection(100, 100, 1, 1, 10, 0); if (!motorRunning) break; } Serial.println("e"); for (int a = 0; a < 5; a++) { backyForthyDirection(200, 100, 1, 1, 10, 1); if (!motorRunning) break; backyForthyDirection(200, 100, 1, 1, 10, 0); if (!motorRunning) break; } Serial.println("f"); for (int a = 0; a < 100; a++) { backyForthyDirection(50, 100, 1, 2, 10, 1); if (!motorRunning) break; backyForthyDirection(50, 100, 1, 2, 10, 0); if (!motorRunning) break; } // bonus medium Serial.println("medium insert...."); Serial.println("g"); for (int a = 0; a < 20; a++) { backyForthyDirection(250, 125, 1, 2, 10, 1); if (!motorRunning) break; backyForthyDirection(250, 125, 1, 2, 10, 0); if (!motorRunning) break; } // finsih him... Serial.println("finish him"); Serial.println("h"); for (int a = 0; a < 100; a++) { backyForthyDirection(100, 100, 1, 2, 10, 1); if (!motorRunning) break; backyForthyDirection(100, 100, 1, 2, 10, 0); if (!motorRunning) break; } Serial.println("i"); for (int a = 0; a < 100; a++) { backyForthyDirection(50, 100, 1, 2, 10, 1); if (!motorRunning) break; backyForthyDirection(50, 100, 1, 2, 10, 0); if (!motorRunning) break; } Serial.println("j"); Serial.println("medium insert.... again"); for (int a = 0; a < 20; a++) { backyForthyDirection(250, 125, 1, 2, 10, 1); if (!motorRunning) break; backyForthyDirection(250, 125, 1, 2, 10, 0); if (!motorRunning) break; } Serial.println("k"); for (int a = 0; a < 100; a++) { backyForthyDirection(100, 100, 1, 2, 10, 1); if (!motorRunning) break; backyForthyDirection(100, 100, 1, 2, 10, 0); if (!motorRunning) break; } Serial.println("l"); for (int a = 0; a < 100; a++) { backyForthyDirection(50, 100, 1, 2, 10, 1); if (!motorRunning) break; backyForthyDirection(50, 100, 1, 2, 10, 0); if (!motorRunning) break; } Serial.println("m"); for (int a = 0; a < 100; a++) { backyForthyDirection(100, 100, 1, 2, 10, 1); if (!motorRunning) break; backyForthyDirection(100, 100, 1, 2, 10, 0); if (!motorRunning) break; } Serial.println("n"); Serial.println("medium insert.... again2"); for (int a = 0; a < 20; a++) { backyForthyDirection(250, 125, 1, 2, 10, 1); if (!motorRunning) break; backyForthyDirection(250, 125, 1, 2, 10, 0); if (!motorRunning) break; } Serial.println("oooh"); for (int a = 0; a < 100; a++) { backyForthyDirection(100, 100, 1, 2, 10, 1); if (!motorRunning) break; backyForthyDirection(100, 100, 1, 2, 10, 0); if (!motorRunning) break; } Serial.println("Done MOVING..."); // If stopped during movement, do not continue. if (!motorRunning) { Serial.println("🚨 Motor was stopped during movement. Waiting for new start."); startButtonReady = false; return; } // Movement complete; stop motors and return to waiting state. Serial.println("✅ MOVEMENT COMPLETE - Press button to restart."); motorRunning = false; startButtonReady = false; // Force system to wait for a new button press. } } // ---------------------------------------------------------------- // HOMING & MOTOR STEP FUNCTIONS (unchanged) // ---------------------------------------------------------------- int homeMotor() { int counter = 0; int homingSpeed = 300; Serial.println("Starting homing..."); digitalWrite(dirPin, LOW); while (digitalRead(homeSwitchMin) == HIGH && motorRunning) { motorStep(1, homingSpeed, stepPin); counter++; } Serial.println("Reached Min Switch."); delay(TIME); digitalWrite(dirPin, HIGH); counter = 0; while (digitalRead(homeSwitchMax) == HIGH && motorRunning) { motorStep(1, homingSpeed, stepPin); counter++; } Serial.println("Reached Max Switch."); delay(TIME); digitalWrite(dirPin, LOW); while (digitalRead(homeSwitchMin) == HIGH && motorRunning) { motorStep(1, homingSpeed, stepPin); } Serial.println("Final Homing to Min Complete."); return counter; } int homeMotor2() { int counter = 0; int homingSpeed = 300; Serial.println("Starting homing..."); digitalWrite(dirPin2, LOW); while (digitalRead(homeSwitchMin2) == HIGH && motorRunning) { motorStep(1, homingSpeed, stepPin2); counter++; } Serial.println("Reached Min Switch."); delay(TIME); digitalWrite(dirPin2, HIGH); counter = 0; while (digitalRead(homeSwitchMax2) == HIGH && motorRunning) { motorStep(1, homingSpeed, stepPin2); counter++; } Serial.println("Reached Max Switch."); delay(TIME); digitalWrite(dirPin2, LOW); while (digitalRead(homeSwitchMin2) == HIGH && motorRunning) { motorStep(1, homingSpeed, stepPin2); } Serial.println("Final Homing to Min Complete."); return counter; } void motorStep(int steps, int speed, int stepPin) { for (int x = 0; x < steps && motorRunning; x++) { digitalWrite(stepPin, HIGH); delayMicroseconds(speed); digitalWrite(stepPin, LOW); delayMicroseconds(speed); } } void middle(int travelDistance) { digitalWrite(dirPin, HIGH); delay(500); motorStep(travelDistance / 2, SPEED, stepPin); } void middle2(int travelDistance) { digitalWrite(dirPin2, HIGH); delay(500); motorStep(travelDistance / 2, SPEED, stepPin2); } void backyForthyDirection(int steps, int speed, bool roundTrip, int motor, int wait, bool dir) { if (motor == 1) { if (roundTrip) { digitalWrite(dirPin, dir); motorStep(steps, speed, stepPin); if (!motorRunning) return; delay(wait); } digitalWrite(dirPin, !dir); motorStep(steps, speed, stepPin); if (!motorRunning) return; delay(wait); } else if (motor == 2) { if (roundTrip) { digitalWrite(dirPin2, dir); motorStep(steps, speed, stepPin2); if (!motorRunning) return; delay(wait); } digitalWrite(dirPin2, !dir); motorStep(steps, speed, stepPin2); if (!motorRunning) return; delay(wait); } }