kinematic kinda works

src/main.cpp

@@ -252,15 +252,66 @@ void setup() {
 }
 
 // ─── Loop ─────────────────────────────────────────────────────────
+// ─── Homing State Machine ─────────────────────────────────────────
+enum HomingState {
+  HOME_IDLE,
+  HOME_ALL_PHASE1,
+  HOME_ALL_PHASE2,
+  HOME_1_PHASE1,
+  HOME_1_PHASE2,
+  HOME_2_PHASE1,
+  HOME_2_PHASE2
+};
+HomingState homingState = HOME_IDLE;
+
 void loop() {
-  // Run steppers (non-blocking AccelStepper)
   stepper1.run();
   stepper2.run();
 
+  if (homingState == HOME_ALL_PHASE1) {
+    if (stepper1.distanceToGo() == 0 && stepper2.distanceToGo() == 0) {
+      stepper1.setCurrentPosition(474);
+      stepper2.setCurrentPosition(530);
+      stepper1.moveTo(1024);
+      stepper2.moveTo(0);
+      homingState = HOME_ALL_PHASE2;
+      Serial.println("[SYS] Homing all: Phase 2 (Move to Outward Home)");
+    }
+  } else if (homingState == HOME_ALL_PHASE2) {
+    if (stepper1.distanceToGo() == 0 && stepper2.distanceToGo() == 0) {
+      stepper1.disableOutputs();
+      stepper2.disableOutputs();
+      homingState = HOME_IDLE;
+      Serial.println("[SYS] Homing all complete.");
+    }
+  } else if (homingState == HOME_1_PHASE1) {
+    if (stepper1.distanceToGo() == 0) {
+      stepper1.setCurrentPosition(474);
+      stepper1.moveTo(1024);
+      homingState = HOME_1_PHASE2;
+    }
+  } else if (homingState == HOME_1_PHASE2) {
+    if (stepper1.distanceToGo() == 0) {
+      stepper1.disableOutputs();
+      homingState = HOME_IDLE;
+    }
+  } else if (homingState == HOME_2_PHASE1) {
+    if (stepper2.distanceToGo() == 0) {
+      stepper2.setCurrentPosition(530);
+      stepper2.moveTo(0);
+      homingState = HOME_2_PHASE2;
+    }
+  } else if (homingState == HOME_2_PHASE2) {
+    if (stepper2.distanceToGo() == 0) {
+      stepper2.disableOutputs();
+      homingState = HOME_IDLE;
+    }
+  }
+
   // Periodic position notify while motors are moving
   if (bleConnected) {
     unsigned long now = millis();
-    bool moving = stepper1.isRunning() || stepper2.isRunning();
+    bool moving = stepper1.isRunning() || stepper2.isRunning() || homingState != HOME_IDLE;
     if (moving && (now - lastNotify >= NOTIFY_INTERVAL_MS)) {
       lastNotify = now;
       sendPosition();
@@ -272,74 +323,19 @@ void loop() {
 
 void performHomingAll() {
   Serial.println("[SYS] Homing all: Phase 1 (Stall UP)");
-  
-  // 1. Swing BOTH motors UP to hit the mechanism housing (stall point)
-  // M1 (Left) swings UP by moving CW (negative)
-  // M2 (Right) swings UP by moving CCW (positive)
   stepper1.move(-2048); 
   stepper2.move(2048);
-  while (stepper1.distanceToGo() != 0 || stepper2.distanceToGo() != 0) {
-    runSteppersWithNotify();
-  }
-  
-  // 2. At this point, BOTH are stalled against the mechanism housing.
-  // We know the physical angles of these stall points!
-  // M1 stall is at +474 steps (83.3 degrees).
-  // M2 stall is at +530 steps (93.1 degrees).
-  stepper1.setCurrentPosition(474);
-  stepper2.setCurrentPosition(530);
-
-  Serial.println("[SYS] Homing all: Phase 2 (Move to Outward Home)");
-
-  // 3. Move BOTH motors to their outward home positions
-  // M1 goes to +1024 (180 degrees, pointing Left)
-  // M2 goes to 0 (0 degrees, pointing Right)
-  stepper1.moveTo(1024);
-  stepper2.moveTo(0);
-  while (stepper1.distanceToGo() != 0 || stepper2.distanceToGo() != 0) {
-    runSteppersWithNotify();
-  }
-
-  // 4. Disable outputs to rest
-  stepper1.disableOutputs();
-  stepper2.disableOutputs();
-  Serial.println("[SYS] Homing all complete.");
+  homingState = HOME_ALL_PHASE1;
 }
 
 void performHoming1() {
   Serial.println("[S1] Homing Motor 1: Phase 1 (Stall UP)");
   stepper1.move(-2048);
-  while (stepper1.distanceToGo() != 0) {
-    runSteppersWithNotify();
-  }
-  
-  stepper1.setCurrentPosition(474);
-  
-  Serial.println("[S1] Homing Motor 1: Phase 2 (Move Outward)");
-  stepper1.moveTo(1024);
-  while (stepper1.distanceToGo() != 0) {
-    runSteppersWithNotify();
-  }
-  
-  stepper1.disableOutputs();
-  Serial.println("[S1] Homing complete.");
+  homingState = HOME_1_PHASE1;
 }
 
 void performHoming2() {
   Serial.println("[S2] Homing Motor 2: Phase 1 (Stall UP)");
   stepper2.move(2048);
-  while (stepper2.distanceToGo() != 0) {
-    runSteppersWithNotify();
-  }
-  
-  stepper2.setCurrentPosition(530);
-  
-  Serial.println("[S2] Homing Motor 2: Phase 2 (Move Outward)");
-  stepper2.moveTo(0);
-  while (stepper2.distanceToGo() != 0) {
-    runSteppersWithNotify();
-  }
-  
-  stepper2.disableOutputs();
-  Serial.println("[S2] Homing complete.");
+  homingState = HOME_2_PHASE1;
 }