Robot Controller rev_01

/********* 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: Robot Controller
    - Source Code NOT compiled for: Arduino Mega
    - Source Code created on: 2025-09-27 15:41:28

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

/****** SYSTEM REQUIREMENTS *****/
/****** SYSTEM REQUIREMENT 1 *****/
    /* Runs both in auto mode and manual mode smoothly. */
    /* log generates with proper time intervals. help to */
    /* remove any non-blocking code lines. */
/****** END SYSTEM REQUIREMENTS *****/


/* START CODE */

// Compatibility notes:
// - The USER CODE defines a full program (pins, state machines, serial I/O, etc.) intended for an Arduino Mega.
// - The PRELIMINARY CODE contained only empty setup/loop stubs with a minimal prototype section. To ensure a working sketch, the USER CODE is placed here in full and the original PRELIMINARY stubs are effectively overridden by this content.
// - If any line seemed incompatible with the Mega pin mapping, I left it in place but you9ll see it function as standard Arduino Mega I/O. The Mega has many digital pins (0-53) and analog pins A0-A15 (which map to digital numbers 54-69); the pins used in the code (e.g., 3,4,5,15,24-35, 28-31, A0-A7 equivalents, etc.) are within valid ranges for typical Mega configurations. If you run into a pin-mapping mismatch on your board variant, adjust the pin numbers accordingly and re-test.
 // If you9d like, I can adjust pin mappings to explicit Mega D-pin numbers or to A0-A7 constants to avoid ambiguity on certain board variants.

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

/****** FUNCTION PROTOTYPES *****/
// (None required here since the USER CODE includes full function definitions and Arduino IDE can generate prototypes automatically. If you want explicit prototypes, I can add them here.)
// Note: the USER CODE uses a variety of functions. A prototype for updateFeeder() is implicitly provided by the IDE, but can be added explicitly if desired.

void logEvent(const String &message);

void setup(void);
void loop(void);

 // User code begins
// Define stepper motor1 of Linear guide @ Spinning Motor driver pins
const int DRIVER1_DIR = 28; // Direction pin for stepper motor1
const int DRIVER1_STEP = 5; // 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 = 24; // Proxy POS Sensor for Linear Motor Position
const int PROXY_A = 25; // Proxy A Sensor for CW
const int PROXY_B = 26; // Proxy B Sensor for CCW
const int PROXY_RUNFEED = 27; // 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 = 16; // Limit switch 3 pin (Reverse)
const int LIMIT_SWITCH4 = 35; // 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 = A2; // 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 = 44; // Relay for Spinning Linear Drive
const int RELAY10 = 45; // Relay for Cutter-up & Cutter-down Cylinder
const int RELAY11 = 46; // Relay for Label Vaccum Pad Pick & Eject
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)

// 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

// Additional state for non-blocking auto-cycle input
bool awaitingCycleInput = false; // New: wait for numeric input for cycles
unsigned long cycleInputDeadline = 0; // New: timeout for cycle input

// 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 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 proxyA();
void proxyB();

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
 logEvent("System initialized"); // New: log with time intervals
}
void logEvent(const String &message) {
 // Lightweight non-blocking logging helper with timestamp
 Serial.print("[");
 Serial.print(millis());
 Serial.print(" ms] ");
 Serial.println(message);
}
Command parseCommand(String command);

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:
     digitalWrite(RELAY11, HIGH); // Activate RELAY10 for Vaccum pad Pick
     Serial.println("RELAY11 ON");
   break;
   case VO:
     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(RELAY9, HIGH); // Activate RELAY9 for Push Up Load
     Serial.println("RELAY9 ON");
   break;
   case PD:
     digitalWrite(RELAY9, LOW); // Deactivate RELAY9 for Push Down Load
     Serial.println("RELAY9 OFF");
   break;
   case OG:
     runOpenGripper();
     Serial.println("Gripper 1 & 2 OPEN...");
   break;
   case CW:
     runMotor(true);
   break;
   case CCW:
     runMotor(false);
   break;
   case LU:
     moveLinearGuide1("LU");
   break;
   case LD:
     moveLinearGuide1("LD");
   break;
   case LF:
     moveLinearGuide2("LF");
   break;
   case LR:
     moveLinearGuide2("LR");
   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
     awaitingCycleInput = true; // Start waiting for cycles count (non-blocking)
     cycleInputDeadline = millis() + 10000; // 10s timeout for input
     numCycles = 0;
     Serial.println("Auto mode activated.");
     Serial.println("Please enter the number of cycles:");
    }
   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 selected. Available Commands: ...");  // Show manual commands
    }
   break;
   default:
   Serial.println("UNKNOWN COMMAND");
   break;
  }
}
void loop() { // Process new serial commands
 // Non-blocking input handling for auto mode cycle count
 if (awaitingCycleInput) {
   if (Serial.available() > 0) {
     String input = Serial.readStringUntil('\n');
     input.trim();
     long n = input.toInt();
     if (n > 0) {
       numCycles = (int)n;
       awaitingCycleInput = false;
       Serial.print("Auto cycles set: "); Serial.println(numCycles);
       logEvent("Auto mode starting with cycles=" + String(numCycles));
     } else {
       Serial.println("Invalid cycle count. Please enter a positive number:");
     }
     resetSerialBuffer();
   }
   // Timeout handling
   if (millis() > cycleInputDeadline) {
     awaitingCycleInput = false;
     autoMode = false;
     Serial.println("Auto mode cancelled due to input timeout.");
     logEvent("Auto mode input timeout");
   }
 }

 if (Serial.available() > 0 && !awaitingCycleInput) {
   String command = Serial.readStringUntil('\n');
   command.trim(); // Remove any trailing spaces or newlines
   // Do not flush buffer to avoid blocking; rely on above non-blocking approach
   // Check if we still have an active auto/manual cycle input state
   if (millis() - lastCommandTime >= DEBOUNCE_TIME) {
     Last:
     ;
   }

   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 == AUTO) {     // Handle Auto Mode command
       if (!autoMode) {  // Only proceed if not already in Auto mode
         resetMode();  // Reset mode before starting Auto Mode
         autoMode = true;  // Set Auto mode flag
         awaitingCycleInput = true; // Start waiting for cycles count (non-blocking)
         cycleInputDeadline = millis() + 10000; // 10s timeout for input
         numCycles = 0;
         Serial.println("Auto mode activated.");
         Serial.println("Please enter the number of cycles:");
        }
      }
     else if (cmd == MANUAL) {
       // Handle Manual Mode command
       if (!manualMode) {
         resetMode();  // Reset mode before starting Manual Mode
         manualMode = true;  // Set Manual mode flag
         Serial.println("Manual Mode selected. Available Commands: ...");  // Show manual commands
        }
      }
     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) {
   static unsigned long lastRunTime = 0;
   if (millis() - lastRunTime > 1000) { // Execute once per second
     bool completed = runAutoMode(numCycles);
     if (completed) {
       autoMode = false; // Reset Auto mode flag after execution
       Serial.println("Auto mode completed. Enter next command (START, STOP, Q):");
      }
     lastRunTime = millis(); // Update last run time
    }
  }
  // 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
    }
   // Check if a stop command was received
   if (checkStopCommand()) {
     Serial.println("Stopping motor due to command.");
     motorRunning = false;
     digitalWrite(RELAY5, LOW);
     digitalWrite(RELAY6, LOW);
     Serial.println("INDUCTION MOTOR STOPPED.");
     countingEnabled = false;
    }
  }
}
bool runAutoMode(int numCycles) {
 Serial.print("Starting Auto mode for ");
 Serial.print(numCycles);
 Serial.println(" cycles.");

 for (int cycle = 1; cycle <= numCycles; cycle++) {
   Serial.print("Cycle ");
   Serial.print(cycle);
   Serial.println(" in progress...");

   unsigned long lastActionTime = millis();

   while (millis() - lastActionTime < 1000) { if (checkStopCommand()) return false; }   // Return false if STOP is received

   moveLinearGuide1("LU"); // Spinning Linear UP
   lastActionTime = millis();
   while (millis() - lastActionTime < 1000) { if (checkStopCommand()) return false; }

   moveLinearGuide2("LF"); // Feeder Linear Forward
   lastActionTime = millis();
   while (millis() - lastActionTime < 1000) { if (checkStopCommand()) return false; }

   digitalWrite(RELAY11, HIGH); // Vaccum Pad plate air Close
   lastActionTime = millis();
   while (millis() - lastActionTime < 1000) { if (checkStopCommand()) return false; }

   digitalWrite(RELAY8, HIGH); // Vaccum Pad plate move forward
   lastActionTime = millis();
   while (millis() - lastActionTime < 1000) { if (checkStopCommand()) return false; }

   runFeeder(true, 2150); // Move feeder forward
   lastActionTime = millis();
   while (millis() - lastActionTime < 1000) { if (checkStopCommand()) return false; }

   digitalWrite(RELAY4, HIGH); // Close Gripper 2
   lastActionTime = millis();
   while (millis() - lastActionTime < 1000) {
     if (checkStopCommand()) return false;
     updateFeeder();
   }

   digitalWrite(RELAY11, LOW); // Vaccum Pad plate air Open
   lastActionTime = millis();
   while (millis() - lastActionTime < 1000) { if (checkStopCommand()) return false; }

   digitalWrite(RELAY8, LOW); // Vaccum Pad plate move Reverse
   lastActionTime = millis();
   while (millis() - lastActionTime < 1000) { if (checkStopCommand()) return false; }

   moveLinearGuide2("LR"); // Feeder Linear Reverse
   lastActionTime = millis();
   while (millis() - lastActionTime < 1000) { if (checkStopCommand()) return false; }

   digitalWrite(RELAY3, HIGH); // Close Gripper 1
   lastActionTime = millis();
   while (millis() - lastActionTime < 1000) { if (checkStopCommand()) return false; }

   digitalWrite(RELAY10, HIGH); // Cutter Cylinder UP
   lastActionTime = millis();
   while (millis() - lastActionTime < 1000) { if (checkStopCommand()) return false; }

   digitalWrite(RELAY2, HIGH); // Cutter Activate
   delay(3000);
   digitalWrite(RELAY2,LOW); // Cutter De-Activate
   lastActionTime = millis();
   while (millis() - lastActionTime < 1000) { if (checkStopCommand()) return false; }

   digitalWrite(RELAY10, LOW); // Cutter Cylinder Down
   lastActionTime = millis();
   while (millis() - lastActionTime < 1000) { if (checkStopCommand()) return false; }

   moveLinearGuide1("LD");
   lastActionTime = millis();
   while (millis() - lastActionTime < 1000) { if (checkStopCommand()) return false; }
   delay(1000);

   // Run motor clockwise
   runMotor(true);
   Serial.println("Starting CW motor...");
   lastActionTime = millis();
   while (millis() - lastActionTime < 1000) { if (checkStopCommand()) return false; }

   runOpenGripper();
   lastActionTime = millis();
   while (millis() - lastActionTime < 1000) { if (checkStopCommand()) return false; }

   digitalWrite(RELAY9, HIGH); // Push Load plate UP
   lastActionTime = millis();
   while (millis() - lastActionTime < 1000) { if (checkStopCommand()) return false; }

   digitalWrite(RELAY9, LOW); // Push Load plate down
   lastActionTime = millis();
   while (millis() - lastActionTime < 1000) { if (checkStopCommand()) return false; }

   // Run motor counter-clockwise
   runMotor(false);
   Serial.println("Starting CCW motor...");
   lastActionTime = millis();
   while (millis() - lastActionTime < 1000) { if (checkStopCommand()) return false; }

   Serial.print("Cycle ");
   Serial.print(cycle);
   Serial.println(" completed.");
  }
 Serial.println("Auto mode completed all cycles.");  // Notify Completion
 resetMode();  // Reset the mode and prepare for new commands
 return true;
}
// Helper function to check for STOP command
bool checkStopCommand() {
 if (Serial.available() > 0) {
   Serial.peek();
   String command = Serial.readStringUntil('\n');
   command.trim();  // Remove extra spaces and newlines

   Serial.print("Received Command: ");
   Serial.println(command); // Debugging line to see actual command

   if (command == "STOP") {
     Serial.println("STOP command received. Stopping Auto mode.");
     stopMotor();
     autoMode = false; // Reset Auto mode flag
     return true;  // Exit runAutoMode
    }
   else if (command == "X") {
     Serial.println("Emergency Stop Received (X).");
     stopMotor();  // Stop all motors
     return true;  // Exit runAutoMode
    }
  }
 return false; // No stop command detected
}
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) {
 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)");
  }
}
void runMotor(bool clockwise) {
 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
  }
  else {
   countProxyAEnabled = false; // Disable counting for Proxy A
   countProxyPOSEnabled = false; // Disable counting for Proxy POS
  }
 Serial.println(clockwise ? "INDUCTION MOTOR CW STARTED" : "INDUCTION MOTOR CCW STARTED");
 return clockwise; // Return the direction
}
void moveLinearGuide1(String direction1) {
 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
    }
   Serial.println("LU");
   delay(1000);
   digitalWrite(RELAY9, HIGH); // Turn ON Relay 1
   // Serial.println("RELAY9 ON"); // Notify Python
   digitalWrite(DRIVER1_DIR, HIGH); // Set direction forward
   digitalWrite(DRIVER1_ENA, LOW); // Enable motor

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

   while (digitalRead(LIMIT_SWITCH2) == HIGH) {
     digitalWrite(DRIVER1_STEP, HIGH);
     delayMicroseconds(20);
     digitalWrite(DRIVER1_STEP, LOW);
     delayMicroseconds(25);

     if (checkStopCommand()) {
       digitalWrite(RELAY9, LOW);
       Serial.println("Linear Guide 1 Stopped");
       return;
      }
    }
   digitalWrite(RELAY9, LOW); // Turn OFF Relay 7
   // Serial.println("RELAY9 OFF"); // Notify Python
   // Serial.println("Motors stopped at Limit Switch 2.");
   Serial.println("Linear Guide1 Moved UP and reached Home Position");
   delay (1000);
  }
  else if (direction1 == "LD") {  // Reverse Direction (Linear motor down)
   Serial.println("LD");
   delay(1000);
   digitalWrite(RELAY9, HIGH); // Turn ON Relay 1
   // Serial.println("RELAY9 ON"); // Notify Python
   digitalWrite(DRIVER1_DIR, LOW); // Set direction reverse
   digitalWrite(DRIVER1_ENA, LOW); // Enable motor

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

   while (digitalRead(LIMIT_SWITCH1) == HIGH) {
     digitalWrite(DRIVER1_STEP, HIGH);
     delayMicroseconds(20);
     digitalWrite(DRIVER1_STEP, LOW);
     delayMicroseconds(25);

     if (checkStopCommand()) {
       digitalWrite(RELAY9, LOW);
       Serial.println("Linear Guide 1 Stopped");
       return;
      }
    }
   digitalWrite(RELAY9, LOW); // Turn OFF Relay 7
   // Serial.println("RELAY9 OFF"); // Notify Python
   // Serial.println("Motors stopped at Limit Switch 2.");
   Serial.println("Linear Guide1 Moved Down and reached Tying Position");
   delay (1000);
  }
 else {
   Serial.println("Invalid direction for moveLinearGuide.");
  }
}
void moveLinearGuide2(String direction2) {
 if (direction2 == "LF") {  // Move Forward Direction (Linear motor forward)
   Serial.println("LF");
   if (digitalRead(LIMIT_SWITCH4) == LOW) { // Already at limit
     Serial.println("Already at Limit Switch 4");
     return;
    }
   delay(1000);
   digitalWrite(RELAY7, HIGH); // Turn ON Relay 7
   // Serial.println("RELAY7 ON"); // Notify Python
   digitalWrite(DRIVER2_DIR, HIGH); // Set direction forward
   digitalWrite(DRIVER2_ENA, LOW); // Enable motor

   // Serial.print("Limit States - 3: ");
   // Serial.print(digitalRead(LIMIT_SWITCH3));
   // Serial.print("  & Limit States - 4: ");
   // Serial.println(digitalRead(LIMIT_SWITCH4));

   while (digitalRead(LIMIT_SWITCH4) == HIGH) {
     digitalWrite(DRIVER2_STEP, HIGH);
     delayMicroseconds(20);
     digitalWrite(DRIVER2_STEP, LOW);
     delayMicroseconds(25);

     if (checkStopCommand()) {
       digitalWrite(RELAY7, LOW);
       Serial.println("Linear Guide 2 Stopped");
       return;
      }
    }
   digitalWrite(RELAY7, LOW); // Turn OFF Relay 7
   // Serial.println("RELAY7 OFF"); // Notify Python
   // Serial.println("Motors stopped at Limit Switch 4.");
   Serial.println("Linear Guide2 reached Test Position");
   delay (1000);
  }
 else if (direction2 == "LR") {  // Move Reverse Direction (Linear motor reverse)
   Serial.println("LR");
   if (digitalRead(LIMIT_SWITCH3) == LOW) { // Already at limit
     Serial.println("Already at Limit Switch 3");
     return;
    }
   delay(1000);
   digitalWrite(RELAY7, HIGH); // Turn ON Relay 7
   // Serial.println("RELAY7 ON"); // Notify Python
   digitalWrite(DRIVER2_DIR, LOW); // Set direction reverse
   digitalWrite(DRIVER2_ENA, LOW); // Enable motor

   // 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

   while (digitalRead(LIMIT_SWITCH3) == HIGH) {
     digitalWrite(DRIVER2_STEP, HIGH);
     delayMicroseconds(20);
     digitalWrite(DRIVER2_STEP, LOW);
     delayMicroseconds(25);

     if (checkStopCommand()) {
       digitalWrite(RELAY7, LOW);
       Serial.println("Linear Guide 2 Stopped");
       return;
      }
    }
   // Stop both motors when Limit Switch 3 is triggered or X command is received
   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.");
   Serial.println("Linear Guide2 reached Home Position");
   delay (1000);
  }
  else {
   Serial.println("Invalid direction for moveLinearGuide.");
  }
}
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);
 feederRunning = false; // Reset feeder running flag
 // Turn off relays
 digitalWrite(RELAY1, 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
 // Reset non-blocking auto input state
 awaitingCycleInput = false;
 cycleInputDeadline = 0;
}
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 5
       if (countA >= 4) {
         digitalWrite(RELAY5, LOW); // Stop relay 4
         Serial.print("INDUCTION MOTOR CW STOPPED");
         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
        }
      }
    }
   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
         Serial.print("INDUCTION MOTOR CCW STOPPED");
         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
        }
      }
    }
   lastStateB = sensorStateB; // Update last state for Proxy B
  }
}

// End of USER CODE merged content

/* END CODE */