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 pin4 = 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 = 2.5;
//onst float CUTTING_DISTANCE = 9.5; (saved old value)
const int SECONDARY_STEPS_PER_INCH = 400 * 2.5;
const float SECONDARY_MOVE_DISTANCE = 3.5; // 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;

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(pin4, INPUT);
  pinMode(secondaryPositionSwitchPin, INPUT);
  pinMode(secondarySolenoidPin, OUTPUT);

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

  initializeSystem();
}

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

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

  if (digitalRead(startButtonPin) == HIGH) {
    runCuttingProcess();
  }
}

void initializeSystem() {
  Serial.println("Initializing system...");
  
  // Home primary motor
  homePrimaryMotor();
  
  // Home secondary motor
  homeSecondaryMotor();
  
  // Activate all solenoids to disengage clamps
  activateAllSolenoids();
  
  initializationComplete = true;
  Serial.println("System initialized and ready.");
}

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() {
  Serial.println("Homing secondary motor...");
  
  secondaryStepper.setSpeed(initSpeed);  // Negative speed to move towards home

  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;
  Serial.println("All solenoids activated, clamps disengaged.");
}

void deactivateAllSolenoids() {
  digitalWrite(CylinderSolenoidPin, HIGH);
  digitalWrite(AutoMoveSolenoidPin, HIGH);
  digitalWrite(secondarySolenoidPin, HIGH);
  cylinderSolenoidActivated = false;
  Serial.println("All solenoids deactivated, clamps engaged.");
}

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

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

void runCuttingProcess() {
  Serial.println("Starting cutting process...");
  
  // 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 4: Return primary motor to home position
  returnPrimaryMotorHome();

  // Step 5: Activate secondary solenoid to allow wood movement
  digitalWrite(secondarySolenoidPin, LOW);

  // Step 6: Return Secondary motor to home position
  delay(100);  // Short delay to ensure solenoid is fully activated
  returnSecondaryMotorHome();


  // Step 7: Deactivate secondary solenoid to secure wood
  digitalWrite(secondarySolenoidPin, HIGH);
  delay(100);  // Short delay to ensure solenoid is fully deactivated

  // Step 9: Activate cylinder solenoid to allow the secondary solenoid to drag the wood into position
  digitalWrite(CylinderSolenoidPin, LOW);
  delay(100);  // Short delay to ensure solenoid is fully deactivated
  
  // Step 10: Move secondary motor to set distance
  moveSecondaryMotor(SECONDARY_MOVE_DISTANCE);
  
  Serial.println("Cutting process completed. Ready for next cycle.");
}

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 returnPrimaryMotorHome() {
  Serial.println("Returning primary motor to home...");
  
  primaryStepper.setMaxSpeed(returnSpeed);
  primaryStepper.moveTo(-positionSwitchLocation);
  
  while (primaryStepper.distanceToGo() != 0) {
    primaryStepper.run();
    if (checkEmergencyStop()) return;
    
    if (digitalRead(positionSwitchPin) == HIGH) {
      primaryStepper.stop();
      primaryStepper.setCurrentPosition(-positionSwitchLocation);
      break;
    }
  }
  
  Serial.println("Primary motor returned to home position.");
}

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

void returnSecondaryMotorHome() {
  Serial.println("Returning secondary motor to home...");
  
  secondaryStepper.setSpeed(secondarySpeed);  // Negative speed to move towards home

  while (!checkEmergencyStop() && digitalRead(secondaryPositionSwitchPin) == LOW) {
    secondaryStepper.runSpeed();
  }
  
  if (!emergencyStop) {
    secondaryStepper.stop();
    secondaryStepper.setCurrentPosition(0);
    Serial.println("Secondary motor returned to home position.");
  }
}