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