added homing.

src/main.cpp

@@ -12,8 +12,9 @@
  *   S2-<n>   → Step motor 2 CCW  n steps
  *   SPD:<n>  → Set max speed for both motors (steps/sec)
  *   ACC:<n>  → Set acceleration for both motors (steps/sec²)
- *   HOME1    → Zero motor 1 position
- *   HOME2    → Zero motor 2 position
+ *   HOME1    → Home motor 1 (Right)
+ *   HOME2    → Home motor 2 (Left)
+ *   HOMEALL  → Home both motors simultaneously
  *   POS      → Request current positions (triggers NOTIFY)
  *
  * BLE Status Characteristic (NOTIFY):
@@ -73,6 +74,9 @@ const unsigned long NOTIFY_INTERVAL_MS = 200;
 // ─── Forward declarations ─────────────────────────────────────────
 void parseCommand(const String& cmd);
 void sendPosition();
+void performHomingAll();
+void performHoming1();
+void performHoming2();
 
 // ─── BLE Server Callbacks ─────────────────────────────────────────
 class ServerCallbacks : public BLEServerCallbacks {
@@ -132,15 +136,22 @@ void parseCommand(const String& cmd) {
     }
     // HOME1
     if (cmd == "HOME1") {
-        stepper1.setCurrentPosition(0);
-        Serial.println("[S1] Zeroed");
+        Serial.println("[S1] Homing Motor 1...");
+        performHoming1();
         sendPosition();
         return;
     }
     // HOME2
     if (cmd == "HOME2") {
-        stepper2.setCurrentPosition(0);
-        Serial.println("[S2] Zeroed");
+        Serial.println("[S2] Homing Motor 2...");
+        performHoming2();
+        sendPosition();
+        return;
+    }
+    // HOMEALL
+    if (cmd == "HOMEALL") {
+        Serial.println("[SYS] Homing both motors...");
+        performHomingAll();
         sendPosition();
         return;
     }
@@ -233,3 +244,77 @@ void loop() {
         }
     }
 }
+
+// ─── Homing Routines ──────────────────────────────────────────────
+
+void performHomingAll() {
+    // 1. Move against limits (M1 CCW, M2 CCW)
+    stepper2.moveTo(1024);
+    stepper1.moveTo(2048);
+    while (stepper1.distanceToGo() != 0 || stepper2.distanceToGo() != 0) {
+        stepper1.run();
+        stepper2.run();
+    }
+    stepper1.setCurrentPosition(0);
+
+    // 2. Move to negative limits (CW)
+    stepper2.moveTo(-1050);
+    stepper1.move(-1300); // equivalent to moveTo(-1300) since pos is 0
+    while (stepper1.distanceToGo() != 0 || stepper2.distanceToGo() != 0) {
+        stepper1.run();
+        stepper2.run();
+    }
+    stepper2.setCurrentPosition(0);
+
+    // 3. Move to final home position
+    stepper2.moveTo(550);
+    stepper1.moveTo(-530);
+    while (stepper1.distanceToGo() != 0 || stepper2.distanceToGo() != 0) {
+        stepper1.run();
+        stepper2.run();
+    }
+    
+    // 4. Set origin relative to this final position
+    stepper2.setCurrentPosition(-1024);
+    stepper1.setCurrentPosition(0);
+
+    stepper1.disableOutputs();
+    stepper2.disableOutputs();
+    Serial.println("[SYS] Homing all complete.");
+}
+
+void performHoming1() {
+    // M1 (Right) stalls going positive (CCW)
+    stepper1.move(2048);
+    while (stepper1.distanceToGo() != 0) {
+        stepper1.run();
+    }
+    stepper1.setCurrentPosition(0);
+
+    // Move to final home position (CW)
+    stepper1.moveTo(-530);
+    while (stepper1.distanceToGo() != 0) {
+        stepper1.run();
+    }
+    stepper1.setCurrentPosition(0);
+    stepper1.disableOutputs();
+    Serial.println("[S1] Homing complete.");
+}
+
+void performHoming2() {
+    // M2 (Left) stalls going negative (CW)
+    stepper2.move(-2048);
+    while (stepper2.distanceToGo() != 0) {
+        stepper2.run();
+    }
+    stepper2.setCurrentPosition(0);
+
+    // Move to final home position (CCW)
+    stepper2.moveTo(550);
+    while (stepper2.distanceToGo() != 0) {
+        stepper2.run();
+    }
+    stepper2.setCurrentPosition(-1024);
+    stepper2.disableOutputs();
+    Serial.println("[S2] Homing complete.");
+}