#include #include #include #include #include #include "Stepper.cpp" #define MotorInterfaceType 4 #define Z_STEP_PIN 4 #define Z_DIR_PIN 7 #define Z2_STEP_PIN 2 #define Z2_DIR_PIN 5 #define Y_STEP_PIN 3 #define Y_DIR_PIN 6 #define Y2_STEP_PIN 12 #define Y2_DIR_PIN 13 #define X_STEP_PIN 52 #define X_DIR_PIN 53 #define END_STEP_PIN 27 #define END_DIR_PIN 29 // #define END_STEP_PIN1 24 // #define END_STEP_PIN2 22 #define X_MIN_PIN 38 #define X_MAX_PIN 36 #define Y_MIN_PIN 44 #define Y_MAX_PIN 42 #define Z_MIN_PIN 46 #define Z_MAX_PIN 40 #define E_HOME_PIN 48 #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; const short STEPS_PER_ROT = 200; // 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; float X_TRAVEL_CM = 0.0; float Y_TRAVEL_CM = 0.0; float Z_TRAVEL_CM = 0.0; const int CALIBRATION_STEP_SPEED = 1000; 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; const float THREAD_ROTATIONS_PER_CM = 5.5; //x ratio 20:35 //yz ration 20:60 //5.5 rotations per cm 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); int initSpeed = 600; stepperX.init(); stepperX.setMaxSpeed(initSpeed); stepperX.setSpeed(initSpeed); stepperY1.init(); stepperY1.setMaxSpeed(initSpeed); stepperY1.setSpeed(initSpeed); stepperY2.init(); stepperY2.setMaxSpeed(initSpeed); stepperY2.setSpeed(initSpeed); stepperZ1.init(); stepperZ1.setMaxSpeed(initSpeed); stepperZ1.setSpeed(initSpeed); stepperZ2.init(); stepperZ2.setMaxSpeed(initSpeed); stepperZ2.setSpeed(initSpeed); stepperEnd.init(); stepperEnd.setMaxSpeed(initSpeed); stepperEnd.setSpeed(initSpeed); stepperEnd.setDirection(-1); // stepperEnd.move(-1200); delay(5000); //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.run(); // stepperEnd.run(); // 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"); // calibrateEndEffectorCarousal(); // delay(2500); // moveEffectorIndex(1); // delay(2500); // moveEffectorIndex(2); // delay(2500); // moveEffectorIndex(3); // delay(2500); // moveEffectorIndex(4); // delay(2500); // moveEffectorIndex(5); // delay(2500); // moveEffectorIndex(2); // delay(2500); // moveEffectorIndex(0); calibrateXaxis(); calculateStepsPerCM(); moveToXCM(X_TRAVEL_CM / 2); calibrateZaxis(); calculateStepsPerCM(); moveToZCM(Z_TRAVEL_CM / 2); calibrateYaxis(); calculateStepsPerCM(); moveToYCM(Y_TRAVEL_CM / 2); saveCalibrationData(); float zCM = 0.0; float yCM = 0.0; float xCM = 0.0; // moveToHome(); // Serial.println("Calibration complete"); Serial.print("CurrentX "); Serial.print(xCM); Serial.print(" CurrentY "); Serial.print(yCM); Serial.print(" CurrentZ "); Serial.print(zCM); // getCurrentPositionCM(xCM, yCM, zCM); // moveToCM(xCM / 2, yCM / 2, zCM / 2); } void calibrateEndEffectorCarousal(){ // Serial.println("Calibrating End Effector Carousal"); // stepperEnd.setPosition(0); stepperEnd.move(-1200); while (digitalRead(E_HOME_PIN) == HIGH) { stepperEnd.run(); delay(15); } delay(500); while (digitalRead(E_HOME_PIN) == LOW) { stepperEnd.runSpeed(); delay(15); } delay(2500); // stepperEnd.move(-20); // Serial.println(digitalRead(E_HOME_PIN)); // Serial.println(abs(millis() - start)); // while (stepperEnd.run()) { // stepperEnd.run(); // delay(15); // } // delay(2500); // stepperEnd.move(15); // while (stepperEnd.run()) { // delay(15); // } // delay(2500); // Serial.println(digitalRead(E_HOME_PIN)); long position = stepperEnd.currentPosition(); while (digitalRead(E_HOME_PIN) == HIGH) { stepperEnd.move(-1 * CALIBRATION_STEP_SPEED); stepperEnd.run(); delay(15); } eMaxSteps = stepperEnd.currentPosition()- position; Serial.print("End Max: "); Serial.println(eMaxSteps); // stepperEnd.setPosition(0); calculateStepsPerIndex(); // stepperEnd.move(-1 * eMaxSteps); } void calibrateXaxis(){ Serial.println("Calibrating X axis"); while (digitalRead(X_MIN_PIN) == HIGH) { stepperX.runSpeed(); delay(1); } xMinSteps = stepperX.currentPosition(); Serial.print("X Min: "); Serial.println(xMinSteps); while (digitalRead(X_MAX_PIN) == HIGH) { stepperX.setDirection(-1); stepperX.runSpeed(); } stepperX.setDirection(1); xMaxSteps = stepperX.currentPosition(); Serial.print("X Max: "); Serial.println(xMaxSteps); } void calibrateYaxis(){ Serial.println("Calibrating Y axis"); while (digitalRead(Y_MAX_PIN) == HIGH) { stepperY1.runSpeed(); stepperY2.runSpeed(); } yMinSteps = stepperY1.currentPosition(); Serial.print("Y Min: "); Serial.println(yMinSteps); while (digitalRead(Y_MIN_PIN) == HIGH) { stepperY1.setDirection(-1); stepperY2.setDirection(-1); stepperY1.runSpeed(); stepperY2.runSpeed(); } stepperY1.setDirection(1); stepperY2.setDirection(1); yMaxSteps = stepperY1.currentPosition(); Serial.print("Y Max: "); Serial.println(yMaxSteps); } void calibrateZaxis(){ Serial.println("Calibrating Z axis"); while (digitalRead(Z_MIN_PIN) == HIGH) { stepperZ1.runSpeed(); stepperZ2.runSpeed(); } zMinSteps = stepperZ1.currentPosition(); Serial.print("Z Min: "); Serial.println(zMinSteps); while (digitalRead(Z_MAX_PIN) == HIGH) { stepperZ1.setDirection(-1); stepperZ2.setDirection(-1); stepperZ1.runSpeed(); stepperZ2.runSpeed(); } stepperZ1.setDirection(1); stepperZ2.setDirection(1); 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.setPosition(xCur); stepperY1.setPosition(yCur); stepperY2.setPosition(yCur); stepperZ1.setPosition(zCur); stepperZ2.setPosition(zCur); } void calculateStepsPerCM() { // Motor specifications const int STEPS_PER_ROTATION = 200; const float X_DRIVE_GEAR = 20.0; const float X_DRIVEN_GEAR = 35.0; const float X_GEAR_RATIO = X_DRIVEN_GEAR / X_DRIVE_GEAR; // 35/20 = 1.75 const float YZ_DRIVE_GEAR = 20.0; const float YZ_DRIVEN_GEAR = 60.0; const float YZ_GEAR_RATIO = YZ_DRIVEN_GEAR / YZ_DRIVE_GEAR; // 60/20 = 3.0 const float CM_PER_DRIVEN_ROTATION = 1.0 / THREAD_ROTATIONS_PER_CM; // 1cm per 5.5 rotations = ~0.1818 cm/rotation // Calculate steps per centimeter for each axis // X-axis: Motor steps -> drive gear -> driven gear -> linear motion // Steps per driven gear rotation = motor steps per rotation * gear ratio float xStepsPerDrivenRotation = STEPS_PER_ROTATION * X_GEAR_RATIO; xStepsPerCM = xStepsPerDrivenRotation / CM_PER_DRIVEN_ROTATION; // Y and Z axis float yzStepsPerDrivenRotation = STEPS_PER_ROTATION * YZ_GEAR_RATIO; yStepsPerCM = yzStepsPerDrivenRotation / CM_PER_DRIVEN_ROTATION; zStepsPerCM = yzStepsPerDrivenRotation / CM_PER_DRIVEN_ROTATION; Serial.println("Steps per CM calculated:"); Serial.print("X-axis: "); Serial.print(xStepsPerCM); Serial.println(" steps/cm"); Serial.print("Y-axis: "); Serial.print(yStepsPerCM); Serial.println(" steps/cm"); Serial.print("Z-axis: "); Serial.print(zStepsPerCM); Serial.println(" steps/cm"); if (xMaxSteps != 0 || xMinSteps != 0) { X_TRAVEL_CM = abs(xMaxSteps - xMinSteps) / xStepsPerCM; Serial.print("X-axis total travel: "); Serial.print(X_TRAVEL_CM); Serial.println(" cm"); } if (yMaxSteps != 0 || yMinSteps != 0) { Y_TRAVEL_CM = abs(yMaxSteps - yMinSteps) / yStepsPerCM; Serial.print("Y-axis total travel: "); Serial.print(Y_TRAVEL_CM); Serial.println(" cm"); } if (zMaxSteps != 0 || zMinSteps != 0) { Z_TRAVEL_CM = abs(zMaxSteps - zMinSteps) / zStepsPerCM; Serial.print("Z-axis total travel: "); Serial.print(Z_TRAVEL_CM); Serial.println(" cm"); } } 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; // Use the minimum step position as the 0 CM reference return min(xMinSteps, xMaxSteps) + (long)(cm * xStepsPerCM); } long cmToStepsY(float cm) { if (cm < 0) cm = 0; if (cm > Y_TRAVEL_CM) cm = Y_TRAVEL_CM; return min(yMinSteps, yMaxSteps) + (long)(cm * yStepsPerCM); } long cmToStepsZ(float cm) { if (cm < 0) cm = 0; if (cm > Z_TRAVEL_CM) cm = Z_TRAVEL_CM; return min(zMinSteps, zMaxSteps) + (long)(cm * zStepsPerCM); } // Convert steps to CM position for each axis float stepsToCmX(long steps) { return (float)abs(steps - min(xMinSteps, xMaxSteps)) / xStepsPerCM; } float stepsToCmY(long steps) { return (float)abs(steps - min(yMinSteps, yMaxSteps)) / yStepsPerCM; } float stepsToCmZ(long steps) { return (float)abs(steps - min(zMinSteps, zMaxSteps)) / zStepsPerCM; } void moveToXCM(float xCM){ long xTarget = cmToStepsX(xCM); Serial.print("Moving to: X="); Serial.print(xCM); Serial.println("cm"); stepperX.moveTo(xTarget); while (stepperX.run()) { stepperX.run(); } Serial.println("Move complete"); } void moveToYCM(float yCM) { long yTarget = cmToStepsY(yCM); Serial.print("Moving to: Y="); Serial.print(yCM);Serial.println("cm"); stepperY1.moveTo(yTarget); stepperY2.moveTo(yTarget); while (stepperY1.run() && stepperY2.run()) { stepperY1.run(); stepperY2.run(); } Serial.println("Move complete"); } // Move to specific CM coordinates void moveToZCM(float zCM) { long zTarget = cmToStepsZ(zCM); Serial.print("Moving to: Z="); Serial.print(zCM); Serial.println("cm"); stepperZ1.moveTo(zTarget); stepperZ2.moveTo(zTarget); while (stepperZ1.run() && stepperZ2.run()) { stepperZ1.run(); stepperZ2.run(); } Serial.println("Move complete"); } // 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.run()|| stepperY1.run() || stepperY2.run() || stepperZ1.run() || stepperZ2.run()) { 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); } // cur 5 goal 2 // void moveEffectorIndex(short index) { Serial.println("moveEffectorIndex"); if(index < 0 || index >= EFFECTOR_COUNT || index == EFFECTOR_INDEX){ return; } // long position = stepperEnd.currentPosition() % eMaxSteps; int indexSteps = eStepsPerIndex; int diff = 0; if(index > EFFECTOR_INDEX){ diff = abs(index - EFFECTOR_INDEX) * indexSteps; }else { // Serial.print("EFFECTOR_INDEX: "); // Serial.print(EFFECTOR_INDEX); // Serial.print(", EFFECTOR_COUNT: "); // Serial.print(EFFECTOR_COUNT); // Serial.print(", EFFECTOR_INDEX: "); // Serial.print(EFFECTOR_INDEX); // Serial.print(", index: "); // Serial.print(index); // Serial.print(", res: "); // Serial.println(((EFFECTOR_COUNT - (EFFECTOR_INDEX)) + index)); diff = ((EFFECTOR_COUNT - (EFFECTOR_INDEX)) + index) * indexSteps; } Serial.println(diff); stepperEnd.move(diff); EFFECTOR_INDEX = index; while (stepperEnd.run()) { delay(15); } delay(500); }