/********* Pleasedontcode.com **********

	Pleasedontcode thanks you for automatic code generation! Enjoy your code!

	- Terms and Conditions:
	You have a non-exclusive, revocable, worldwide, royalty-free license
	for personal and commercial use. Attribution is optional; modifications
	are allowed, but you're responsible for code maintenance. We're not
	liable for any loss or damage. For full terms,
	please visit pleasedontcode.com/termsandconditions.

	- Project: Dual-Mode Control
	- Source Code NOT compiled for: Arduino Mega
	- Source Code created on: 2025-09-27 14:44:33

********* Pleasedontcode.com **********/

/****** SYSTEM REQUIREMENTS *****/
/****** SYSTEM REQUIREMENT 1 *****/
	/* help refine the code. want to run the code */
	/* smoothly both in auto mode and manual mode. */
/****** END SYSTEM REQUIREMENTS *****/


/* START CODE */

/****** DEFINITION OF LIBRARIES *****/

/****** FUNCTION PROTOTYPES *****/
void setup(void);
void loop(void);

 // Merged content from USER CODE, adapted to fit the PRELIMINARY CODE structure for Arduino Mega

//Last Updated 12092025
// Define stepper motor1 of Linear guide @ Spinning Motor driver pins
const int DRIVER1_DIR = 28; // Direction pin for stepper motor1
const int DRIVER1_STEP = 18; // Step pin for stepper motor1
const int DRIVER1_ENA = 29; // Enable pin for stepper motor1

// Define stepper motor2 of Linear guide @ Feeder Motor driver pins
const int DRIVER2_DIR = 30; // Direction pin for stepper motor2
const int DRIVER2_STEP = 6; // Step pin for stepper motor2
const int DRIVER2_ENA = 31; // Enable pin for stepper motor2

// Proximity Sensor for Induction Motor CW and CCW
const int PROXY_POS = 10; // Proxy POS Sensor for Linear Motor Position
const int PROXY_A = 11; // Proxy A Sensor for CW
const int PROXY_B = 12; // Proxy B Sensor for CCW
// const int PROXY_RUNFEED = 13; // Proxy Feed Run Sensor

// Define BTS7960B motor driver pins for Feeder motor
const int EN_FEEDER = 20; // Enable pin for feeder motor
const int LPWM_FEEDER = 3; // Left PWM pin for feeder motor
const int RPWM_FEEDER = 4; // Right PWM pin for feeder motor

// Define Limit switchs of Linear drive @ Spinning motor
const int LIMIT_SWITCH1 = 15; // 32; // Limit switch 1 pin (Bottom)
const int LIMIT_SWITCH2 = 33; // Limit switch 2 pin (Up)
// Define Limit switchs of Linear drive @ Feeder motor
const int LIMIT_SWITCH3 = 35; // Limit switch 3 pin (Reverse)
const int LIMIT_SWITCH4 = 16; // Limit switch 4 pin (Forward)

// Define relay pins
const int RELAY1 = A0; // Relay for 
const int RELAY2 = A1; // Relay for Air-Nipper Cutter
const int RELAY3 = A12; // Relay for Gripper 1
const int RELAY4 = A3; // Relay for Gripper 2
const int RELAY5 = A4; // Relay for CW direction
const int RELAY6 = A5; // Relay for CCW direction
const int RELAY7 = A6; // Relay for Feeder Linear Drive
const int RELAY8 = A7; // Relay for Label Vaccum pad Forward/Backward
const int RELAY9 = 43; // Relay for Spinning Linear Drive
const int RELAY10 = 45; // Relay for Cutter-up & Cutter-down Cylinder
const int RELAY11 = 46; // Relay for
const int RELAY12 = 47; // Relay for
const int RELAY13 = 48; // Relay for
const int RELAY14 = 49; // Relay for
const int RELAY15 = 50; // Relay for
const int RELAY16 = 51; // Relay for

// Flags and variables
bool autoMode = false; // Flag for Auto Mode
bool manualMode = false; // Flag for Manual Mode
int numCycles = 0; // Variable to store number of cycles
bool feederRunning = false; // Flag to indicate if the feeder is running
bool isForward = false;
int targetCounts = 0; // Stores the required counts for current operation
volatile int countA = 0; // Count for Proxy A
volatile int countB = 0; // Count for Proxy B
bool motorRunning = false; // Flag to indicate if the motor is currently running
bool countingEnabled = false; // Flag to enable counting based on relay state
bool isClockwise = false; // Global variable to track motor direction
bool countProxyAEnabled = true; // Flag to enable/disable counting for Proxy A
bool countProxyBEnabled = true; // Flag to enable/disable counting for Proxy B
bool countProxyPOSEnabled = true; // Flag to enable/disable counting for Proxy POS
volatile bool isMoving1 = false; // Flag to indicate if the linear guide is currently moving
volatile bool isMoving2 = false; // Flag to indicate if the linear guide is currently moving
unsigned long feederStartTime = 0;  // When feeder started
unsigned long feederDuration = 0;   // How long to run (ms)
bool linearGuide1Complete = false;
bool linearGuide2Complete = false;
bool feederComplete = false;
bool motorComplete = false;

// Command enumeration
enum Command { START, STOP, G1, G2, G3, G4, VC, VO, VF, VR, PU, PD, OG, CW, CCW, LU, LD, LF, LR, CU, CD, CX, X, FD, RD, AUTO, MANUAL, NONE, UNKNOWN };

// Debounce variables
const unsigned long DEBOUNCE_TIME = 1000; // 1 second debounce time
unsigned long lastCommandTime = 0; // Variable to store the last command execution time

// Auto mode state machine variables
enum AutoState { AUTO_IDLE, AUTO_CHECK_POS, AUTO_LU, AUTO_LF, AUTO_VF, AUTO_FD, AUTO_G3, AUTO_LR, AUTO_G1, AUTO_CU, AUTO_CX, AUTO_CD, AUTO_LD, AUTO_VR, AUTO_CW, AUTO_CCW, AUTO_OG, AUTO_COMPLETE, AUTO_CW_RUNNING, AUTO_CCW_RUNNING, AUTO_STOPPED };
AutoState autoState = AUTO_IDLE;
int currentCycle = 0;
unsigned long autoStepStartTime = 0;
const unsigned long STEP_DELAY = 100; // 100ms delay between steps

// Completion check functions
bool isLinearGuide1Complete() { return linearGuide1Complete; }
bool isLinearGuide2Complete() { return linearGuide2Complete; }
bool isFeederComplete() { return feederComplete; }
bool isMotorComplete() { return motorComplete; }

// Function declarations
void stopMotor(); // To Stop Linear Motor, Feeder Motor, Spinning Motor
void moveLinearGuide1(String direction); // To move Linear UP and Down
void moveLinearGuide2(String direction); // To move Linear Right and Left
void moveStepper1(bool direction, int stopPin); // To Execute Linear & Spinning in sequence
void moveStepper2(bool direction, int stopPin); // To Execute Linear & Feeder in sequence 
void runFeeder(bool forward, unsigned long durationMillis = 0, int speed = 25); // To activte Feeder motor
void setFeederMotorSpeed(int speed, bool forward);
void runOpenGripper(); // To deactivate Gripper Relay 3 & 4
void runMotor(bool clockwise); // Combined CW and CCW motor function
void handleCommand(Command cmd); // To handle and execute Manual Input Command
bool runAutoMode(int numCycles); // To execute Auto Input Command sequence
void resetSerialBuffer(); // To reset Command line
bool checkStopCommand(); // To check for STOP command during process
void resetMode();
void CutterSensor();
void proxyA();
void proxyB();

// Subsystem wrappers to isolate auto/manual control (new)
void Subsystem_AutoStart(int cycles) { // Initialize and start Auto mode for a fixed cycle count
  numCycles = cycles;
  autoMode = true;
  autoState = AUTO_IDLE;
  currentCycle = 0;
  Serial.print("Subsystem Auto Start: "); Serial.println(cycles);
}
void Subsystem_AutoTick() { updateAutoMode(); }
void Subsystem_AutoStop() { stopAutoMode(); }
void Subsystem_ManualActivate() { manualMode = true; resetMode(); Serial.println("Subsystem Manual Mode Activated"); }
void Subsystem_ManualDeactivate() { manualMode = false; Serial.println("Subsystem Manual Mode Deactivated"); }
bool Subsystem_AutoRunning() { return autoMode; }

Command parseCommand(String command);
Command currentMode = NONE;
// Helper functions to set completion flags
void setLinearGuide1Complete() {
 linearGuide1Complete = true;
 Serial.println("Linear Guide 1 operation complete");
}
void setLinearGuide2Complete() {
 linearGuide2Complete = true;
 Serial.println("Linear Guide 2 operation complete");
}
void setFeederComplete() {
 feederComplete = true;
 Serial.println("Feeder operation complete");
}
void setMotorComplete() {
 motorComplete = true;
 Serial.println("Motor operation complete");
}
void setup() {
 // Set pin modes for motors and relays
 pinMode(RPWM_FEEDER, OUTPUT);
 pinMode(LPWM_FEEDER, OUTPUT);
 pinMode(EN_FEEDER, OUTPUT);
 pinMode(PROXY_POS, INPUT_PULLUP); // Set proxy POS as input
 pinMode(PROXY_A, INPUT_PULLUP); // Set proxy A as input
 pinMode(PROXY_B, INPUT_PULLUP); // Set proxy B as input
 // pinMode(PROXY_RUNFEED, INPUT); // Set proxy Feed Run as input
 pinMode(RELAY1, OUTPUT);
 pinMode(RELAY2, OUTPUT);
 pinMode(RELAY3, OUTPUT);
 pinMode(RELAY4, OUTPUT);
 pinMode(RELAY5, OUTPUT);
 pinMode(RELAY6, OUTPUT);
 pinMode(RELAY7, OUTPUT);
 pinMode(RELAY8, OUTPUT);
 pinMode(RELAY9, OUTPUT);
 pinMode(RELAY10, OUTPUT);
 pinMode(RELAY11, OUTPUT);
 pinMode(RELAY12, OUTPUT);
 pinMode(RELAY13, OUTPUT);
 pinMode(RELAY14, OUTPUT);
 pinMode(RELAY15, OUTPUT);
 pinMode(RELAY16, OUTPUT);
 pinMode(LIMIT_SWITCH1, INPUT_PULLUP);
 pinMode(LIMIT_SWITCH2, INPUT_PULLUP);
 pinMode(LIMIT_SWITCH3, INPUT_PULLUP);
 pinMode(LIMIT_SWITCH4, INPUT_PULLUP);
 pinMode(DRIVER1_STEP, OUTPUT);
 pinMode(DRIVER1_DIR, OUTPUT);
 pinMode(DRIVER1_ENA, OUTPUT);
 pinMode(DRIVER2_STEP, OUTPUT);
 pinMode(DRIVER2_DIR, OUTPUT);
 pinMode(DRIVER2_ENA, OUTPUT);

 // Initialize relays to LOW (off state)
 digitalWrite(RELAY1, LOW);
 digitalWrite(RELAY2, LOW);
 digitalWrite(RELAY3, LOW);
 digitalWrite(RELAY4, LOW);
 digitalWrite(RELAY5, LOW);
 digitalWrite(RELAY6, LOW);
 digitalWrite(RELAY7, LOW);
 digitalWrite(RELAY8, LOW);
 digitalWrite(RELAY9, LOW);
 digitalWrite(RELAY10, LOW);
 digitalWrite(RELAY11, LOW);
 digitalWrite(RELAY12, LOW);
 digitalWrite(RELAY13, LOW);
 digitalWrite(RELAY14, LOW);
 digitalWrite(RELAY15, LOW);
 digitalWrite(RELAY16, LOW);
 
 digitalWrite(DRIVER1_ENA, LOW);  // Enable stepper motor1
 digitalWrite(DRIVER2_ENA, LOW);  // Enable stepper motor2
 
 Serial.begin(9600);  // Start serial communication
 resetSerialBuffer();  // Clear serial buffer on startup
}
Command parseCommand(String command) {
 command.trim();
 // Parse incoming command strings and return corresponding Command enum
 if (command == "START") return START;
 if (command == "STOP") return STOP;
 if (command == "G1") return G1;
 if (command == "G2") return G2;
 if (command == "G3") return G3;
 if (command == "G4") return G4;
 if (command == "VC") return VC;
 if (command == "VO") return VO;
 if (command == "VF") return VF;
 if (command == "VR") return VR;
 if (command == "PU") return PU;
 if (command == "PD") return PD;
 if (command == "OG") return OG;
 if (command == "CW") return CW;
 if (command == "CCW") return CCW;
 if (command == "LU") return LU;
 if (command == "LD") return LD;
 if (command == "LF") return LF;
 if (command == "LR") return LR;
 if (command == "CU") return CU;
 if (command == "CD") return CD;
 if (command == "CX") return CX; 
 if (command == "X") return X;
 if (command == "FD") return FD; // Forward command
 if (command == "RD") return RD; // Reverse command
 if (command == "AUTO") return AUTO; // Auto mode
 if (command == "MANUAL") return MANUAL; // Manual mode
 return UNKNOWN; // Return UNKNOWN if command is not recognized
}
void handleCommand(Command cmd) {
 // Handle commands based on the parsed Command enum
 switch (cmd) {
   case START:
     Serial.println("Process started.");
   break;
   case STOP:
     stopMotor();
     Serial.println("MOTORS STOPPED");
   break;
   case G1:
     digitalWrite(RELAY3, HIGH); // Activate RELAY3 for G1 Close
     Serial.println("RELAY3 ON");
   break;
   case G2:
     digitalWrite(RELAY3, LOW); // Deactivate RELAY3 for G2 Open
     Serial.println("RELAY3 OFF");
   break;
   case G3:
     digitalWrite(RELAY4, HIGH); // Activate RELAY4 for G2 Close
     Serial.println("RELAY4 ON");
   break;
   case G4:
     digitalWrite(RELAY4, LOW); // Deactivate RELAY4 for G2 Open
     Serial.println("RELAY4 OFF");
   break;
   case VC: // Removed as on 28082025
     digitalWrite(RELAY11, HIGH); // Activate RELAY10 for Vaccum pad Pick
     Serial.println("RELAY11 ON");
   break;
   case VO: // Removed as on 28082025
     digitalWrite(RELAY11, LOW); // Deactivate RELAY10 for Vaccum pad Eject
     Serial.println("RELAY11 OFF");
   break;
   case VF:
     digitalWrite(RELAY8, HIGH); // Activate RELAY8 for Vaccum pad Forward
     Serial.println("RELAY8 ON");
   break;
   case VR:
     digitalWrite(RELAY8, LOW); // Deactivate RELAY8 for Vaccum pad Reverse
     Serial.println("RELAY8 OFF");
   break;
   case PU:
     digitalWrite(RELAY11, HIGH); // Activate RELAY11 for Push Up Load
     Serial.println("RELAY11 ON");
   break;
   case PD:
     digitalWrite(RELAY11, LOW); // Deactivate RELAY11 for Push Down Load
     Serial.println("RELAY11 OFF");
   break;
   case OG:
     runOpenGripper();
     Serial.println("Gripper 1 & 2 OPEN...");
   break;
   case CW:
     runMotor(true);
     Serial.println("Starting CW motor...");
   break;
   case CCW:
     runMotor(false);
     Serial.println("Starting CCW motor...");
   break;
   case LU:
     moveLinearGuide1("LU");
     Serial.println("Linear Guide1 Moving Up");
   break;
   case LD:
     moveLinearGuide1("LD");
     Serial.println("Linear Guide1 Moving Down");
   break;
   case LF:
     moveLinearGuide2("LF");
     Serial.println("Linear Guide2 Moving Forward");
   break;
   case LR:
     moveLinearGuide2("LR");
     Serial.println("Linear Guide2 Moving Reverse");
   break;
   case CX:
     digitalWrite(RELAY2, HIGH);
     delay(1000);
     digitalWrite(RELAY2, LOW);
     Serial.println("Air-Nipper Cutter Operated.");
   break;
   case CU:
     digitalWrite(RELAY10, HIGH); // Activate RELAY10 for Cutter Up Load
     Serial.println("RELAY10 ON");
   break;
   case CD:
     digitalWrite(RELAY10, LOW); // Deactivate RELAY10 for Cutter Down Load
     Serial.println("RELAY10 OFF");
   break;
   case X:
     stopMotor();
     Serial.println(" All motors stopped.");
   break;
   case FD:
     runFeeder(true, 2150); // Move feeder forward
     Serial.println("Feeder moving forward.");
   break;
   case RD:
     runFeeder(false, 2150); // Move feeder reverse
     Serial.println("Feeder moving reverse.");
   break;
   case AUTO:
   if (!autoMode) {  // Only prompt if not already in Auto mode
     resetMode();  // Reset mode before starting Auto Mode
     autoMode = true;  // Set Auto mode flag
     autoState = AUTO_IDLE;
     currentCycle = 0;
     Serial.println("Auto mode activated.");
     // delay(2000);
     Serial.println("Please enter the number of cycles:");

     unsigned long startWaitTime = millis();
     while (millis() - startWaitTime < 10000) {
       if (Serial.available() > 0) {
         numCycles = Serial.parseInt();
         if (numCycles > 0) { 
           Serial.print("Starting Auto Mode for " );
           Serial.print(numCycles);
           Serial.println(" cycles.");
           break;
          } else {
           Serial.println("Invalid input. Please entre a positive number:");
          }
        }
      }
      if (numCycles == 0) {
       Serial.println("Timeout! Auto mode cancelled.");
       autoMode = false;        
      }
    }
   break;
   case MANUAL:
   if (!manualMode) {  // Only prompt if not already in Manual mode
     resetMode();  // Reset mode before starting Manual Mode
     manualMode = true;  // Set Manual mode flag
     Serial.println("Manual mode activated.");
    }
   break;
   default:
     Serial.println("UNKNOWN COMMAND");
   break;
  }
}
void loop() { 
 // Process serial commands non-blockingly
 if (Serial.available() > 0) {
   String command = Serial.readStringUntil('\n');
   command.trim(); // Remove any trailing spaces or newlines
   Serial.flush();   // Clear buffer after reading
   Serial.println("Received: " + command);  // Debugging
      
   // Check if enough time has passed since the last command
   if (millis() - lastCommandTime >= DEBOUNCE_TIME) {
     Command cmd = parseCommand(command);
     lastCommandTime = millis();

     if (cmd == STOP || cmd == X) {
       // Handle stop command immediately
       if (autoMode) {
         stopAutoMode();
        }
       handleCommand(cmd);
      } else if (cmd == AUTO) {
        handleCommand(cmd);
      } else if (cmd == MANUAL) {
        handleCommand(cmd);
      } else {
       // Handle other commands (START, STOP, Q, etc.)
        handleCommand(cmd);
      }
      // Clear serial buffer after mode changes or commands
      resetSerialBuffer();
    }
  }
 // If in Auto mode, run the auto sequence incrementally
 if (autoMode) {
   updateAutoMode();
  }
  // State monitoring (no more proxy function calls)
 updateFeeder();

 if (motorRunning) {
   if (isClockwise) { // Check if the motor is running CW
     proxyA(); // Check Proxy A
    } 
   else { // Otherwise, it must be CCW
     proxyB(); // Check Proxy B
    }
  }
}
void updateAutoMode() {
 static unsigned long lastUpdateTime = 0;
 static bool firstTimeInState = true;
 unsigned long currentTime = millis();
  
 // Update at 100ms intervals for responsiveness
 if (currentTime - lastUpdateTime < 100) {
   return;
  }
 lastUpdateTime = currentTime;

 // Check for stop commands continuously
 if (checkStopCommand()) {
   stopAutoMode();
   return;
 }
 switch (autoState) {
   case AUTO_IDLE:
     if (firstTimeInState) {
       Serial.print("AUTO_IDLE - Starting cycle ");
       Serial.println(currentCycle + 1);
       firstTimeInState = false;
       autoStepStartTime = currentTime;
      }
     if (currentTime - autoStepStartTime > 500) {
       if (currentCycle < numCycles) {
         currentCycle++;
         Serial.print("Starting cycle ");
         Serial.println(currentCycle);
         autoState = AUTO_CHECK_POS;
         firstTimeInState = true;
         autoStepStartTime = currentTime;       
        } else {
         autoState = AUTO_COMPLETE;
        }
      }
   break;
   case AUTO_CHECK_POS:
     if (firstTimeInState) {
       int sensorState = digitalRead(PROXY_POS);
       Serial.print("Proxy POS Sensor State: ");
       Serial.println(sensorState);
        
       if (sensorState == LOW) {
         Serial.println("Proxy POS not active! Cannot proceed.");
         autoState = AUTO_STOPPED;
        } else {
         Serial.println("Proxy POS active. Proceeding to LU...");
         autoState = AUTO_LU;
         firstTimeInState = true;
        }
       autoStepStartTime = currentTime;
      }
   break;
   case AUTO_LU:
     if (firstTimeInState) {
       linearGuide1Complete = false;
       moveLinearGuide1("LU");
       Serial.println("Executing LU command...");
       firstTimeInState = false;
       autoStepStartTime = currentTime;
      }
     if (isLinearGuide1Complete()) {
       autoState = AUTO_LF;
       firstTimeInState = true;
       autoStepStartTime = currentTime;
       break;
      }
     // Timeout safety
     if (currentTime - autoStepStartTime > 15000) {
       Serial.println("LU operation timeout!");
       autoState = AUTO_STOPPED;
      }
   break;
   case AUTO_LF:
     if (firstTimeInState) {
       linearGuide2Complete = false;
       moveLinearGuide2("LF");
       Serial.println("Executing LF command...");
       firstTimeInState = false;
       autoStepStartTime = currentTime;
       delay(1000);
      }
     if (isLinearGuide2Complete()) {
       autoState = AUTO_VF;
       firstTimeInState = true;
       autoStepStartTime = currentTime;
       break;
      }
     if (currentTime - autoStepStartTime > 15000) {
       Serial.println("LF operation timeout!");
       autoState = AUTO_STOPPED;
      }
   break;
   case AUTO_VF:
     digitalWrite(RELAY8, HIGH);
     Serial.println("Executing VF command...");
     autoState = AUTO_FD;
     autoStepStartTime = currentTime;
     delay(1000);
   break;
   case AUTO_FD:
     if (firstTimeInState) {
       feederComplete = false;
       runFeeder(true, 2150);
       Serial.println("Executing FD command...");
       firstTimeInState = false;
       autoStepStartTime = currentTime;
      }
     if (isFeederComplete()) {
       autoState = AUTO_G3;
       firstTimeInState = true;
       autoStepStartTime = currentTime;
       break;
      }
     if (currentTime - autoStepStartTime > 5000) {
       Serial.println("FD operation timeout!");
       autoState = AUTO_STOPPED;
      }
   break;
   case AUTO_G3:
     digitalWrite(RELAY4, HIGH);
     Serial.println("Executing G3 command...");
     autoState = AUTO_LR;
     autoStepStartTime = currentTime;
   break;
   case AUTO_LR:
     if (firstTimeInState) {
       linearGuide2Complete = false;
       moveLinearGuide2("LR");
       Serial.println("Executing LR command...");
       firstTimeInState = false;
       autoStepStartTime = currentTime;
      }
     if (isLinearGuide2Complete()) {
       autoState = AUTO_G1;
       firstTimeInState = true;
       autoStepStartTime = currentTime;
       break;
      }
     if (currentTime - autoStepStartTime > 15000) {
       Serial.println("LR operation timeout!");
       autoState = AUTO_STOPPED;
      }
   break;
   case AUTO_G1:
     digitalWrite(RELAY3, HIGH);
     Serial.println("Executing G1 command...");
     autoState = AUTO_CU;
     autoStepStartTime = currentTime;
     delay(1000);
   break;
   case AUTO_CU:
     digitalWrite(RELAY10, HIGH);
     Serial.println("Executing CU command...");
     autoState = AUTO_CX;
     autoStepStartTime = currentTime;
     delay(1000);
   break;
   case AUTO_CX:
     digitalWrite(RELAY2, HIGH);
     Serial.println("Executing CX command...");
     autoState = AUTO_CD;
     autoStepStartTime = currentTime;
     delay(1000);
   break;
   case AUTO_CD:
     digitalWrite(RELAY2, LOW);
     digitalWrite(RELAY10, LOW);
     Serial.println("Executing CD command...");
     autoState = AUTO_LD;
     autoStepStartTime = currentTime;
     delay(1000);
   break;
   case AUTO_LD:
     if (firstTimeInState) {
       linearGuide1Complete = false;
       moveLinearGuide1("LD");
       Serial.println("Executing LD command...");
       firstTimeInState = false;
       autoStepStartTime = currentTime;
      }
     if (isLinearGuide1Complete()) {
       autoState = AUTO_CW;
       firstTimeInState = true;
       autoStepStartTime = currentTime;
       break;
      }
     if (currentTime - autoStepStartTime > 15000) {
       Serial.println("LD operation timeout!");
       autoState = AUTO_STOPPED;
      }
   break;
   case AUTO_CW:
     if (firstTimeInState) {
       motorComplete = false;
       runMotor(true);
       Serial.println("Executing CW command...");
       firstTimeInState = false;
       autoStepStartTime = currentTime;
       autoState = AUTO_CW_RUNNING;  // Move to intermediate state
      }
   break;
   case AUTO_CW_RUNNING:
     // Check if motor has completed its operation
     if (isMotorComplete()) {
       Serial.println("CW operation completed.");
       autoState = AUTO_VR;  // Move to next main state
       firstTimeInState = true;
      }
     // Timeout safety
     if (currentTime - autoStepStartTime > 30000) {
       Serial.println("CW operation timeout!");
       stopMotor();
       autoState = AUTO_STOPPED;
      }
   break;
   case AUTO_VR:
     digitalWrite(RELAY8, LOW);
     Serial.println("Executing VR command...");
     autoState = AUTO_OG;
     autoStepStartTime = currentTime;
   break;
   case AUTO_OG:
     if (firstTimeInState) {
       runOpenGripper();
       Serial.println("Executing OG command...");
       firstTimeInState = false;
       autoStepStartTime = currentTime;
      }
     // Wait 3000ms for OG delay
     if (currentTime - autoStepStartTime >= 3000) {
       autoState = AUTO_CCW;
       firstTimeInState = true;
       autoStepStartTime = currentTime;
      }
   break;
   case AUTO_CCW:
     if (firstTimeInState) {
       motorComplete = false;
       runMotor(false); // Start CCW motor
       Serial.println("Executing CCW command...");
       firstTimeInState = false;
       autoStepStartTime = currentTime;
       autoState = AUTO_CCW_RUNNING;  // Move to intermediate state
      }
   break;
   case AUTO_CCW_RUNNING:
     // Check if motor has completed its operation
     if (isMotorComplete()) {
       Serial.println("CCW operation completed.");
       // Move to whatever state comes after CCW, or back to IDLE
       autoState = AUTO_IDLE; 
       firstTimeInState = true;
      }
     // Timeout safety
     if (currentTime - autoStepStartTime > 30000) {
       Serial.println("CCW operation timeout!");
       stopMotor();
       autoState = AUTO_STOPPED;
      }
   break;
   case AUTO_COMPLETE:
     Serial.println("Auto mode completed all cycles.");
     resetMode();
     autoMode = false;
   break;
   case AUTO_STOPPED:
     // Do nothing until reset
     default:
   break;
  }
}
void stopAutoMode() {
 // Stop all ongoing operations
 stopMotor();
 digitalWrite(RELAY2, LOW);
 digitalWrite(RELAY3, LOW);
 digitalWrite(RELAY4, LOW);
 digitalWrite(RELAY8, LOW);
 digitalWrite(RELAY9, LOW);
 digitalWrite(RELAY10, LOW);
 digitalWrite(DRIVER1_ENA, HIGH);
 digitalWrite(DRIVER2_ENA, HIGH);
 digitalWrite(EN_FEEDER, LOW);
  
 autoState = AUTO_STOPPED;
 Serial.println("Auto mode stopped by command.");
}
bool checkStopCommand() {
 while (Serial.available() > 0) {
   String command = Serial.readStringUntil('\n');
   command.trim();
    
   if (command == "STOP" || command == "X") {
     Serial.println("Stop command received: " + command);
     return true;
    }
   // Put the command back into the buffer for normal processing
   // for (int i = 0; i < command.length(); i++) {
   //   Serial.print(command[i]);
   // }
  }
 return false;
}
void setFeederMotorSpeed(int speed, bool forward) {
 analogWrite(RPWM_FEEDER, forward ? map(speed, 0, 100, 0, 255) : 0);
 analogWrite(LPWM_FEEDER, forward ? 0 : map(speed, 0, 100, 0, 255));
}
void runFeeder(bool forward, unsigned long durationMillis, int speed) {
 feederComplete = false;
 if (feederRunning) {
   Serial.println("Feeder is already running.");
   return; // Return the current direction if already running
  }
 // Start the feeder motor
 setFeederMotorSpeed(speed, forward);
 digitalWrite(EN_FEEDER, HIGH);  // Enable feeder motor 
 delay (100);
 feederRunning = true; // set feeder running flag to true
 isForward = forward;
 // Set duration if provided (0 means run indefinitely)
 if (durationMillis > 0) {
   feederStartTime = millis();
   feederDuration = durationMillis;
  } else {
   feederDuration = 0; // Run until manually stopped
  }
 Serial.print(forward ? "FEEDER MOTOR FD STARTED" : "FEEDER MOTOR RD STARTED");
 if (durationMillis > 0) {
   Serial.print(" for ");
   Serial.print(durationMillis / 1000.0);
   Serial.println(" seconds");
  } else {
   Serial.println(" (indefinite run)");
  }
}
void updateFeeder() {
 if (feederRunning && feederDuration > 0 && (millis() - feederStartTime >= feederDuration)) {
   // Time's up - stop the motor
   digitalWrite(EN_FEEDER, LOW);
   stopMotor();
   feederRunning = false;
   Serial.println("Feeder motor stopped (time elapsed)");
   // SET COMPLETION FLAG HERE
   setFeederComplete();
  }
}

void moveLinearGuide1(String direction1) {
 linearGuide1Complete = false;
 if (direction1 == "LU") {  // Forward Direction (Linear motor up)
   int sensorState = digitalRead(PROXY_POS);
   Serial.print("Proxy POS Sensor State: ");
   Serial.println(sensorState); // Print the actual state of the sensor

   // Check if proxyPOS is active
   if (sensorState == LOW) { // Assuming HIGH means the sensor is inactive
     Serial.println("Cannot move up. Proxy POS is not active.");
     return; // Exit the function if proxyPOS is not active
    }
    // Check if already at upper limit (like LF/LR does)
   if (digitalRead(LIMIT_SWITCH2) == LOW) {
     Serial.println("Already at Limit Switch 2");
     setLinearGuide1Complete(); // ADD THIS - was missing!
     return;
    }
   Serial.println("LU");
   digitalWrite(DRIVER1_ENA, HIGH);
   delay(100);
   digitalWrite(RELAY9, HIGH); // Turn ON Relay 1
   Serial.println("RELAY9 ON"); // Notify Python
   delay(100);
   digitalWrite(DRIVER1_DIR, HIGH); // Set direction forward
   delay(100);
   digitalWrite(DRIVER1_ENA, LOW); // Enable motor
   delay(200);

   Serial.print("Limit States - 1: ");
   Serial.print(digitalRead(LIMIT_SWITCH1));
   Serial.print("  & Limit States - 2: ");
   Serial.println(digitalRead(LIMIT_SWITCH2));

   // Direct stepper movement (similar to LF/LR)
   unsigned long startTime = millis();
   const unsigned long timeout = 15000; // 15 second timeout
    
   while (digitalRead(LIMIT_SWITCH2) == HIGH) {
     digitalWrite(DRIVER1_STEP, HIGH);
     delayMicroseconds(40);
     digitalWrite(DRIVER1_STEP, LOW);
     delayMicroseconds(45);
      
     // Check for timeout
     if (millis() - startTime > timeout) {
       Serial.println("LU movement timeout! Stopping.");
       break;
      }
     if (checkStopCommand()) {
       Serial.println("Emergency Stop Triggered");
       stopMotor();
       digitalWrite(RELAY9, LOW);
       return;
      }
    }
   digitalWrite(RELAY9, LOW);
   Serial.println("RELAY9 OFF");
   Serial.println("Motors stopped at Limit Switch 2 or Emergency Stop.");
   setLinearGuide1Complete();
  }
 else if (direction1 == "LD") {  // Linear motor down
   // Check if already at lower limit
   if (digitalRead(LIMIT_SWITCH1) == LOW) {
     Serial.println("Already at Limit Switch 1");
     setLinearGuide1Complete();
     return;
    }
   Serial.println("LD");
   digitalWrite(DRIVER1_ENA, HIGH);
   delay(100);
   digitalWrite(RELAY9, HIGH);
   Serial.println("RELAY9 ON");
   delay(100);
   digitalWrite(DRIVER1_DIR, LOW); // Set direction for DOWN movement
   delay(100);
   digitalWrite(DRIVER1_ENA, LOW);
   delay(200);
  
   Serial.print("Limit States - 1: ");
   Serial.print(digitalRead(LIMIT_SWITCH1));
   Serial.print("  & Limit States - 2: ");
   Serial.println(digitalRead(LIMIT_SWITCH2));
   
   // Direct stepper movement (similar to LF/LR)
   unsigned long startTime = millis();
   const unsigned long timeout = 15000; // 15 second timeout
    
   while (digitalRead(LIMIT_SWITCH1) == HIGH) {
     digitalWrite(DRIVER1_STEP, HIGH);
     delayMicroseconds(40);
     digitalWrite(DRIVER1_STEP, LOW);
     delayMicroseconds(45);
      
     // Check for timeout
     if (millis() - startTime > timeout) {
       Serial.println("LD movement timeout! Stopping.");
       break;
      }
     if (checkStopCommand()) {
       Serial.println("Emergency Stop Triggered");
       stopMotor();
       digitalWrite(RELAY9, LOW);
       return;
      }
    }
   digitalWrite(RELAY9, LOW);
   Serial.println("RELAY9 OFF");
   Serial.println("Motors stopped at Limit Switch 1 or Emergency Stop.");
   delay(100);
   setLinearGuide1Complete();
  }
 else {
   Serial.println("Invalid direction for moveLinearGuide.");
  }
}
void moveLinearGuide2(String direction2) {
 linearGuide2Complete = false;
 if (direction2 == "LF") {  // Linear motor forward
   if (digitalRead(LIMIT_SWITCH4) == LOW) {
     Serial.println("Already at Limit Switch 4");
     setLinearGuide2Complete();
     return;
    }
   Serial.println("LF");
   digitalWrite(DRIVER2_ENA, HIGH);
   delay(100);
   digitalWrite(RELAY7, HIGH); // Turn ON Relay 7
   Serial.println("RELAY7 ON"); // Notify Python
   delay(100);
   digitalWrite(DRIVER2_DIR, HIGH); // Set direction forward
   delay(100);
   digitalWrite(DRIVER2_ENA, LOW); // Enable motor
   delay(100);
   
   Serial.print("Limit States - 3: ");
   Serial.print(digitalRead(LIMIT_SWITCH3));
   Serial.print("  & Limit States - 4: ");
   Serial.println(digitalRead(LIMIT_SWITCH4));

   // Direct stepper movement (similar to LF/LR)
   unsigned long startTime = millis();
   const unsigned long timeout = 15000; // 15 second timeout

   while (digitalRead(LIMIT_SWITCH4) == HIGH) {
     digitalWrite(DRIVER2_STEP, HIGH);
     delayMicroseconds(20);
     digitalWrite(DRIVER2_STEP, LOW);
     delayMicroseconds(25);
     
     // Check for timeout
     if (millis() - startTime > timeout) {
       Serial.println("LF movement timeout! Stopping.");
       break;
      }
     if (checkStopCommand()) {
       Serial.println("Emergency Stop Triggered");
       stopMotor();
       digitalWrite(RELAY7, LOW);
       digitalWrite(DRIVER2_ENA, HIGH);
       return;
      }
    }
   digitalWrite(DRIVER2_ENA, HIGH);
   delay(100);
   digitalWrite(RELAY7, LOW); // Turn OFF Relay 7
   Serial.println("RELAY7 OFF"); // Notify Python
   Serial.println("Motors stopped at Limit Switch 4 or Emergency Stop.");
   delay (100);
   // SET COMPLETION FLAG HERE
   setLinearGuide2Complete();
  } 
 else if (direction2 == "LR") {  // Move Reverse Direction (Linear motor reverse)
  // Check if already at lower limit
   if (digitalRead(LIMIT_SWITCH3) == LOW) { // Already at limit
     Serial.println("Already at Limit Switch 3");
     setLinearGuide2Complete();
     return;
    }
   Serial.println("LR");
   digitalWrite(DRIVER2_ENA, HIGH);
   delay(100);
   digitalWrite(RELAY7, HIGH); // Turn ON Relay 7
   Serial.println("RELAY7 ON"); // Notify Python
   delay(100);
   digitalWrite(DRIVER2_DIR, LOW); // Set direction reverse
   delay(100);
   digitalWrite(DRIVER2_ENA, LOW); // Enable motor
   delay(100);
   
   Serial.print("Limit States - 3: ");
   Serial.print(digitalRead(LIMIT_SWITCH3));
   Serial.print("  & Limit States - 4: ");
   Serial.println(digitalRead(LIMIT_SWITCH4));
      
   // Run BTS7960B motor forward at 5 speed
   digitalWrite(EN_FEEDER, HIGH); // Enable feeder motor
   setFeederMotorSpeed(35, true); // Use the helper function to set speed and direction

   // Direct stepper movement (similar to LF/LR)
   unsigned long startTime = millis();
   const unsigned long timeout = 15000; // 15 second timeout
   
   while (digitalRead(LIMIT_SWITCH3) == HIGH) {
     digitalWrite(DRIVER2_STEP, HIGH);
     delayMicroseconds(20);
     digitalWrite(DRIVER2_STEP, LOW);
     delayMicroseconds(25);

     // Check for timeout
     if (millis() - startTime > timeout) {
       Serial.println("LD movement timeout! Stopping.");
       break;
      }
      
     if (checkStopCommand()) {
       Serial.println("Emergency Stop Triggered");
       stopMotor();
       digitalWrite(RELAY7, LOW);
       digitalWrite(DRIVER2_ENA, HIGH);
       return;
      }
    }
   // Stop both motors when Limit Switch 3 is triggered or X command is received
   digitalWrite(DRIVER2_ENA, HIGH);
   delay(100);
   digitalWrite(RELAY7, LOW); // Turn OFF Relay 7
   digitalWrite(EN_FEEDER, LOW);
   Serial.println("RELAY7 OFF"); // Notify Python
   Serial.println("Motors stopped at Limit Switch 3 or Emergency Stop.");
   delay (100);
   // SET COMPLETION FLAG HERE
   setLinearGuide2Complete();
  }
  else {
   Serial.println("Invalid direction for moveLinearGuide.");
  }
}
void moveStepper1(bool direction, int stopPin) {
 digitalWrite(DRIVER1_DIR, direction);
 digitalWrite(RELAY9, HIGH);
 digitalWrite(DRIVER1_ENA, LOW); // Enable stepper motor
 
 // Move stepper motor until limit switch is triggered or "X" command is received
 while (digitalRead(stopPin) == HIGH) {
   digitalWrite(DRIVER1_STEP, HIGH);
   delayMicroseconds(25);
   digitalWrite(DRIVER1_STEP, LOW);
   delayMicroseconds(30);
   
   if (checkStopCommand()) {
     Serial.println("Emergency Stop Triggered");
     stopMotor(); // Stop when "X" command is received
     return;
    }
  }
 digitalWrite(RELAY9, LOW); // Turn off Relay 1 to stop stepper motor control
}
void runOpenGripper() {
 // Define the behavior for Opening Grippers 1 & 2 after tying
 digitalWrite(RELAY3, LOW); // Deactivate Gripper 1 (RELAY3)
 digitalWrite(RELAY4, LOW); // Deactivate Gripper 2 (RELAY4)
 delay(1000);
}
void stopMotor() {
 // Stop Feeder motor
 analogWrite(RPWM_FEEDER, 0);
 analogWrite(LPWM_FEEDER, 0);
 digitalWrite(EN_FEEDER, LOW);
 digitalWrite(DRIVER1_ENA, HIGH);
 digitalWrite(DRIVER2_ENA, HIGH);
 feederRunning = false; // Reset feeder running flag
 // Turn off relays
 digitalWrite(RELAY9, LOW);
 digitalWrite(RELAY5, LOW);      // Disable both relays
 digitalWrite(RELAY6, LOW);
 motorRunning = false;  
 Serial.println("Motor stopped");
}
void resetSerialBuffer() {
 while (Serial.available()) {
   Serial.read();  // Clear serial input buffer
  }
}
void resetMode() {
 autoMode = false; // Reset Auto mode flag
 manualMode = false; // Reset Manual mode flag
 currentMode = NONE; // Reset the current mode to a default state
 resetSerialBuffer(); // Clear the serial buffer
 Serial.println("Enter next command (START, STOP, Q):");  // Prompt for the next command
}
void runMotor(bool clockwise) {
 motorComplete = false;
 if (motorRunning) {
   Serial.println("Motor is already running.");
   return isClockwise; // Return the current direction if already running
  }
 int relayPin = clockwise ? RELAY5 : RELAY6; // Choose correct relay
 digitalWrite(relayPin, HIGH); // Start motor
 motorRunning = true; // Set motor running flag to true
 countA = 0; // Reset count for Proxy A
 countB = 0; // Reset count for Proxy B
 countingEnabled = true; // Enable counting
 isClockwise = clockwise; // Set the direction

 // Disable counting for the opposite proxy and Proxy POS
 if (clockwise) {
   countProxyBEnabled = false; // Disable counting for Proxy B
   countProxyPOSEnabled = false; // Disable counting for Proxy POS
   countProxyAEnabled = true; // Enable counting for current direction
  } 
  else {
   countProxyAEnabled = false; // Disable counting for Proxy A
   countProxyPOSEnabled = false; // Disable counting for Proxy POS
   countProxyBEnabled = true; // Enable counting for current direction
  }
 Serial.println(clockwise ? "INDUCTION MOTOR CW STARTED" : "INDUCTION MOTOR CCW STARTED");
 return clockwise; // Return the direction
}
void proxyA() {
 static int lastStateA = LOW; // Remember previous state for Proxy A
 static unsigned long lastTriggerTimeA = 0;
 const unsigned long debounceDelay = 300;
 const unsigned long minCountInterval = 2600; // Minimum time between counts (1 second)
 
 int sensorStateA = digitalRead(PROXY_A); // Read the current state of the sensor
 unsigned long currentTime = millis();

 // Status feedback
 Serial.print("Proxy A Status: ");
 Serial.println(sensorStateA == HIGH ? "Triggered (High)" : "Not Triggered (Low)");

 // Check for Proxy A (CW)
 if (isClockwise && countProxyAEnabled) {
   // Only detect Rising edge (LOW to HIGH)
   if (lastStateA == LOW && sensorStateA == HIGH) {
     // Debounce check
     if ((currentTime - lastTriggerTimeA) > debounceDelay && (currentTime - lastTriggerTimeA) > minCountInterval) {
       countA++;  // Increment count for Proxy A
       lastTriggerTimeA = currentTime;  // Update last trigger time
       Serial.print("Proxy A Count: ");
       Serial.println(countA);

       // Stop relay 4 if count reaches 4
       if (countA >= 4) {
         digitalWrite(RELAY5, LOW); // Stop relay 4
         stopMotor();
         motorRunning = false; // Set flag to stop motor
         countA = 0; // Reset count after stopping
         countProxyAEnabled = false; // Disable counting for Proxy A
         countProxyBEnabled = true; // Re-enable counting for Proxy B
         // SET COMPLETION FLAG HERE
         setMotorComplete();
        }
      }
    }
   lastStateA = sensorStateA; // Update last state for Proxy A
  }
}
void proxyB() {
 static int lastStateB = LOW; // Remember previous state for Proxy B
 static unsigned long lastTriggerTimeB = 0;
 const unsigned long debounceDelay = 300;
 const unsigned long minCountInterval = 2600; // Minimum time between counts (1 second)

 int sensorStateB = digitalRead(PROXY_B);  // Read the current state of the sensor
 unsigned long currentTime = millis();

 // Status feedback
 Serial.print("Proxy B Status: ");
 Serial.println(sensorStateB == HIGH ? "Triggered (High)" : "Not Triggered (Low)");

 // Check for Proxy B (CCW)
 if (!isClockwise && countProxyBEnabled) {
   // Only detect Rising edge (LOW to HIGH)
   if (lastStateB == LOW && sensorStateB == HIGH) {
     // Debounce check
      if ((currentTime - lastTriggerTimeB) > debounceDelay && (currentTime - lastTriggerTimeB) > minCountInterval) {
       countB++;  // Increment count for Proxy B
       lastTriggerTimeB = currentTime;
       Serial.print("Proxy B Count: ");
       Serial.println(countB);

        // Stop relay 5 if count reaches 5
        if (countB >= 4) {
         digitalWrite(RELAY6, LOW); // Stop relay 5
         stopMotor();
         motorRunning = false; // Set flag to stop motor
         countB = 0; // Reset count after stopping
         countProxyBEnabled = false; // Disable counting for Proxy B
         countProxyAEnabled = true; // Re-enable counting for Proxy A
         // SET COMPLETION FLAG HERE
         setMotorComplete();
        }
      }
    }
   lastStateB = sensorStateB; // Update last state for Proxy B
  }
}

/* END CODE */