Untitled

#include <AccelStepper.h>

// Pin definitions
const int dirPin = 10;
const int pulPin = 9;
const int positionSwitchPin = 13;
const int startButtonPin = 2;
const int emergencyStopPin = A5;
const int CylinderSolenoidPin = 7;
const int AutoMoveSolenoidPin = 6;
const int proximityPin = A0;
const int woodPositionSwitchPin = A1;
const int releaseClamps = 4;
const int secondaryDirPin = 11;
const int secondaryPulPin = 12;
const int secondaryPositionSwitchPin = A3;
const int secondarySolenoidPin = 8;

// Constants
const unsigned long DEBOUNCE_DELAY = 20;
const float STEPS_PER_INCH = 400 * 2.5;
const float CUTTING_DISTANCE = 9.5;
const int SECONDARY_STEPS_PER_INCH = 2000;
const float SECONDARY_MOVE_DISTANCE = 3.74; // 1 inch, adjust as needed

// Speed settings
float initSpeed = 1000;
float cuttingSpeed = 1100;
float returnSpeed = 7500;
float secondarySpeed = 4000;

// AccelStepper setup
AccelStepper primaryStepper(AccelStepper::DRIVER, pulPin, dirPin);
AccelStepper secondaryStepper(AccelStepper::DRIVER, secondaryPulPin, secondaryDirPin);

// State variables
bool emergencyStop = false;
bool initializationComplete = false;
bool cylinderSolenoidActivated = false;
long positionSwitchLocation = 0;
bool isSecondaryHomed = false;

// Debounce variables for start button
unsigned long lastDebounceTime = 0;
int lastButtonState = LOW;
int buttonState = LOW;

void setup() {
  Serial.begin(115200);

  // Setup for primary system
  primaryStepper.setMaxSpeed(7500);
  primaryStepper.setAcceleration(7000);
  
  // Setup for secondary system
  secondaryStepper.setMaxSpeed(secondarySpeed);
  secondaryStepper.setAcceleration(5000);

  // Pin mode setup
  pinMode(positionSwitchPin, INPUT);
  pinMode(emergencyStopPin, INPUT);
  pinMode(startButtonPin, INPUT);
  pinMode(CylinderSolenoidPin, OUTPUT);
  pinMode(AutoMoveSolenoidPin, OUTPUT);
  pinMode(proximityPin, INPUT);
  pinMode(woodPositionSwitchPin, INPUT);
  pinMode(releaseClamps, INPUT);
  pinMode(secondaryPositionSwitchPin, INPUT);
  pinMode(secondarySolenoidPin, OUTPUT);

  // Initial state
  digitalWrite(CylinderSolenoidPin, LOW);
  digitalWrite(AutoMoveSolenoidPin, LOW);
  digitalWrite(secondarySolenoidPin, LOW);

  initializeSystem();
}

void loop() {
  if (checkEmergencyStop()) {
    checkAndActivateCylinders();
    return;
  }

  if (!initializationComplete) {
    initializeSystem();
    return;
  }

  // Debounce start button
  int reading = digitalRead(startButtonPin);
  if (reading != lastButtonState) {
    lastDebounceTime = millis();
  }

  if ((millis() - lastDebounceTime) > 100) {
    if (reading != buttonState) {
      buttonState = reading;
      if (buttonState == HIGH) {
        runCuttingProcess();
      }
    }
  }

  lastButtonState = reading;

  checkAndActivateCylinders();
}

void checkAndActivateCylinders() {
  if (digitalRead(woodPositionSwitchPin) == LOW) {
    activateAllSolenoids();
  } else {
    deactivateAllSolenoids();
  }
}

void initializeSystem() {
  homePrimaryMotor();
  activateAllSolenoids();
  initializationComplete = true;
}

void homePrimaryMotor() {
  Serial.println("Homing primary motor...");
  
  primaryStepper.setMaxSpeed(initSpeed);
  primaryStepper.setSpeed(initSpeed);

  while (!checkEmergencyStop() && digitalRead(positionSwitchPin) == LOW) {
    primaryStepper.runSpeed();
  }
  
  if (!emergencyStop) {
    primaryStepper.stop();
    primaryStepper.setCurrentPosition(0);
    positionSwitchLocation = 0;
    Serial.println("Primary motor homed and cylinders retracted");
  }
}

void homeSecondaryMotor() {
  secondaryStepper.setSpeed(initSpeed);

  while (!checkEmergencyStop() && digitalRead(secondaryPositionSwitchPin) == LOW) {
    secondaryStepper.runSpeed();
  }
  
  if (!emergencyStop) {
    secondaryStepper.stop();
    secondaryStepper.setCurrentPosition(0);
    isSecondaryHomed = true;
    Serial.println("Secondary motor homed.");
  }
}

void activateAllSolenoids() {
  digitalWrite(CylinderSolenoidPin, LOW);
  digitalWrite(AutoMoveSolenoidPin, LOW);
  digitalWrite(secondarySolenoidPin, LOW);
  cylinderSolenoidActivated = true;
}

void deactivateAllSolenoids() {
  digitalWrite(CylinderSolenoidPin, HIGH);
  digitalWrite(AutoMoveSolenoidPin, HIGH);
  digitalWrite(secondarySolenoidPin, HIGH);
  cylinderSolenoidActivated = false;
}

bool checkEmergencyStop() {
  if (digitalRead(emergencyStopPin) == LOW) {
    handleEmergencyStop();
    return true;
  }
  return false;
}

void handleEmergencyStop() {
  emergencyStop = true;
  primaryStepper.stop();
  secondaryStepper.stop();
  deactivateAllSolenoids();
  
  while (digitalRead(emergencyStopPin) == LOW) {
    checkAndActivateCylinders();
    delay(100);
  }
  
  Serial.println("Emergency stop released. Reinitializing system...");
  delay(1000);
  emergencyStop = false;
  initializationComplete = false;
  isSecondaryHomed = false;
}

void runCuttingProcess() {
  // Step 1: Deactivate all solenoids to engage clamps
  deactivateAllSolenoids();
  
  // Step 2: Wait for clamps to engage
  delay(500);
  
  // Step 3: Move primary motor to cutting distance
  movePrimaryMotor(CUTTING_DISTANCE, cuttingSpeed);
    
  // Step 5: Activate secondary solenoid to allow wood movement
  digitalWrite(secondarySolenoidPin, LOW);
  delay(100);

  // Step 5 combined: Return both primary and secondary motors to home position
  returnBothMotorsHome();

  // Step 6: Deactivate secondary solenoid to secure wood
  digitalWrite(secondarySolenoidPin, HIGH);
  delay(100);

  // Step 7: Activate cylinder solenoid to allow the secondary solenoid to drag the wood into position
  digitalWrite(CylinderSolenoidPin, LOW);
  delay(100);
  
  // Step 9: Move secondary motor to set distance
  moveSecondaryMotor(SECONDARY_MOVE_DISTANCE);
}

void movePrimaryMotor(float distance, float speed) {
  long steps = distance * STEPS_PER_INCH;
  primaryStepper.setMaxSpeed(speed);
  primaryStepper.move(-steps);  // Negative for counterclockwise movement
  
  while (primaryStepper.distanceToGo() != 0) {
    primaryStepper.run();
    if (checkEmergencyStop()) return;
  }
}

void returnBothMotorsHome() {
  primaryStepper.setMaxSpeed(returnSpeed);
  primaryStepper.moveTo(-positionSwitchLocation);
  
  secondaryStepper.setSpeed(1700);

  bool primaryHomed = false;
  bool secondaryHomed = false;

  while (!primaryHomed || !secondaryHomed) {
    if (!primaryHomed) {
      primaryStepper.run();
      if (digitalRead(positionSwitchPin) == HIGH) {
        primaryStepper.stop();
        primaryStepper.setCurrentPosition(-positionSwitchLocation);
        primaryHomed = true;
      }
    }

    if (!secondaryHomed) {
      secondaryStepper.runSpeed();
      if (digitalRead(secondaryPositionSwitchPin) == HIGH) {
        secondaryStepper.stop();
        secondaryStepper.setCurrentPosition(0);
        secondaryHomed = true;
      }
    }

    if (checkEmergencyStop()) return;
  }
}

void moveSecondaryMotor(float distance) {
  long steps = distance * SECONDARY_STEPS_PER_INCH;
  secondaryStepper.move(-steps);
  
  while (secondaryStepper.distanceToGo() != 0) {
    secondaryStepper.run();
    if (checkEmergencyStop()) return;
  }
}