more kinematics

src/main.cpp

@@ -77,6 +77,20 @@ void performHomingAll();
 void performHoming1();
 void performHoming2();
 
+// ─── Homing Routines ──────────────────────────────────────────────
+
+void runSteppersWithNotify() {
+  stepper1.run();
+  stepper2.run();
+  if (bleConnected) {
+    unsigned long now = millis();
+    if (now - lastNotify >= NOTIFY_INTERVAL_MS) {
+      lastNotify = now;
+      sendPosition();
+    }
+  }
+}
+
 // ─── BLE Server Callbacks ─────────────────────────────────────────
 class ServerCallbacks : public BLEServerCallbacks {
   void onConnect(BLEServer *) override {
@@ -247,34 +261,31 @@ void loop() {
 
 void performHomingAll() {
   // 1. Move against limits (M1 CCW, M2 CCW)
-  stepper2.moveTo(1024);
-  stepper1.moveTo(2048);
+  stepper1.moveTo(1024);
+  stepper2.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();
+    runSteppersWithNotify();
   }
   stepper2.setCurrentPosition(0);
 
-  // 3. Move to final home position
-  stepper2.moveTo(550);
-  stepper1.moveTo(-530);
+  // 2. Move to negative limits (CW)
+  stepper1.moveTo(-1050);
+  stepper2.move(-1300);
   while (stepper1.distanceToGo() != 0 || stepper2.distanceToGo() != 0) {
-    stepper1.run();
-    stepper2.run();
+    runSteppersWithNotify();
+  }
+  stepper1.setCurrentPosition(0);
+
+  // 3. Move to final home position
+  stepper1.moveTo(550);
+  stepper2.moveTo(-530);
+  while (stepper1.distanceToGo() != 0 || stepper2.distanceToGo() != 0) {
+    runSteppersWithNotify();
   }
 
   // 4. Set origin relative to this final position
-  stepper2.setCurrentPosition(-1024);
-  stepper1.setCurrentPosition(0);
+  stepper1.setCurrentPosition(-1024);
+  stepper2.setCurrentPosition(0);
 
   stepper1.disableOutputs();
   stepper2.disableOutputs();
@@ -282,37 +293,37 @@ void performHomingAll() {
 }
 
 void performHoming1() {
-  // M1 (Right) stalls going positive (CCW)
-  stepper1.move(2048);
+  // M1 (Left) stalls going negative (CW)
+  stepper1.move(-2048);
   while (stepper1.distanceToGo() != 0) {
-    stepper1.run();
+    runSteppersWithNotify();
   }
   stepper1.setCurrentPosition(0);
 
-  // Move to final home position (CW)
-  stepper1.moveTo(-530);
+  // Move to final home position (CCW)
+  stepper1.moveTo(550);
   while (stepper1.distanceToGo() != 0) {
-    stepper1.run();
+    runSteppersWithNotify();
   }
-  stepper1.setCurrentPosition(0);
+  stepper1.setCurrentPosition(-1024);
   stepper1.disableOutputs();
   Serial.println("[S1] Homing complete.");
 }
 
 void performHoming2() {
-  // M2 (Left) stalls going negative (CW)
-  stepper2.move(-2048);
+  // M2 (Right) stalls going positive (CCW)
+  stepper2.move(2048);
   while (stepper2.distanceToGo() != 0) {
-    stepper2.run();
+    runSteppersWithNotify();
   }
   stepper2.setCurrentPosition(0);
 
-  // Move to final home position (CCW)
-  stepper2.moveTo(550);
+  // Move to final home position (CW)
+  stepper2.moveTo(-530);
   while (stepper2.distanceToGo() != 0) {
-    stepper2.run();
+    runSteppersWithNotify();
   }
-  stepper2.setCurrentPosition(-1024);
+  stepper2.setCurrentPosition(0);
   stepper2.disableOutputs();
   Serial.println("[S2] Homing complete.");
 }