commit 1e8f70f7a9484bc532085e8bd39f06420ffb36b1
Author: 2ManyProjects <shaivkamat@gmail.com>
Date:   Wed Sep 17 14:43:04 2025 -0500

    commiting codebase

diff --git a/sketch/Stepper.cpp b/sketch/Stepper.cpp
new file mode 100644
index 0000000..012a0ae
--- /dev/null
+++ b/sketch/Stepper.cpp
@@ -0,0 +1,181 @@
+#import <Arduino.h>
+#define DIRECTION_CW 1
+#define DIRECTION_CCW 0
+
+
+class Stepper {
+  private:
+    short enablePin = 8;
+    long currentPos = 0;
+    long targetPos = 0;
+    float speed = 0;
+    float maxSpeed = 1000;
+    float acceleration = 0;
+    short stepPin = 0;
+    short dirPin = 0;
+    long n;
+    float c0;
+    float cn;
+    float cmin; 
+    unsigned long stepInterval;
+    unsigned long lastStepTime;
+    unsigned long minPulseWidth = 1;
+    bool direction = DIRECTION_CW; // 1 == CW
+
+  public:
+    Stepper(short sP, short dP) {
+      stepPin = sP;
+      dirPin = dP;
+    }
+    Stepper(short sP, short dP, short en) {
+      stepPin = sP;
+      dirPin = dP;
+      enablePin = en;
+    }
+    void setSpeed(float sp){
+      if(sp > maxSpeed){
+        speed = maxSpeed;
+        return;
+      }
+      speed = sp;
+      return;
+    }
+    void setMaxSpeed(float sp){
+      maxSpeed = sp;
+      return;
+    }
+
+    float getSpeed(){
+      return speed;
+    }
+    float getMaxSpeed(){
+      return maxSpeed;
+    }
+    void init(){
+      pinMode(enablePin, OUTPUT);
+      pinMode(stepPin, OUTPUT); 
+      pinMode(dirPin, OUTPUT);
+      enableOutputs();
+      // digitalWrite(enablePin, LOW);
+    }
+    void disableOutputs(){
+      digitalWrite(enablePin, HIGH);
+    }
+    void enableOutputs(){
+      digitalWrite(enablePin, LOW);
+    }
+    long distanceToGo()
+    {
+      return targetPos - currentPos;
+    }
+
+    long targetPosition()
+    {
+      return targetPos;
+    }
+
+    long currentPosition()
+    {
+      return currentPos;
+    }
+    void blockingRunToTarget(){
+      while(abs(distanceToGo()) > 0){
+        run();
+      }
+    }
+    void setCurrentPosition(long position)
+    {
+      targetPos = currentPos = position;
+      n = 0;
+      stepInterval = 0;
+      speed = 0.0;
+    }
+
+    void stop()
+    {
+      targetPos = currentPos;
+      n = 0;
+      speed = 0.0;
+    //   move(0);
+    }
+
+    bool isRunning()
+    {
+      return !(speed == 0.0 && targetPos == currentPos);
+    }
+    void moveTo(long absolute)
+    {
+      if (targetPos != absolute)
+      {
+        targetPos = absolute;
+        stepInterval = fabs(1000000.0 / speed);
+      }
+    }
+
+    void move(long relative)
+    {
+      if(0 < relative){
+        direction = DIRECTION_CW;
+      }else {
+        direction = DIRECTION_CCW;
+      }
+      moveTo(currentPos + relative);
+    }
+
+    void setDirection(int dir){
+      if(0 < dir){
+        direction = DIRECTION_CW;
+      }else {
+        direction = DIRECTION_CCW;
+      }
+    }
+
+    bool runSpeed()
+    {
+        // Dont do anything unless we actually have a step interval
+      if (!speed)
+        return false;
+      stepInterval = fabs(1000000.0 / speed);
+
+      unsigned long time = micros();   
+      if (time - lastStepTime >= stepInterval)
+      {
+        if (direction == DIRECTION_CW)
+        {
+          // Clockwise
+          currentPos += 1;
+        }
+        else
+        {
+          // Anticlockwise  
+          currentPos -= 1;
+        }
+        step();
+
+        lastStepTime = time; // Caution: does not account for costs in step()
+
+        return true;
+      }else
+      {
+        return false;
+      }
+    }
+
+    bool run()
+    {
+      runSpeed();
+      return speed != 0.0 || distanceToGo() != 0;
+    }
+
+    void step()
+    {
+      if(direction == DIRECTION_CW)
+        digitalWrite(dirPin, HIGH);
+      else 
+        digitalWrite(dirPin, LOW);
+      digitalWrite(stepPin,HIGH);
+      delayMicroseconds(400);
+      digitalWrite(stepPin,LOW);
+      delayMicroseconds(400);
+    }
+};
diff --git a/sketch/sketch.ino b/sketch/sketch.ino
new file mode 100644
index 0000000..b5a66fa
--- /dev/null
+++ b/sketch/sketch.ino
@@ -0,0 +1,695 @@
+#include <AccelStepper.h>
+#include <SafeString.h>
+#include <EEPROM.h>
+#include <BufferedOutput.h>
+#include <Servo.h>
+#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);
+}
\ No newline at end of file