API tools faq
paste
pleasedontcode

Dual-Mode Control rev_01

pleasedontcode
Sep 27th, 2025
86
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
Arduino 39.67 KB | None | 0 0
raw download clone embed print report
  1. /********* Pleasedontcode.com **********
  2.  
  3.     Pleasedontcode thanks you for automatic code generation! Enjoy your code!
  4.  
  5.     - Terms and Conditions:
  6.     You have a non-exclusive, revocable, worldwide, royalty-free license
  7.     for personal and commercial use. Attribution is optional; modifications
  8.     are allowed, but you're responsible for code maintenance. We're not
  9.     liable for any loss or damage. For full terms,
  10.     please visit pleasedontcode.com/termsandconditions.
  11.  
  12.     - Project: Dual-Mode Control
  13.     - Source Code NOT compiled for: Arduino Mega
  14.     - Source Code created on: 2025-09-27 14:44:33
  15.  
  16. ********* Pleasedontcode.com **********/
  17.  
  18. /****** SYSTEM REQUIREMENTS *****/
  19. /****** SYSTEM REQUIREMENT 1 *****/
  20.     /* help refine the code. want to run the code */
  21.     /* smoothly both in auto mode and manual mode. */
  22. /****** END SYSTEM REQUIREMENTS *****/
  23.  
  24.  
  25. /* START CODE */
  26.  
  27. /****** DEFINITION OF LIBRARIES *****/
  28.  
  29. /****** FUNCTION PROTOTYPES *****/
  30. void setup(void);
  31. void loop(void);
  32.  
  33.  // Merged content from USER CODE, adapted to fit the PRELIMINARY CODE structure for Arduino Mega
  34.  
  35. //Last Updated 12092025
  36. // Define stepper motor1 of Linear guide @ Spinning Motor driver pins
  37. const int DRIVER1_DIR = 28; // Direction pin for stepper motor1
  38. const int DRIVER1_STEP = 18; // Step pin for stepper motor1
  39. const int DRIVER1_ENA = 29; // Enable pin for stepper motor1
  40.  
  41. // Define stepper motor2 of Linear guide @ Feeder Motor driver pins
  42. const int DRIVER2_DIR = 30; // Direction pin for stepper motor2
  43. const int DRIVER2_STEP = 6; // Step pin for stepper motor2
  44. const int DRIVER2_ENA = 31; // Enable pin for stepper motor2
  45.  
  46. // Proximity Sensor for Induction Motor CW and CCW
  47. const int PROXY_POS = 10; // Proxy POS Sensor for Linear Motor Position
  48. const int PROXY_A = 11; // Proxy A Sensor for CW
  49. const int PROXY_B = 12; // Proxy B Sensor for CCW
  50. // const int PROXY_RUNFEED = 13; // Proxy Feed Run Sensor
  51.  
  52. // Define BTS7960B motor driver pins for Feeder motor
  53. const int EN_FEEDER = 20; // Enable pin for feeder motor
  54. const int LPWM_FEEDER = 3; // Left PWM pin for feeder motor
  55. const int RPWM_FEEDER = 4; // Right PWM pin for feeder motor
  56.  
  57. // Define Limit switchs of Linear drive @ Spinning motor
  58. const int LIMIT_SWITCH1 = 15; // 32; // Limit switch 1 pin (Bottom)
  59. const int LIMIT_SWITCH2 = 33; // Limit switch 2 pin (Up)
  60. // Define Limit switchs of Linear drive @ Feeder motor
  61. const int LIMIT_SWITCH3 = 35; // Limit switch 3 pin (Reverse)
  62. const int LIMIT_SWITCH4 = 16; // Limit switch 4 pin (Forward)
  63.  
  64. // Define relay pins
  65. const int RELAY1 = A0; // Relay for
  66. const int RELAY2 = A1; // Relay for Air-Nipper Cutter
  67. const int RELAY3 = A12; // Relay for Gripper 1
  68. const int RELAY4 = A3; // Relay for Gripper 2
  69. const int RELAY5 = A4; // Relay for CW direction
  70. const int RELAY6 = A5; // Relay for CCW direction
  71. const int RELAY7 = A6; // Relay for Feeder Linear Drive
  72. const int RELAY8 = A7; // Relay for Label Vaccum pad Forward/Backward
  73. const int RELAY9 = 43; // Relay for Spinning Linear Drive
  74. const int RELAY10 = 45; // Relay for Cutter-up & Cutter-down Cylinder
  75. const int RELAY11 = 46; // Relay for
  76. const int RELAY12 = 47; // Relay for
  77. const int RELAY13 = 48; // Relay for
  78. const int RELAY14 = 49; // Relay for
  79. const int RELAY15 = 50; // Relay for
  80. const int RELAY16 = 51; // Relay for
  81.  
  82. // Flags and variables
  83. bool autoMode = false; // Flag for Auto Mode
  84. bool manualMode = false; // Flag for Manual Mode
  85. int numCycles = 0; // Variable to store number of cycles
  86. bool feederRunning = false; // Flag to indicate if the feeder is running
  87. bool isForward = false;
  88. int targetCounts = 0; // Stores the required counts for current operation
  89. volatile int countA = 0; // Count for Proxy A
  90. volatile int countB = 0; // Count for Proxy B
  91. bool motorRunning = false; // Flag to indicate if the motor is currently running
  92. bool countingEnabled = false; // Flag to enable counting based on relay state
  93. bool isClockwise = false; // Global variable to track motor direction
  94. bool countProxyAEnabled = true; // Flag to enable/disable counting for Proxy A
  95. bool countProxyBEnabled = true; // Flag to enable/disable counting for Proxy B
  96. bool countProxyPOSEnabled = true; // Flag to enable/disable counting for Proxy POS
  97. volatile bool isMoving1 = false; // Flag to indicate if the linear guide is currently moving
  98. volatile bool isMoving2 = false; // Flag to indicate if the linear guide is currently moving
  99. unsigned long feederStartTime = 0;  // When feeder started
  100. unsigned long feederDuration = 0;   // How long to run (ms)
  101. bool linearGuide1Complete = false;
  102. bool linearGuide2Complete = false;
  103. bool feederComplete = false;
  104. bool motorComplete = false;
  105.  
  106. // Command enumeration
  107. 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 };
  108.  
  109. // Debounce variables
  110. const unsigned long DEBOUNCE_TIME = 1000; // 1 second debounce time
  111. unsigned long lastCommandTime = 0; // Variable to store the last command execution time
  112.  
  113. // Auto mode state machine variables
  114. 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 };
  115. AutoState autoState = AUTO_IDLE;
  116. int currentCycle = 0;
  117. unsigned long autoStepStartTime = 0;
  118. const unsigned long STEP_DELAY = 100; // 100ms delay between steps
  119.  
  120. // Completion check functions
  121. bool isLinearGuide1Complete() { return linearGuide1Complete; }
  122. bool isLinearGuide2Complete() { return linearGuide2Complete; }
  123. bool isFeederComplete() { return feederComplete; }
  124. bool isMotorComplete() { return motorComplete; }
  125.  
  126. // Function declarations
  127. void stopMotor(); // To Stop Linear Motor, Feeder Motor, Spinning Motor
  128. void moveLinearGuide1(String direction); // To move Linear UP and Down
  129. void moveLinearGuide2(String direction); // To move Linear Right and Left
  130. void moveStepper1(bool direction, int stopPin); // To Execute Linear & Spinning in sequence
  131. void moveStepper2(bool direction, int stopPin); // To Execute Linear & Feeder in sequence
  132. void runFeeder(bool forward, unsigned long durationMillis = 0, int speed = 25); // To activte Feeder motor
  133. void setFeederMotorSpeed(int speed, bool forward);
  134. void runOpenGripper(); // To deactivate Gripper Relay 3 & 4
  135. void runMotor(bool clockwise); // Combined CW and CCW motor function
  136. void handleCommand(Command cmd); // To handle and execute Manual Input Command
  137. bool runAutoMode(int numCycles); // To execute Auto Input Command sequence
  138. void resetSerialBuffer(); // To reset Command line
  139. bool checkStopCommand(); // To check for STOP command during process
  140. void resetMode();
  141. void CutterSensor();
  142. void proxyA();
  143. void proxyB();
  144.  
  145. // Subsystem wrappers to isolate auto/manual control (new)
  146. void Subsystem_AutoStart(int cycles) { // Initialize and start Auto mode for a fixed cycle count
  147.   numCycles = cycles;
  148.   autoMode = true;
  149.   autoState = AUTO_IDLE;
  150.   currentCycle = 0;
  151.   Serial.print("Subsystem Auto Start: "); Serial.println(cycles);
  152. }
  153. void Subsystem_AutoTick() { updateAutoMode(); }
  154. void Subsystem_AutoStop() { stopAutoMode(); }
  155. void Subsystem_ManualActivate() { manualMode = true; resetMode(); Serial.println("Subsystem Manual Mode Activated"); }
  156. void Subsystem_ManualDeactivate() { manualMode = false; Serial.println("Subsystem Manual Mode Deactivated"); }
  157. bool Subsystem_AutoRunning() { return autoMode; }
  158.  
  159. Command parseCommand(String command);
  160. Command currentMode = NONE;
  161. // Helper functions to set completion flags
  162. void setLinearGuide1Complete() {
  163.  linearGuide1Complete = true;
  164.  Serial.println("Linear Guide 1 operation complete");
  165. }
  166. void setLinearGuide2Complete() {
  167.  linearGuide2Complete = true;
  168.  Serial.println("Linear Guide 2 operation complete");
  169. }
  170. void setFeederComplete() {
  171.  feederComplete = true;
  172.  Serial.println("Feeder operation complete");
  173. }
  174. void setMotorComplete() {
  175.  motorComplete = true;
  176.  Serial.println("Motor operation complete");
  177. }
  178. void setup() {
  179.  // Set pin modes for motors and relays
  180.  pinMode(RPWM_FEEDER, OUTPUT);
  181.  pinMode(LPWM_FEEDER, OUTPUT);
  182.  pinMode(EN_FEEDER, OUTPUT);
  183.  pinMode(PROXY_POS, INPUT_PULLUP); // Set proxy POS as input
  184.  pinMode(PROXY_A, INPUT_PULLUP); // Set proxy A as input
  185.  pinMode(PROXY_B, INPUT_PULLUP); // Set proxy B as input
  186.  // pinMode(PROXY_RUNFEED, INPUT); // Set proxy Feed Run as input
  187.  pinMode(RELAY1, OUTPUT);
  188.  pinMode(RELAY2, OUTPUT);
  189.  pinMode(RELAY3, OUTPUT);
  190.  pinMode(RELAY4, OUTPUT);
  191.  pinMode(RELAY5, OUTPUT);
  192.  pinMode(RELAY6, OUTPUT);
  193.  pinMode(RELAY7, OUTPUT);
  194.  pinMode(RELAY8, OUTPUT);
  195.  pinMode(RELAY9, OUTPUT);
  196.  pinMode(RELAY10, OUTPUT);
  197.  pinMode(RELAY11, OUTPUT);
  198.  pinMode(RELAY12, OUTPUT);
  199.  pinMode(RELAY13, OUTPUT);
  200.  pinMode(RELAY14, OUTPUT);
  201.  pinMode(RELAY15, OUTPUT);
  202.  pinMode(RELAY16, OUTPUT);
  203.  pinMode(LIMIT_SWITCH1, INPUT_PULLUP);
  204.  pinMode(LIMIT_SWITCH2, INPUT_PULLUP);
  205.  pinMode(LIMIT_SWITCH3, INPUT_PULLUP);
  206.  pinMode(LIMIT_SWITCH4, INPUT_PULLUP);
  207.  pinMode(DRIVER1_STEP, OUTPUT);
  208.  pinMode(DRIVER1_DIR, OUTPUT);
  209.  pinMode(DRIVER1_ENA, OUTPUT);
  210.  pinMode(DRIVER2_STEP, OUTPUT);
  211.  pinMode(DRIVER2_DIR, OUTPUT);
  212.  pinMode(DRIVER2_ENA, OUTPUT);
  213.  
  214.  // Initialize relays to LOW (off state)
  215.  digitalWrite(RELAY1, LOW);
  216.  digitalWrite(RELAY2, LOW);
  217.  digitalWrite(RELAY3, LOW);
  218.  digitalWrite(RELAY4, LOW);
  219.  digitalWrite(RELAY5, LOW);
  220.  digitalWrite(RELAY6, LOW);
  221.  digitalWrite(RELAY7, LOW);
  222.  digitalWrite(RELAY8, LOW);
  223.  digitalWrite(RELAY9, LOW);
  224.  digitalWrite(RELAY10, LOW);
  225.  digitalWrite(RELAY11, LOW);
  226.  digitalWrite(RELAY12, LOW);
  227.  digitalWrite(RELAY13, LOW);
  228.  digitalWrite(RELAY14, LOW);
  229.  digitalWrite(RELAY15, LOW);
  230.  digitalWrite(RELAY16, LOW);
  231.  
  232.  digitalWrite(DRIVER1_ENA, LOW);  // Enable stepper motor1
  233.  digitalWrite(DRIVER2_ENA, LOW);  // Enable stepper motor2
  234.  
  235.  Serial.begin(9600);  // Start serial communication
  236.  resetSerialBuffer();  // Clear serial buffer on startup
  237. }
  238. Command parseCommand(String command) {
  239.  command.trim();
  240.  // Parse incoming command strings and return corresponding Command enum
  241.  if (command == "START") return START;
  242.  if (command == "STOP") return STOP;
  243.  if (command == "G1") return G1;
  244.  if (command == "G2") return G2;
  245.  if (command == "G3") return G3;
  246.  if (command == "G4") return G4;
  247.  if (command == "VC") return VC;
  248.  if (command == "VO") return VO;
  249.  if (command == "VF") return VF;
  250.  if (command == "VR") return VR;
  251.  if (command == "PU") return PU;
  252.  if (command == "PD") return PD;
  253.  if (command == "OG") return OG;
  254.  if (command == "CW") return CW;
  255.  if (command == "CCW") return CCW;
  256.  if (command == "LU") return LU;
  257.  if (command == "LD") return LD;
  258.  if (command == "LF") return LF;
  259.  if (command == "LR") return LR;
  260.  if (command == "CU") return CU;
  261.  if (command == "CD") return CD;
  262.  if (command == "CX") return CX;
  263.  if (command == "X") return X;
  264.  if (command == "FD") return FD; // Forward command
  265.  if (command == "RD") return RD; // Reverse command
  266.  if (command == "AUTO") return AUTO; // Auto mode
  267.  if (command == "MANUAL") return MANUAL; // Manual mode
  268.  return UNKNOWN; // Return UNKNOWN if command is not recognized
  269. }
  270. void handleCommand(Command cmd) {
  271.  // Handle commands based on the parsed Command enum
  272.  switch (cmd) {
  273.    case START:
  274.      Serial.println("Process started.");
  275.    break;
  276.    case STOP:
  277.      stopMotor();
  278.      Serial.println("MOTORS STOPPED");
  279.    break;
  280.    case G1:
  281.      digitalWrite(RELAY3, HIGH); // Activate RELAY3 for G1 Close
  282.      Serial.println("RELAY3 ON");
  283.    break;
  284.    case G2:
  285.      digitalWrite(RELAY3, LOW); // Deactivate RELAY3 for G2 Open
  286.      Serial.println("RELAY3 OFF");
  287.    break;
  288.    case G3:
  289.      digitalWrite(RELAY4, HIGH); // Activate RELAY4 for G2 Close
  290.      Serial.println("RELAY4 ON");
  291.    break;
  292.    case G4:
  293.      digitalWrite(RELAY4, LOW); // Deactivate RELAY4 for G2 Open
  294.      Serial.println("RELAY4 OFF");
  295.    break;
  296.    case VC: // Removed as on 28082025
  297.      digitalWrite(RELAY11, HIGH); // Activate RELAY10 for Vaccum pad Pick
  298.      Serial.println("RELAY11 ON");
  299.    break;
  300.    case VO: // Removed as on 28082025
  301.      digitalWrite(RELAY11, LOW); // Deactivate RELAY10 for Vaccum pad Eject
  302.      Serial.println("RELAY11 OFF");
  303.    break;
  304.    case VF:
  305.      digitalWrite(RELAY8, HIGH); // Activate RELAY8 for Vaccum pad Forward
  306.      Serial.println("RELAY8 ON");
  307.    break;
  308.    case VR:
  309.      digitalWrite(RELAY8, LOW); // Deactivate RELAY8 for Vaccum pad Reverse
  310.      Serial.println("RELAY8 OFF");
  311.    break;
  312.    case PU:
  313.      digitalWrite(RELAY11, HIGH); // Activate RELAY11 for Push Up Load
  314.      Serial.println("RELAY11 ON");
  315.    break;
  316.    case PD:
  317.      digitalWrite(RELAY11, LOW); // Deactivate RELAY11 for Push Down Load
  318.      Serial.println("RELAY11 OFF");
  319.    break;
  320.    case OG:
  321.      runOpenGripper();
  322.      Serial.println("Gripper 1 & 2 OPEN...");
  323.    break;
  324.    case CW:
  325.      runMotor(true);
  326.      Serial.println("Starting CW motor...");
  327.    break;
  328.    case CCW:
  329.      runMotor(false);
  330.      Serial.println("Starting CCW motor...");
  331.    break;
  332.    case LU:
  333.      moveLinearGuide1("LU");
  334.      Serial.println("Linear Guide1 Moving Up");
  335.    break;
  336.    case LD:
  337.      moveLinearGuide1("LD");
  338.      Serial.println("Linear Guide1 Moving Down");
  339.    break;
  340.    case LF:
  341.      moveLinearGuide2("LF");
  342.      Serial.println("Linear Guide2 Moving Forward");
  343.    break;
  344.    case LR:
  345.      moveLinearGuide2("LR");
  346.      Serial.println("Linear Guide2 Moving Reverse");
  347.    break;
  348.    case CX:
  349.      digitalWrite(RELAY2, HIGH);
  350.      delay(1000);
  351.      digitalWrite(RELAY2, LOW);
  352.      Serial.println("Air-Nipper Cutter Operated.");
  353.    break;
  354.    case CU:
  355.      digitalWrite(RELAY10, HIGH); // Activate RELAY10 for Cutter Up Load
  356.      Serial.println("RELAY10 ON");
  357.    break;
  358.    case CD:
  359.      digitalWrite(RELAY10, LOW); // Deactivate RELAY10 for Cutter Down Load
  360.      Serial.println("RELAY10 OFF");
  361.    break;
  362.    case X:
  363.      stopMotor();
  364.      Serial.println(" All motors stopped.");
  365.    break;
  366.    case FD:
  367.      runFeeder(true, 2150); // Move feeder forward
  368.      Serial.println("Feeder moving forward.");
  369.    break;
  370.    case RD:
  371.      runFeeder(false, 2150); // Move feeder reverse
  372.      Serial.println("Feeder moving reverse.");
  373.    break;
  374.    case AUTO:
  375.    if (!autoMode) {  // Only prompt if not already in Auto mode
  376.      resetMode();  // Reset mode before starting Auto Mode
  377.      autoMode = true;  // Set Auto mode flag
  378.      autoState = AUTO_IDLE;
  379.      currentCycle = 0;
  380.      Serial.println("Auto mode activated.");
  381.      // delay(2000);
  382.      Serial.println("Please enter the number of cycles:");
  383.  
  384.      unsigned long startWaitTime = millis();
  385.      while (millis() - startWaitTime < 10000) {
  386.        if (Serial.available() > 0) {
  387.          numCycles = Serial.parseInt();
  388.          if (numCycles > 0) {
  389.            Serial.print("Starting Auto Mode for " );
  390.            Serial.print(numCycles);
  391.            Serial.println(" cycles.");
  392.            break;
  393.           } else {
  394.            Serial.println("Invalid input. Please entre a positive number:");
  395.           }
  396.         }
  397.       }
  398.       if (numCycles == 0) {
  399.        Serial.println("Timeout! Auto mode cancelled.");
  400.        autoMode = false;        
  401.       }
  402.     }
  403.    break;
  404.    case MANUAL:
  405.    if (!manualMode) {  // Only prompt if not already in Manual mode
  406.      resetMode();  // Reset mode before starting Manual Mode
  407.      manualMode = true;  // Set Manual mode flag
  408.      Serial.println("Manual mode activated.");
  409.     }
  410.    break;
  411.    default:
  412.      Serial.println("UNKNOWN COMMAND");
  413.    break;
  414.   }
  415. }
  416. void loop() {
  417.  // Process serial commands non-blockingly
  418.  if (Serial.available() > 0) {
  419.    String command = Serial.readStringUntil('\n');
  420.    command.trim(); // Remove any trailing spaces or newlines
  421.    Serial.flush();   // Clear buffer after reading
  422.    Serial.println("Received: " + command);  // Debugging
  423.      
  424.    // Check if enough time has passed since the last command
  425.    if (millis() - lastCommandTime >= DEBOUNCE_TIME) {
  426.      Command cmd = parseCommand(command);
  427.      lastCommandTime = millis();
  428.  
  429.      if (cmd == STOP || cmd == X) {
  430.        // Handle stop command immediately
  431.        if (autoMode) {
  432.          stopAutoMode();
  433.         }
  434.        handleCommand(cmd);
  435.       } else if (cmd == AUTO) {
  436.         handleCommand(cmd);
  437.       } else if (cmd == MANUAL) {
  438.         handleCommand(cmd);
  439.       } else {
  440.        // Handle other commands (START, STOP, Q, etc.)
  441.         handleCommand(cmd);
  442.       }
  443.       // Clear serial buffer after mode changes or commands
  444.       resetSerialBuffer();
  445.     }
  446.   }
  447.  // If in Auto mode, run the auto sequence incrementally
  448.  if (autoMode) {
  449.    updateAutoMode();
  450.   }
  451.   // State monitoring (no more proxy function calls)
  452.  updateFeeder();
  453.  
  454.  if (motorRunning) {
  455.    if (isClockwise) { // Check if the motor is running CW
  456.      proxyA(); // Check Proxy A
  457.     }
  458.    else { // Otherwise, it must be CCW
  459.      proxyB(); // Check Proxy B
  460.     }
  461.   }
  462. }
  463. void updateAutoMode() {
  464.  static unsigned long lastUpdateTime = 0;
  465.  static bool firstTimeInState = true;
  466.  unsigned long currentTime = millis();
  467.  
  468.  // Update at 100ms intervals for responsiveness
  469.  if (currentTime - lastUpdateTime < 100) {
  470.    return;
  471.   }
  472.  lastUpdateTime = currentTime;
  473.  
  474.  // Check for stop commands continuously
  475.  if (checkStopCommand()) {
  476.    stopAutoMode();
  477.    return;
  478.  }
  479.  switch (autoState) {
  480.    case AUTO_IDLE:
  481.      if (firstTimeInState) {
  482.        Serial.print("AUTO_IDLE - Starting cycle ");
  483.        Serial.println(currentCycle + 1);
  484.        firstTimeInState = false;
  485.        autoStepStartTime = currentTime;
  486.       }
  487.      if (currentTime - autoStepStartTime > 500) {
  488.        if (currentCycle < numCycles) {
  489.          currentCycle++;
  490.          Serial.print("Starting cycle ");
  491.          Serial.println(currentCycle);
  492.          autoState = AUTO_CHECK_POS;
  493.          firstTimeInState = true;
  494.          autoStepStartTime = currentTime;      
  495.         } else {
  496.          autoState = AUTO_COMPLETE;
  497.         }
  498.       }
  499.    break;
  500.    case AUTO_CHECK_POS:
  501.      if (firstTimeInState) {
  502.        int sensorState = digitalRead(PROXY_POS);
  503.        Serial.print("Proxy POS Sensor State: ");
  504.        Serial.println(sensorState);
  505.        
  506.        if (sensorState == LOW) {
  507.          Serial.println("Proxy POS not active! Cannot proceed.");
  508.          autoState = AUTO_STOPPED;
  509.         } else {
  510.          Serial.println("Proxy POS active. Proceeding to LU...");
  511.          autoState = AUTO_LU;
  512.          firstTimeInState = true;
  513.         }
  514.        autoStepStartTime = currentTime;
  515.       }
  516.    break;
  517.    case AUTO_LU:
  518.      if (firstTimeInState) {
  519.        linearGuide1Complete = false;
  520.        moveLinearGuide1("LU");
  521.        Serial.println("Executing LU command...");
  522.        firstTimeInState = false;
  523.        autoStepStartTime = currentTime;
  524.       }
  525.      if (isLinearGuide1Complete()) {
  526.        autoState = AUTO_LF;
  527.        firstTimeInState = true;
  528.        autoStepStartTime = currentTime;
  529.        break;
  530.       }
  531.      // Timeout safety
  532.      if (currentTime - autoStepStartTime > 15000) {
  533.        Serial.println("LU operation timeout!");
  534.        autoState = AUTO_STOPPED;
  535.       }
  536.    break;
  537.    case AUTO_LF:
  538.      if (firstTimeInState) {
  539.        linearGuide2Complete = false;
  540.        moveLinearGuide2("LF");
  541.        Serial.println("Executing LF command...");
  542.        firstTimeInState = false;
  543.        autoStepStartTime = currentTime;
  544.        delay(1000);
  545.       }
  546.      if (isLinearGuide2Complete()) {
  547.        autoState = AUTO_VF;
  548.        firstTimeInState = true;
  549.        autoStepStartTime = currentTime;
  550.        break;
  551.       }
  552.      if (currentTime - autoStepStartTime > 15000) {
  553.        Serial.println("LF operation timeout!");
  554.        autoState = AUTO_STOPPED;
  555.       }
  556.    break;
  557.    case AUTO_VF:
  558.      digitalWrite(RELAY8, HIGH);
  559.      Serial.println("Executing VF command...");
  560.      autoState = AUTO_FD;
  561.      autoStepStartTime = currentTime;
  562.      delay(1000);
  563.    break;
  564.    case AUTO_FD:
  565.      if (firstTimeInState) {
  566.        feederComplete = false;
  567.        runFeeder(true, 2150);
  568.        Serial.println("Executing FD command...");
  569.        firstTimeInState = false;
  570.        autoStepStartTime = currentTime;
  571.       }
  572.      if (isFeederComplete()) {
  573.        autoState = AUTO_G3;
  574.        firstTimeInState = true;
  575.        autoStepStartTime = currentTime;
  576.        break;
  577.       }
  578.      if (currentTime - autoStepStartTime > 5000) {
  579.        Serial.println("FD operation timeout!");
  580.        autoState = AUTO_STOPPED;
  581.       }
  582.    break;
  583.    case AUTO_G3:
  584.      digitalWrite(RELAY4, HIGH);
  585.      Serial.println("Executing G3 command...");
  586.      autoState = AUTO_LR;
  587.      autoStepStartTime = currentTime;
  588.    break;
  589.    case AUTO_LR:
  590.      if (firstTimeInState) {
  591.        linearGuide2Complete = false;
  592.        moveLinearGuide2("LR");
  593.        Serial.println("Executing LR command...");
  594.        firstTimeInState = false;
  595.        autoStepStartTime = currentTime;
  596.       }
  597.      if (isLinearGuide2Complete()) {
  598.        autoState = AUTO_G1;
  599.        firstTimeInState = true;
  600.        autoStepStartTime = currentTime;
  601.        break;
  602.       }
  603.      if (currentTime - autoStepStartTime > 15000) {
  604.        Serial.println("LR operation timeout!");
  605.        autoState = AUTO_STOPPED;
  606.       }
  607.    break;
  608.    case AUTO_G1:
  609.      digitalWrite(RELAY3, HIGH);
  610.      Serial.println("Executing G1 command...");
  611.      autoState = AUTO_CU;
  612.      autoStepStartTime = currentTime;
  613.      delay(1000);
  614.    break;
  615.    case AUTO_CU:
  616.      digitalWrite(RELAY10, HIGH);
  617.      Serial.println("Executing CU command...");
  618.      autoState = AUTO_CX;
  619.      autoStepStartTime = currentTime;
  620.      delay(1000);
  621.    break;
  622.    case AUTO_CX:
  623.      digitalWrite(RELAY2, HIGH);
  624.      Serial.println("Executing CX command...");
  625.      autoState = AUTO_CD;
  626.      autoStepStartTime = currentTime;
  627.      delay(1000);
  628.    break;
  629.    case AUTO_CD:
  630.      digitalWrite(RELAY2, LOW);
  631.      digitalWrite(RELAY10, LOW);
  632.      Serial.println("Executing CD command...");
  633.      autoState = AUTO_LD;
  634.      autoStepStartTime = currentTime;
  635.      delay(1000);
  636.    break;
  637.    case AUTO_LD:
  638.      if (firstTimeInState) {
  639.        linearGuide1Complete = false;
  640.        moveLinearGuide1("LD");
  641.        Serial.println("Executing LD command...");
  642.        firstTimeInState = false;
  643.        autoStepStartTime = currentTime;
  644.       }
  645.      if (isLinearGuide1Complete()) {
  646.        autoState = AUTO_CW;
  647.        firstTimeInState = true;
  648.        autoStepStartTime = currentTime;
  649.        break;
  650.       }
  651.      if (currentTime - autoStepStartTime > 15000) {
  652.        Serial.println("LD operation timeout!");
  653.        autoState = AUTO_STOPPED;
  654.       }
  655.    break;
  656.    case AUTO_CW:
  657.      if (firstTimeInState) {
  658.        motorComplete = false;
  659.        runMotor(true);
  660.        Serial.println("Executing CW command...");
  661.        firstTimeInState = false;
  662.        autoStepStartTime = currentTime;
  663.        autoState = AUTO_CW_RUNNING;  // Move to intermediate state
  664.       }
  665.    break;
  666.    case AUTO_CW_RUNNING:
  667.      // Check if motor has completed its operation
  668.      if (isMotorComplete()) {
  669.        Serial.println("CW operation completed.");
  670.        autoState = AUTO_VR;  // Move to next main state
  671.        firstTimeInState = true;
  672.       }
  673.      // Timeout safety
  674.      if (currentTime - autoStepStartTime > 30000) {
  675.        Serial.println("CW operation timeout!");
  676.        stopMotor();
  677.        autoState = AUTO_STOPPED;
  678.       }
  679.    break;
  680.    case AUTO_VR:
  681.      digitalWrite(RELAY8, LOW);
  682.      Serial.println("Executing VR command...");
  683.      autoState = AUTO_OG;
  684.      autoStepStartTime = currentTime;
  685.    break;
  686.    case AUTO_OG:
  687.      if (firstTimeInState) {
  688.        runOpenGripper();
  689.        Serial.println("Executing OG command...");
  690.        firstTimeInState = false;
  691.        autoStepStartTime = currentTime;
  692.       }
  693.      // Wait 3000ms for OG delay
  694.      if (currentTime - autoStepStartTime >= 3000) {
  695.        autoState = AUTO_CCW;
  696.        firstTimeInState = true;
  697.        autoStepStartTime = currentTime;
  698.       }
  699.    break;
  700.    case AUTO_CCW:
  701.      if (firstTimeInState) {
  702.        motorComplete = false;
  703.        runMotor(false); // Start CCW motor
  704.        Serial.println("Executing CCW command...");
  705.        firstTimeInState = false;
  706.        autoStepStartTime = currentTime;
  707.        autoState = AUTO_CCW_RUNNING;  // Move to intermediate state
  708.       }
  709.    break;
  710.    case AUTO_CCW_RUNNING:
  711.      // Check if motor has completed its operation
  712.      if (isMotorComplete()) {
  713.        Serial.println("CCW operation completed.");
  714.        // Move to whatever state comes after CCW, or back to IDLE
  715.        autoState = AUTO_IDLE;
  716.        firstTimeInState = true;
  717.       }
  718.      // Timeout safety
  719.      if (currentTime - autoStepStartTime > 30000) {
  720.        Serial.println("CCW operation timeout!");
  721.        stopMotor();
  722.        autoState = AUTO_STOPPED;
  723.       }
  724.    break;
  725.    case AUTO_COMPLETE:
  726.      Serial.println("Auto mode completed all cycles.");
  727.      resetMode();
  728.      autoMode = false;
  729.    break;
  730.    case AUTO_STOPPED:
  731.      // Do nothing until reset
  732.      default:
  733.    break;
  734.   }
  735. }
  736. void stopAutoMode() {
  737.  // Stop all ongoing operations
  738.  stopMotor();
  739.  digitalWrite(RELAY2, LOW);
  740.  digitalWrite(RELAY3, LOW);
  741.  digitalWrite(RELAY4, LOW);
  742.  digitalWrite(RELAY8, LOW);
  743.  digitalWrite(RELAY9, LOW);
  744.  digitalWrite(RELAY10, LOW);
  745.  digitalWrite(DRIVER1_ENA, HIGH);
  746.  digitalWrite(DRIVER2_ENA, HIGH);
  747.  digitalWrite(EN_FEEDER, LOW);
  748.  
  749.  autoState = AUTO_STOPPED;
  750.  Serial.println("Auto mode stopped by command.");
  751. }
  752. bool checkStopCommand() {
  753.  while (Serial.available() > 0) {
  754.    String command = Serial.readStringUntil('\n');
  755.    command.trim();
  756.    
  757.    if (command == "STOP" || command == "X") {
  758.      Serial.println("Stop command received: " + command);
  759.      return true;
  760.     }
  761.    // Put the command back into the buffer for normal processing
  762.    // for (int i = 0; i < command.length(); i++) {
  763.    //   Serial.print(command[i]);
  764.    // }
  765.   }
  766.  return false;
  767. }
  768. void setFeederMotorSpeed(int speed, bool forward) {
  769.  analogWrite(RPWM_FEEDER, forward ? map(speed, 0, 100, 0, 255) : 0);
  770.  analogWrite(LPWM_FEEDER, forward ? 0 : map(speed, 0, 100, 0, 255));
  771. }
  772. void runFeeder(bool forward, unsigned long durationMillis, int speed) {
  773.  feederComplete = false;
  774.  if (feederRunning) {
  775.    Serial.println("Feeder is already running.");
  776.    return; // Return the current direction if already running
  777.   }
  778.  // Start the feeder motor
  779.  setFeederMotorSpeed(speed, forward);
  780.  digitalWrite(EN_FEEDER, HIGH);  // Enable feeder motor
  781.  delay (100);
  782.  feederRunning = true; // set feeder running flag to true
  783.  isForward = forward;
  784.  // Set duration if provided (0 means run indefinitely)
  785.  if (durationMillis > 0) {
  786.    feederStartTime = millis();
  787.    feederDuration = durationMillis;
  788.   } else {
  789.    feederDuration = 0; // Run until manually stopped
  790.   }
  791.  Serial.print(forward ? "FEEDER MOTOR FD STARTED" : "FEEDER MOTOR RD STARTED");
  792.  if (durationMillis > 0) {
  793.    Serial.print(" for ");
  794.    Serial.print(durationMillis / 1000.0);
  795.    Serial.println(" seconds");
  796.   } else {
  797.    Serial.println(" (indefinite run)");
  798.   }
  799. }
  800. void updateFeeder() {
  801.  if (feederRunning && feederDuration > 0 && (millis() - feederStartTime >= feederDuration)) {
  802.    // Time's up - stop the motor
  803.    digitalWrite(EN_FEEDER, LOW);
  804.    stopMotor();
  805.    feederRunning = false;
  806.    Serial.println("Feeder motor stopped (time elapsed)");
  807.    // SET COMPLETION FLAG HERE
  808.    setFeederComplete();
  809.   }
  810. }
  811.  
  812. void moveLinearGuide1(String direction1) {
  813.  linearGuide1Complete = false;
  814.  if (direction1 == "LU") {  // Forward Direction (Linear motor up)
  815.    int sensorState = digitalRead(PROXY_POS);
  816.    Serial.print("Proxy POS Sensor State: ");
  817.    Serial.println(sensorState); // Print the actual state of the sensor
  818.  
  819.    // Check if proxyPOS is active
  820.    if (sensorState == LOW) { // Assuming HIGH means the sensor is inactive
  821.      Serial.println("Cannot move up. Proxy POS is not active.");
  822.      return; // Exit the function if proxyPOS is not active
  823.     }
  824.     // Check if already at upper limit (like LF/LR does)
  825.    if (digitalRead(LIMIT_SWITCH2) == LOW) {
  826.      Serial.println("Already at Limit Switch 2");
  827.      setLinearGuide1Complete(); // ADD THIS - was missing!
  828.      return;
  829.     }
  830.    Serial.println("LU");
  831.    digitalWrite(DRIVER1_ENA, HIGH);
  832.    delay(100);
  833.    digitalWrite(RELAY9, HIGH); // Turn ON Relay 1
  834.    Serial.println("RELAY9 ON"); // Notify Python
  835.    delay(100);
  836.    digitalWrite(DRIVER1_DIR, HIGH); // Set direction forward
  837.    delay(100);
  838.    digitalWrite(DRIVER1_ENA, LOW); // Enable motor
  839.    delay(200);
  840.  
  841.    Serial.print("Limit States - 1: ");
  842.    Serial.print(digitalRead(LIMIT_SWITCH1));
  843.    Serial.print("  & Limit States - 2: ");
  844.    Serial.println(digitalRead(LIMIT_SWITCH2));
  845.  
  846.    // Direct stepper movement (similar to LF/LR)
  847.    unsigned long startTime = millis();
  848.    const unsigned long timeout = 15000; // 15 second timeout
  849.    
  850.    while (digitalRead(LIMIT_SWITCH2) == HIGH) {
  851.      digitalWrite(DRIVER1_STEP, HIGH);
  852.      delayMicroseconds(40);
  853.      digitalWrite(DRIVER1_STEP, LOW);
  854.      delayMicroseconds(45);
  855.      
  856.      // Check for timeout
  857.      if (millis() - startTime > timeout) {
  858.        Serial.println("LU movement timeout! Stopping.");
  859.        break;
  860.       }
  861.      if (checkStopCommand()) {
  862.        Serial.println("Emergency Stop Triggered");
  863.        stopMotor();
  864.        digitalWrite(RELAY9, LOW);
  865.        return;
  866.       }
  867.     }
  868.    digitalWrite(RELAY9, LOW);
  869.    Serial.println("RELAY9 OFF");
  870.    Serial.println("Motors stopped at Limit Switch 2 or Emergency Stop.");
  871.    setLinearGuide1Complete();
  872.   }
  873.  else if (direction1 == "LD") {  // Linear motor down
  874.    // Check if already at lower limit
  875.    if (digitalRead(LIMIT_SWITCH1) == LOW) {
  876.      Serial.println("Already at Limit Switch 1");
  877.      setLinearGuide1Complete();
  878.      return;
  879.     }
  880.    Serial.println("LD");
  881.    digitalWrite(DRIVER1_ENA, HIGH);
  882.    delay(100);
  883.    digitalWrite(RELAY9, HIGH);
  884.    Serial.println("RELAY9 ON");
  885.    delay(100);
  886.    digitalWrite(DRIVER1_DIR, LOW); // Set direction for DOWN movement
  887.    delay(100);
  888.    digitalWrite(DRIVER1_ENA, LOW);
  889.    delay(200);
  890.  
  891.    Serial.print("Limit States - 1: ");
  892.    Serial.print(digitalRead(LIMIT_SWITCH1));
  893.    Serial.print("  & Limit States - 2: ");
  894.    Serial.println(digitalRead(LIMIT_SWITCH2));
  895.    
  896.    // Direct stepper movement (similar to LF/LR)
  897.    unsigned long startTime = millis();
  898.    const unsigned long timeout = 15000; // 15 second timeout
  899.    
  900.    while (digitalRead(LIMIT_SWITCH1) == HIGH) {
  901.      digitalWrite(DRIVER1_STEP, HIGH);
  902.      delayMicroseconds(40);
  903.      digitalWrite(DRIVER1_STEP, LOW);
  904.      delayMicroseconds(45);
  905.      
  906.      // Check for timeout
  907.      if (millis() - startTime > timeout) {
  908.        Serial.println("LD movement timeout! Stopping.");
  909.        break;
  910.       }
  911.      if (checkStopCommand()) {
  912.        Serial.println("Emergency Stop Triggered");
  913.        stopMotor();
  914.        digitalWrite(RELAY9, LOW);
  915.        return;
  916.       }
  917.     }
  918.    digitalWrite(RELAY9, LOW);
  919.    Serial.println("RELAY9 OFF");
  920.    Serial.println("Motors stopped at Limit Switch 1 or Emergency Stop.");
  921.    delay(100);
  922.    setLinearGuide1Complete();
  923.   }
  924.  else {
  925.    Serial.println("Invalid direction for moveLinearGuide.");
  926.   }
  927. }
  928. void moveLinearGuide2(String direction2) {
  929.  linearGuide2Complete = false;
  930.  if (direction2 == "LF") {  // Linear motor forward
  931.    if (digitalRead(LIMIT_SWITCH4) == LOW) {
  932.      Serial.println("Already at Limit Switch 4");
  933.      setLinearGuide2Complete();
  934.      return;
  935.     }
  936.    Serial.println("LF");
  937.    digitalWrite(DRIVER2_ENA, HIGH);
  938.    delay(100);
  939.    digitalWrite(RELAY7, HIGH); // Turn ON Relay 7
  940.    Serial.println("RELAY7 ON"); // Notify Python
  941.    delay(100);
  942.    digitalWrite(DRIVER2_DIR, HIGH); // Set direction forward
  943.    delay(100);
  944.    digitalWrite(DRIVER2_ENA, LOW); // Enable motor
  945.    delay(100);
  946.    
  947.    Serial.print("Limit States - 3: ");
  948.    Serial.print(digitalRead(LIMIT_SWITCH3));
  949.    Serial.print("  & Limit States - 4: ");
  950.    Serial.println(digitalRead(LIMIT_SWITCH4));
  951.  
  952.    // Direct stepper movement (similar to LF/LR)
  953.    unsigned long startTime = millis();
  954.    const unsigned long timeout = 15000; // 15 second timeout
  955.  
  956.    while (digitalRead(LIMIT_SWITCH4) == HIGH) {
  957.      digitalWrite(DRIVER2_STEP, HIGH);
  958.      delayMicroseconds(20);
  959.      digitalWrite(DRIVER2_STEP, LOW);
  960.      delayMicroseconds(25);
  961.      
  962.      // Check for timeout
  963.      if (millis() - startTime > timeout) {
  964.        Serial.println("LF movement timeout! Stopping.");
  965.        break;
  966.       }
  967.      if (checkStopCommand()) {
  968.        Serial.println("Emergency Stop Triggered");
  969.        stopMotor();
  970.        digitalWrite(RELAY7, LOW);
  971.        digitalWrite(DRIVER2_ENA, HIGH);
  972.        return;
  973.       }
  974.     }
  975.    digitalWrite(DRIVER2_ENA, HIGH);
  976.    delay(100);
  977.    digitalWrite(RELAY7, LOW); // Turn OFF Relay 7
  978.    Serial.println("RELAY7 OFF"); // Notify Python
  979.    Serial.println("Motors stopped at Limit Switch 4 or Emergency Stop.");
  980.    delay (100);
  981.    // SET COMPLETION FLAG HERE
  982.    setLinearGuide2Complete();
  983.   }
  984.  else if (direction2 == "LR") {  // Move Reverse Direction (Linear motor reverse)
  985.   // Check if already at lower limit
  986.    if (digitalRead(LIMIT_SWITCH3) == LOW) { // Already at limit
  987.      Serial.println("Already at Limit Switch 3");
  988.      setLinearGuide2Complete();
  989.      return;
  990.     }
  991.    Serial.println("LR");
  992.    digitalWrite(DRIVER2_ENA, HIGH);
  993.    delay(100);
  994.    digitalWrite(RELAY7, HIGH); // Turn ON Relay 7
  995.    Serial.println("RELAY7 ON"); // Notify Python
  996.    delay(100);
  997.    digitalWrite(DRIVER2_DIR, LOW); // Set direction reverse
  998.    delay(100);
  999.    digitalWrite(DRIVER2_ENA, LOW); // Enable motor
  1000.    delay(100);
  1001.    
  1002.    Serial.print("Limit States - 3: ");
  1003.    Serial.print(digitalRead(LIMIT_SWITCH3));
  1004.    Serial.print("  & Limit States - 4: ");
  1005.    Serial.println(digitalRead(LIMIT_SWITCH4));
  1006.      
  1007.    // Run BTS7960B motor forward at 5 speed
  1008.    digitalWrite(EN_FEEDER, HIGH); // Enable feeder motor
  1009.    setFeederMotorSpeed(35, true); // Use the helper function to set speed and direction
  1010.  
  1011.    // Direct stepper movement (similar to LF/LR)
  1012.    unsigned long startTime = millis();
  1013.    const unsigned long timeout = 15000; // 15 second timeout
  1014.    
  1015.    while (digitalRead(LIMIT_SWITCH3) == HIGH) {
  1016.      digitalWrite(DRIVER2_STEP, HIGH);
  1017.      delayMicroseconds(20);
  1018.      digitalWrite(DRIVER2_STEP, LOW);
  1019.      delayMicroseconds(25);
  1020.  
  1021.      // Check for timeout
  1022.      if (millis() - startTime > timeout) {
  1023.        Serial.println("LD movement timeout! Stopping.");
  1024.        break;
  1025.       }
  1026.      
  1027.      if (checkStopCommand()) {
  1028.        Serial.println("Emergency Stop Triggered");
  1029.        stopMotor();
  1030.        digitalWrite(RELAY7, LOW);
  1031.        digitalWrite(DRIVER2_ENA, HIGH);
  1032.        return;
  1033.       }
  1034.     }
  1035.    // Stop both motors when Limit Switch 3 is triggered or X command is received
  1036.    digitalWrite(DRIVER2_ENA, HIGH);
  1037.    delay(100);
  1038.    digitalWrite(RELAY7, LOW); // Turn OFF Relay 7
  1039.    digitalWrite(EN_FEEDER, LOW);
  1040.    Serial.println("RELAY7 OFF"); // Notify Python
  1041.    Serial.println("Motors stopped at Limit Switch 3 or Emergency Stop.");
  1042.    delay (100);
  1043.    // SET COMPLETION FLAG HERE
  1044.    setLinearGuide2Complete();
  1045.   }
  1046.   else {
  1047.    Serial.println("Invalid direction for moveLinearGuide.");
  1048.   }
  1049. }
  1050. void moveStepper1(bool direction, int stopPin) {
  1051.  digitalWrite(DRIVER1_DIR, direction);
  1052.  digitalWrite(RELAY9, HIGH);
  1053.  digitalWrite(DRIVER1_ENA, LOW); // Enable stepper motor
  1054.  
  1055.  // Move stepper motor until limit switch is triggered or "X" command is received
  1056.  while (digitalRead(stopPin) == HIGH) {
  1057.    digitalWrite(DRIVER1_STEP, HIGH);
  1058.    delayMicroseconds(25);
  1059.    digitalWrite(DRIVER1_STEP, LOW);
  1060.    delayMicroseconds(30);
  1061.    
  1062.    if (checkStopCommand()) {
  1063.      Serial.println("Emergency Stop Triggered");
  1064.      stopMotor(); // Stop when "X" command is received
  1065.      return;
  1066.     }
  1067.   }
  1068.  digitalWrite(RELAY9, LOW); // Turn off Relay 1 to stop stepper motor control
  1069. }
  1070. void runOpenGripper() {
  1071.  // Define the behavior for Opening Grippers 1 & 2 after tying
  1072.  digitalWrite(RELAY3, LOW); // Deactivate Gripper 1 (RELAY3)
  1073.  digitalWrite(RELAY4, LOW); // Deactivate Gripper 2 (RELAY4)
  1074.  delay(1000);
  1075. }
  1076. void stopMotor() {
  1077.  // Stop Feeder motor
  1078.  analogWrite(RPWM_FEEDER, 0);
  1079.  analogWrite(LPWM_FEEDER, 0);
  1080.  digitalWrite(EN_FEEDER, LOW);
  1081.  digitalWrite(DRIVER1_ENA, HIGH);
  1082.  digitalWrite(DRIVER2_ENA, HIGH);
  1083.  feederRunning = false; // Reset feeder running flag
  1084.  // Turn off relays
  1085.  digitalWrite(RELAY9, LOW);
  1086.  digitalWrite(RELAY5, LOW);      // Disable both relays
  1087.  digitalWrite(RELAY6, LOW);
  1088.  motorRunning = false;  
  1089.  Serial.println("Motor stopped");
  1090. }
  1091. void resetSerialBuffer() {
  1092.  while (Serial.available()) {
  1093.    Serial.read();  // Clear serial input buffer
  1094.   }
  1095. }
  1096. void resetMode() {
  1097.  autoMode = false; // Reset Auto mode flag
  1098.  manualMode = false; // Reset Manual mode flag
  1099.  currentMode = NONE; // Reset the current mode to a default state
  1100.  resetSerialBuffer(); // Clear the serial buffer
  1101.  Serial.println("Enter next command (START, STOP, Q):");  // Prompt for the next command
  1102. }
  1103. void runMotor(bool clockwise) {
  1104.  motorComplete = false;
  1105.  if (motorRunning) {
  1106.    Serial.println("Motor is already running.");
  1107.    return isClockwise; // Return the current direction if already running
  1108.   }
  1109.  int relayPin = clockwise ? RELAY5 : RELAY6; // Choose correct relay
  1110.  digitalWrite(relayPin, HIGH); // Start motor
  1111.  motorRunning = true; // Set motor running flag to true
  1112.  countA = 0; // Reset count for Proxy A
  1113.  countB = 0; // Reset count for Proxy B
  1114.  countingEnabled = true; // Enable counting
  1115.  isClockwise = clockwise; // Set the direction
  1116.  
  1117.  // Disable counting for the opposite proxy and Proxy POS
  1118.  if (clockwise) {
  1119.    countProxyBEnabled = false; // Disable counting for Proxy B
  1120.    countProxyPOSEnabled = false; // Disable counting for Proxy POS
  1121.    countProxyAEnabled = true; // Enable counting for current direction
  1122.   }
  1123.   else {
  1124.    countProxyAEnabled = false; // Disable counting for Proxy A
  1125.    countProxyPOSEnabled = false; // Disable counting for Proxy POS
  1126.    countProxyBEnabled = true; // Enable counting for current direction
  1127.   }
  1128.  Serial.println(clockwise ? "INDUCTION MOTOR CW STARTED" : "INDUCTION MOTOR CCW STARTED");
  1129.  return clockwise; // Return the direction
  1130. }
  1131. void proxyA() {
  1132.  static int lastStateA = LOW; // Remember previous state for Proxy A
  1133.  static unsigned long lastTriggerTimeA = 0;
  1134.  const unsigned long debounceDelay = 300;
  1135.  const unsigned long minCountInterval = 2600; // Minimum time between counts (1 second)
  1136.  
  1137.  int sensorStateA = digitalRead(PROXY_A); // Read the current state of the sensor
  1138.  unsigned long currentTime = millis();
  1139.  
  1140.  // Status feedback
  1141.  Serial.print("Proxy A Status: ");
  1142.  Serial.println(sensorStateA == HIGH ? "Triggered (High)" : "Not Triggered (Low)");
  1143.  
  1144.  // Check for Proxy A (CW)
  1145.  if (isClockwise && countProxyAEnabled) {
  1146.    // Only detect Rising edge (LOW to HIGH)
  1147.    if (lastStateA == LOW && sensorStateA == HIGH) {
  1148.      // Debounce check
  1149.      if ((currentTime - lastTriggerTimeA) > debounceDelay && (currentTime - lastTriggerTimeA) > minCountInterval) {
  1150.        countA++;  // Increment count for Proxy A
  1151.        lastTriggerTimeA = currentTime;  // Update last trigger time
  1152.        Serial.print("Proxy A Count: ");
  1153.        Serial.println(countA);
  1154.  
  1155.        // Stop relay 4 if count reaches 4
  1156.        if (countA >= 4) {
  1157.          digitalWrite(RELAY5, LOW); // Stop relay 4
  1158.          stopMotor();
  1159.          motorRunning = false; // Set flag to stop motor
  1160.          countA = 0; // Reset count after stopping
  1161.          countProxyAEnabled = false; // Disable counting for Proxy A
  1162.          countProxyBEnabled = true; // Re-enable counting for Proxy B
  1163.          // SET COMPLETION FLAG HERE
  1164.          setMotorComplete();
  1165.         }
  1166.       }
  1167.     }
  1168.    lastStateA = sensorStateA; // Update last state for Proxy A
  1169.   }
  1170. }
  1171. void proxyB() {
  1172.  static int lastStateB = LOW; // Remember previous state for Proxy B
  1173.  static unsigned long lastTriggerTimeB = 0;
  1174.  const unsigned long debounceDelay = 300;
  1175.  const unsigned long minCountInterval = 2600; // Minimum time between counts (1 second)
  1176.  
  1177.  int sensorStateB = digitalRead(PROXY_B);  // Read the current state of the sensor
  1178.  unsigned long currentTime = millis();
  1179.  
  1180.  // Status feedback
  1181.  Serial.print("Proxy B Status: ");
  1182.  Serial.println(sensorStateB == HIGH ? "Triggered (High)" : "Not Triggered (Low)");
  1183.  
  1184.  // Check for Proxy B (CCW)
  1185.  if (!isClockwise && countProxyBEnabled) {
  1186.    // Only detect Rising edge (LOW to HIGH)
  1187.    if (lastStateB == LOW && sensorStateB == HIGH) {
  1188.      // Debounce check
  1189.       if ((currentTime - lastTriggerTimeB) > debounceDelay && (currentTime - lastTriggerTimeB) > minCountInterval) {
  1190.        countB++;  // Increment count for Proxy B
  1191.        lastTriggerTimeB = currentTime;
  1192.        Serial.print("Proxy B Count: ");
  1193.        Serial.println(countB);
  1194.  
  1195.         // Stop relay 5 if count reaches 5
  1196.         if (countB >= 4) {
  1197.          digitalWrite(RELAY6, LOW); // Stop relay 5
  1198.          stopMotor();
  1199.          motorRunning = false; // Set flag to stop motor
  1200.          countB = 0; // Reset count after stopping
  1201.          countProxyBEnabled = false; // Disable counting for Proxy B
  1202.          countProxyAEnabled = true; // Re-enable counting for Proxy A
  1203.          // SET COMPLETION FLAG HERE
  1204.          setMotorComplete();
  1205.         }
  1206.       }
  1207.     }
  1208.    lastStateB = sensorStateB; // Update last state for Proxy B
  1209.   }
  1210. }
  1211.  
  1212. /* END CODE */
  1213.  
Advertisement
Add Comment
Please, Sign In to add comment
Public Pastes
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!