mcu: Change mcu_stepper.set_position() to take a location in millimeters

Update the set_position() method to convert from millimeters to
absolute step position.

Also, update PrinterStepper.get_homed_offset() and
mcu_stepper.get_commanded_position() to return millimeters.

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>

5 files changed, 16 insertions, 21 deletions

diff --git a/klippy/cartesian.py b/klippy/cartesian.py
index 6ec12a5b..c1e07a84 100644
--- a/klippy/cartesian.py
+++ b/klippy/cartesian.py
@@ -23,8 +23,7 @@ class CartKinematics:
         self.steppers[2].set_max_jerk(0., self.max_z_accel)
     def set_position(self, newpos):
         for i in StepList:
-            s = self.steppers[i]
-            s.mcu_stepper.set_position(int(newpos[i]*s.inv_step_dist + 0.5))
+            self.steppers[i].mcu_stepper.set_position(newpos[i])
     def home(self, homing_state):
         # Each axis is homed independently and in order
         for axis in homing_state.get_axes():
@@ -55,7 +54,7 @@ class CartKinematics:
             homing_state.home(
                 list(coord), homepos, [s], s.homing_speed/2.0, second_home=True)
             # Set final homed position
-            coord[axis] = s.position_endstop + s.get_homed_offset()*s.step_dist
+            coord[axis] = s.position_endstop + s.get_homed_offset()
             homing_state.set_homed_position(coord)
     def motor_off(self, move_time):
         self.limits = [(1.0, -1.0)] * 3
diff --git a/klippy/corexy.py b/klippy/corexy.py
index e9454a2c..b724a4e1 100644
--- a/klippy/corexy.py
+++ b/klippy/corexy.py
@@ -26,12 +26,7 @@ class CoreXYKinematics:
     def set_position(self, newpos):
         pos = (newpos[0] + newpos[1], newpos[0] - newpos[1], newpos[2])
         for i in StepList:
-            s = self.steppers[i]
-            if pos[i] >= 0.:
-                steppos = int(pos[i]*s.inv_step_dist + 0.5)
-            else:
-                steppos = int(pos[i]*s.inv_step_dist - 0.5)
-            s.mcu_stepper.set_position(steppos)
+            self.steppers[i].mcu_stepper.set_position(pos[i])
     def home(self, homing_state):
         # Each axis is homed independently and in order
         for axis in homing_state.get_axes():
@@ -63,8 +58,7 @@ class CoreXYKinematics:
                 list(coord), homepos, [s], s.homing_speed/2.0, second_home=True)
             if axis == 2:
                 # Support endstop phase detection on Z axis
-                coord[axis] = (s.position_endstop
-                               + s.get_homed_offset()*s.step_dist)
+                coord[axis] = s.position_endstop + s.get_homed_offset()
                 homing_state.set_homed_position(coord)
     def motor_off(self, move_time):
         self.limits = [(1.0, -1.0)] * 3
diff --git a/klippy/delta.py b/klippy/delta.py
index e54fe340..9ae72810 100644
--- a/klippy/delta.py
+++ b/klippy/delta.py
@@ -59,10 +59,9 @@ class DeltaKinematics:
         for stepper in self.steppers:
             stepper.set_max_jerk(max_xy_halt_velocity, max_accel)
     def _cartesian_to_actuator(self, coord):
-        return [int((math.sqrt(self.arm_length2
-                               - (self.towers[i][0] - coord[0])**2
-                               - (self.towers[i][1] - coord[1])**2) + coord[2])
-                    * self.steppers[i].inv_step_dist + 0.5)
+        return [math.sqrt(self.arm_length2
+                          - (self.towers[i][0] - coord[0])**2
+                          - (self.towers[i][1] - coord[1])**2) + coord[2]
                 for i in StepList]
     def _actuator_to_cartesian(self, pos):
         # Based on code from Smoothieware
@@ -118,8 +117,7 @@ class DeltaKinematics:
         homing_state.home(list(coord), homepos, self.steppers
                           , s.homing_speed/2.0, second_home=True)
         # Set final homed position
-        coord = [(s.mcu_stepper.get_commanded_position() + s.get_homed_offset())
-                 * s.step_dist
+        coord = [s.mcu_stepper.get_commanded_position() + s.get_homed_offset()
                  for s in self.steppers]
         homing_state.set_homed_position(self._actuator_to_cartesian(coord))
     def motor_off(self, move_time):
diff --git a/klippy/mcu.py b/klippy/mcu.py
index 080c71ac..94e94d63 100644
--- a/klippy/mcu.py
+++ b/klippy/mcu.py
@@ -69,10 +69,14 @@ class MCU_stepper:
     def get_oid(self):
         return self._oid
     def set_position(self, pos):
-        self._mcu_position_offset += self._commanded_pos - pos
-        self._commanded_pos = pos
+        if pos >= 0.:
+            steppos = int(pos * self._inv_step_dist + 0.5)
+        else:
+            steppos = int(pos * self._inv_step_dist - 0.5)
+        self._mcu_position_offset += self._commanded_pos - steppos
+        self._commanded_pos = steppos
     def get_commanded_position(self):
-        return self._commanded_pos
+        return self._commanded_pos * self._step_dist
     def get_mcu_position(self):
         return self._commanded_pos + self._mcu_position_offset
     def note_homing_start(self, homing_clock):
diff --git a/klippy/stepper.py b/klippy/stepper.py
index a5a10c7a..5bdcb374 100644
--- a/klippy/stepper.py
+++ b/klippy/stepper.py
@@ -99,4 +99,4 @@ class PrinterStepper:
             raise homing.EndstopError(
                 "Endstop %s incorrect phase (got %d vs %d)" % (
                     self.name, pos, self.homing_endstop_phase))
-        return delta
+        return delta * self.step_dist