// #include // #include // #include // #include // #include "Stepper.cpp" // #define MotorInterfaceType 4 // #define Z_STEP_PIN 2 // #define Z_DIR_PIN 5 // #define Z2_STEP_PIN 3 // #define Z2_DIR_PIN 6 // #define Y_STEP_PIN 4 // #define Y_DIR_PIN 7 // #define Y2_STEP_PIN 12 // #define Y2_DIR_PIN 13 // #define X_STEP_PIN 52 // #define X_DIR_PIN 53 // //end names dont match function // #define END_STEP_PIN 24 // #define END_DIR_PIN 26 // #define X_MIN_PIN 48 // #define X_MAX_PIN 46 // #define Y_MIN_PIN 44 // #define Y_MAX_PIN 42 // #define Z_MIN_PIN 40 // #define Z_MAX_PIN 38 // #define E_HOME_PIN 36 // #define ELECTROMAGNET_RELAY_PIN 10 // #define LATCH_PIN 11 // //Mem addresses // #define EEPROM_X_MIN 0 // #define EEPROM_X_MAX 4 // #define EEPROM_Y_MIN 8 // #define EEPROM_Y_MAX 12 // #define EEPROM_Z_MIN 16 // #define EEPROM_Z_MAX 20 // #define EEPROM_E_MAX 24 // #define EEPROM_CALIBRATED 28 // #define EEPROM_X_CUR 32 // #define EEPROM_Y_CUR 36 // #define EEPROM_Z_CUR 40 // createBufferedOutput(bufferedOut, 80, DROP_UNTIL_EMPTY); // // Global variables for axis limits and steps per CM // long xMinSteps, xMaxSteps, yMinSteps, yMaxSteps, zMinSteps, zMaxSteps, eMaxSteps; // float xStepsPerCM, yStepsPerCM, zStepsPerCM, eStepsPerIndex; // short EFFECTOR_INDEX = 0; // // consts // const float END_EFFECTOR_WIDTH_CM = 5.0; // const float HORIZONTAL_CARRAGE_WIDTH_CM = 5.6; // const float VERTICAL_CARRAGE_WIDTH_CM = 2.5; // const float STOPPER_WIDTH_CM = 1.0; // const float X_TRAVEL_CM = 25.0 - END_EFFECTOR_WIDTH_CM; // const float Y_TRAVEL_CM = 23.5 - VERTICAL_CARRAGE_WIDTH_CM - STOPPER_WIDTH_CM; // const float Z_TRAVEL_CM = 20.0 - HORIZONTAL_CARRAGE_WIDTH_CM - STOPPER_WIDTH_CM; // const int CALIBRATION_STEP_SPEED = 100; // const int MIN_DIR_SIGN = 1; // const int MAX_DIR_SIGN = -1; // const int EFFECTOR_COUNT = 6; // const int UNLATCH_ANGLE = 180; // const int LATCH_ANGLE = 110; // Stepper stepperX(X_STEP_PIN, X_DIR_PIN, 23); // Stepper stepperY1(Y_STEP_PIN, Y_DIR_PIN); // Stepper stepperY2(Y2_STEP_PIN, Y2_DIR_PIN); // Stepper stepperZ1(Z_STEP_PIN, Z_DIR_PIN); // Stepper stepperZ2(Z2_STEP_PIN, Z2_DIR_PIN); // Stepper stepperEnd(END_STEP_PIN, END_DIR_PIN, 25); // Servo servo; // void setup() { // Serial.begin(115200); // Serial.println("Gantry System Initializing"); // bufferedOut.connect(Serial); // servo.attach(LATCH_PIN); // servo.write(UNLATCH_ANGLE); // pinMode(X_MIN_PIN, INPUT_PULLUP); // pinMode(X_MAX_PIN, INPUT_PULLUP); // pinMode(Y_MIN_PIN, INPUT_PULLUP); // pinMode(Y_MAX_PIN, INPUT_PULLUP); // pinMode(Z_MIN_PIN, INPUT_PULLUP); // pinMode(Z_MAX_PIN, INPUT_PULLUP); // pinMode(E_HOME_PIN, INPUT_PULLUP); // // Set electromagnet relay pin as output and ensure it's off initially // pinMode(ELECTROMAGNET_RELAY_PIN, OUTPUT); // digitalWrite(ELECTROMAGNET_RELAY_PIN, LOW); // stepperX.init(); // stepperX.setMaxSpeed(1000); // stepperX.setSpeed(1000); // stepperY1.init(); // stepperY1.setMaxSpeed(1000); // stepperY1.setSpeed(1000); // stepperY2.init(); // stepperY2.setMaxSpeed(1000); // stepperY2.setSpeed(1000); // stepperZ1.init(); // stepperZ1.setMaxSpeed(1000); // stepperZ1.setSpeed(1000); // stepperZ2.init(); // stepperZ2.setMaxSpeed(1000); // stepperZ2.setSpeed(1000); // stepperEnd.init(); // stepperEnd.setSpeed(1000); // stepperEnd.setMaxSpeed(1000); // //calibrate each time // // if (EEPROM.read(EEPROM_CALIBRATED) != 0xAA) { // Serial.println("Calibration needed. Starting"); // // calibrateAndHome(); // // } else { // // Serial.println("Loading stored calibration data"); // // loadCalibrationData(); // // calculateStepsPerCM(); // // calculateStepsPerIndex(); // // Serial.println("Calibration data loaded successfully."); // // moveToCM(X_TRAVEL_CM / 2, 0, 0); // // } // Serial.println("Gantry System Ready!"); // } // void loop() { // stepperX.runSpeed(); // stepperY1.runSpeed(); // stepperY2.runSpeed(); // stepperZ1.runSpeed(); // stepperZ2.runSpeed(); // stepperEnd.runSpeed(); // // connectElectromagnet(); // // delay(2000); // // disconnectElectromagnet(); // // delay(2000); // // attachment/detachment sequences // // attachAtPosition(15.0, 10.0, 5.0); // Move to position and connect // // delay(3000); // // detachAndMove(5.0, 5.0, 12.0); // Disconnect and move away // // delay(3000); // // moveToCM(5.0, 3.0, 7.5); // // delay(2000); // // moveToHome(); // // delay(2000); // // if (!stepperX.isRunning()) // // stepperX.disableOutputs(); // // else // // stepperX.enableOutputs(); // // if (!stepperY1.isRunning()) // // stepperY1.disableOutputs(); // // else // // stepperY1.enableOutputs(); // // if (!stepperY2.isRunning()) // // stepperY2.disableOutputs(); // // else // // stepperY2.enableOutputs(); // // if (!stepperZ1.isRunning()) // // stepperZ1.disableOutputs(); // // else // // stepperZ1.enableOutputs(); // // if (!stepperZ2.isRunning()) // // stepperZ2.disableOutputs(); // // else // // stepperZ2.enableOutputs(); // // if (!stepperEnd.isRunning()) // // stepperEnd.disableOutputs(); // // else // // stepperEnd.enableOutputs(); // } // void calibrateAndHome() { // Serial.println("Starting calibration"); // calibrateXaxis(); // calibrateZaxis(); // calibrateYaxis(); // calibrateEndEffectorCarousal(); // saveCalibrationData(); // calculateStepsPerCM(); // calculateStepsPerIndex(); // moveToHome(); // Serial.println("Calibration complete"); // } // void calibrateEndEffectorCarousal(){ // Serial.println("Calibrating End Effector Carousal"); // stepperEnd.setCurrentPosition(0); // while (digitalRead(E_HOME_PIN) == HIGH) { // stepperEnd.move(MIN_DIR_SIGN * CALIBRATION_STEP_SPEED); // stepperEnd.run(); // } // stepperEnd.setCurrentPosition(0); // while (digitalRead(E_HOME_PIN) == LOW) { // stepperEnd.move(MIN_DIR_SIGN * CALIBRATION_STEP_SPEED); // stepperEnd.run(); // } // while (digitalRead(E_HOME_PIN) == HIGH) { // stepperEnd.move(MIN_DIR_SIGN * CALIBRATION_STEP_SPEED); // stepperEnd.run(); // } // eMaxSteps = stepperEnd.currentPosition(); // Serial.print("End Max: "); Serial.println(eMaxSteps); // } // void calibrateXaxis(){ // Serial.println("Calibrating X axis"); // stepperX.setCurrentPosition(0); // while (digitalRead(X_MIN_PIN) == HIGH) { // stepperX.move(MIN_DIR_SIGN * CALIBRATION_STEP_SPEED); // stepperX.run(); // } // stepperX.setCurrentPosition(0); // xMinSteps = stepperX.currentPosition(); // Serial.print("X Min: "); Serial.println(xMinSteps); // while (digitalRead(X_MAX_PIN) == HIGH) { // stepperX.move(MAX_DIR_SIGN * CALIBRATION_STEP_SPEED); // stepperX.run(); // } // xMaxSteps = stepperX.currentPosition(); // Serial.print("X Max: "); Serial.println(xMaxSteps); // } // void calibrateYaxis(){ // Serial.println("Calibrating Y axis"); // while (digitalRead(Y_MIN_PIN) == HIGH) { // stepperY1.move(MIN_DIR_SIGN * CALIBRATION_STEP_SPEED); // stepperY2.move(MIN_DIR_SIGN * CALIBRATION_STEP_SPEED); // stepperY1.run(); // stepperY2.run(); // } // stepperY1.setCurrentPosition(0); // stepperY2.setCurrentPosition(0); // yMinSteps = stepperY1.currentPosition(); // Serial.print("Y Min: "); Serial.println(yMinSteps); // while (digitalRead(Y_MAX_PIN) == HIGH) { // stepperY1.move(MAX_DIR_SIGN * CALIBRATION_STEP_SPEED); // stepperY2.move(MAX_DIR_SIGN * CALIBRATION_STEP_SPEED); // stepperY1.run(); // stepperY2.run(); // } // yMaxSteps = stepperY1.currentPosition(); // Serial.print("Y Max: "); Serial.println(yMaxSteps); // } // void calibrateZaxis(){ // Serial.println("Calibrating Z axis"); // while (digitalRead(Z_MIN_PIN) == HIGH) { // stepperZ1.move(MIN_DIR_SIGN * CALIBRATION_STEP_SPEED); // stepperZ2.move(MIN_DIR_SIGN * CALIBRATION_STEP_SPEED); // stepperZ1.run(); // stepperZ2.run(); // } // stepperZ1.setCurrentPosition(0); // stepperZ2.setCurrentPosition(0); // zMinSteps = stepperZ1.currentPosition(); // Serial.print("Z Min: "); Serial.println(zMinSteps); // while (digitalRead(Z_MAX_PIN) == HIGH) { // stepperZ1.move(MAX_DIR_SIGN * CALIBRATION_STEP_SPEED); // stepperZ2.move(MAX_DIR_SIGN * CALIBRATION_STEP_SPEED); // stepperZ1.run(); // stepperZ2.run(); // } // zMaxSteps = stepperZ1.currentPosition(); // Serial.print("Z Max: "); Serial.println(zMaxSteps); // } // void saveCalibrationData() { // EEPROM.put(EEPROM_X_MIN, xMinSteps); // EEPROM.put(EEPROM_X_MAX, xMaxSteps); // EEPROM.put(EEPROM_Y_MIN, yMinSteps); // EEPROM.put(EEPROM_Y_MAX, yMaxSteps); // EEPROM.put(EEPROM_Z_MIN, zMinSteps); // EEPROM.put(EEPROM_Z_MAX, zMaxSteps); // EEPROM.put(EEPROM_E_MAX, eMaxSteps); // EEPROM.write(EEPROM_CALIBRATED, 0xAA); // EEPROM.put(EEPROM_X_CUR, stepperX.currentPosition()); // EEPROM.put(EEPROM_Y_CUR, stepperY1.currentPosition()); // EEPROM.put(EEPROM_Z_CUR, stepperZ1.currentPosition()); // Serial.println("Calibration data saved to EEPROM"); // } // void saveCurrentAxisStepperPositions(){ // EEPROM.put(EEPROM_X_CUR, stepperX.currentPosition()); // EEPROM.put(EEPROM_Y_CUR, stepperY1.currentPosition()); // EEPROM.put(EEPROM_Z_CUR, stepperZ1.currentPosition()); // Serial.println("Current Stepper Position data saved to EEPROM"); // } // void loadCalibrationData() { // long xCur, yCur, zCur; // EEPROM.get(EEPROM_X_MIN, xMinSteps); // EEPROM.get(EEPROM_X_MAX, xMaxSteps); // EEPROM.get(EEPROM_Y_MIN, yMinSteps); // EEPROM.get(EEPROM_Y_MAX, yMaxSteps); // EEPROM.get(EEPROM_Z_MIN, zMinSteps); // EEPROM.get(EEPROM_Z_MAX, zMaxSteps); // EEPROM.get(EEPROM_E_MAX, eMaxSteps); // EEPROM.get(EEPROM_X_CUR, xCur); // EEPROM.get(EEPROM_Y_CUR, yCur); // EEPROM.get(EEPROM_Z_CUR, zCur); // stepperX.setCurrentPosition(xCur); // stepperY1.setCurrentPosition(yCur); // stepperY2.setCurrentPosition(yCur); // stepperZ1.setCurrentPosition(zCur); // stepperZ2.setCurrentPosition(zCur); // } // void calculateStepsPerCM() { // xStepsPerCM = abs(xMaxSteps - xMinSteps) / X_TRAVEL_CM; // yStepsPerCM = abs(yMaxSteps - yMinSteps) / Y_TRAVEL_CM; // zStepsPerCM = abs(zMaxSteps - zMinSteps) / Z_TRAVEL_CM; // Serial.println("Steps per CM calculated:"); // Serial.print("X: "); Serial.println(xStepsPerCM); // Serial.print("Y: "); Serial.println(yStepsPerCM); // Serial.print("Z: "); Serial.println(zStepsPerCM); // } // void calculateStepsPerIndex() { // eStepsPerIndex = eMaxSteps / EFFECTOR_COUNT; // //EFFECTOR_COUNT // Serial.println("Steps per Index calculated:"); // Serial.print("E: "); Serial.println(eStepsPerIndex); // } // // Convert CM position to steps for each axis // long cmToStepsX(float cm) { // if (cm < 0) cm = 0; // if (cm > X_TRAVEL_CM) cm = X_TRAVEL_CM; // return xMinSteps + (long)(cm * xStepsPerCM); // } // long cmToStepsY(float cm) { // if (cm < 0) cm = 0; // if (cm > Y_TRAVEL_CM) cm = Y_TRAVEL_CM; // return yMinSteps + (long)(cm * yStepsPerCM); // } // long cmToStepsZ(float cm) { // if (cm < 0) cm = 0; // if (cm > Z_TRAVEL_CM) cm = Z_TRAVEL_CM; // return zMinSteps + (long)(cm * zStepsPerCM); // } // // Convert steps to CM position for each axis // float stepsToCmX(long steps) { // return (float)(steps - xMinSteps) / xStepsPerCM; // } // float stepsToCmY(long steps) { // return (float)(steps - yMinSteps) / yStepsPerCM; // } // float stepsToCmZ(long steps) { // return (float)(steps - zMinSteps) / zStepsPerCM; // } // // Move to specific CM coordinates // void moveToCM(float xCM, float yCM, float zCM) { // long xTarget = cmToStepsX(xCM); // long yTarget = cmToStepsY(yCM); // long zTarget = cmToStepsZ(zCM); // Serial.print("Moving to: X="); Serial.print(xCM); // Serial.print("cm, Y="); Serial.print(yCM); // Serial.print("cm, Z="); Serial.print(zCM); Serial.println("cm"); // stepperX.moveTo(xTarget); // stepperY1.moveTo(yTarget); // stepperY2.moveTo(yTarget); // stepperZ1.moveTo(zTarget); // stepperZ2.moveTo(zTarget); // while (stepperX.distanceToGo() != 0 || stepperY1.distanceToGo() != 0 || // stepperY2.distanceToGo() != 0 || stepperZ1.distanceToGo() != 0 || // stepperZ2.distanceToGo() != 0) { // stepperX.run(); // stepperY1.run(); // stepperY2.run(); // stepperZ1.run(); // stepperZ2.run(); // } // Serial.println("Move complete"); // } // void moveToHome() { // moveToCM(0, 0, Z_TRAVEL_CM); // Serial.println("Moved to home position"); // } // // Get current position in CM // void getCurrentPositionCM(float &xCM, float &yCM, float &zCM) { // xCM = stepsToCmX(stepperX.currentPosition()); // yCM = stepsToCmY(stepperY1.currentPosition()); // zCM = stepsToCmZ(stepperZ1.currentPosition()); // } // // Print current position // void printCurrentPosition() { // float x, y, z; // getCurrentPositionCM(x, y, z); // Serial.print("Current Position - X: "); Serial.print(x); // Serial.print("cm, Y: "); Serial.print(y); // Serial.print("cm, Z: "); Serial.print(z); Serial.println("cm"); // } // void connectElectromagnet() { // digitalWrite(ELECTROMAGNET_RELAY_PIN, HIGH); // Serial.println("Electromagnet ON - End effector connected"); // } // void disconnectElectromagnet() { // digitalWrite(ELECTROMAGNET_RELAY_PIN, LOW); // Serial.println("Electromagnet OFF - End effector disconnected"); // } // bool isElectromagnetConnected() { // return digitalRead(ELECTROMAGNET_RELAY_PIN) == HIGH; // } // void attachAtPosition(float xCM, float yCM, float zCM) { // Serial.println("Starting attachment sequence"); // disconnectElectromagnet(); // moveToCM(xCM, yCM, zCM); // delay(500); // connectElectromagnet(); // unlatch(); // Serial.println("Attachment sequence complete"); // } // void detachAndMove(float xCM, float yCM, float zCM) { // Serial.println("Starting detachment sequence"); // disconnectElectromagnet(); // latch(); // delay(200); // moveToCM(xCM, yCM, zCM); // Serial.println("Detachment sequence complete"); // } // void moveAndDetach(float xCM, float yCM, float zCM) { // Serial.println("Starting detachment sequence"); // moveToCM(xCM, yCM, zCM); // delay(200); // disconnectElectromagnet(); // latch(); // Serial.println("Detachment sequence complete"); // } // void unlatch() // { // servo.write(UNLATCH_ANGLE); // delay(500); // } // void latch() // { // servo.write(LATCH_ANGLE); // delay(500); // }