Merge remote-tracking branch 'upstream/edge' into use-pendsv-handler-for-ticker

diff --git a/src/modules/robot/Robot.cpp b/src/modules/robot/Robot.cpp
index 6cc8375..8397039 100644 (file)
--- a/src/modules/robot/Robot.cpp
+++ b/src/modules/robot/Robot.cpp
@@ -622,6 +622,11 @@ void Robot::reset_position_from_current_actuator_position()
     float actuator_pos[]= {actuators[X_AXIS]->get_current_position(), actuators[Y_AXIS]->get_current_position(), actuators[Z_AXIS]->get_current_position()};
     arm_solution->actuator_to_cartesian(actuator_pos, this->last_milestone);
     memcpy(this->transformed_last_milestone, this->last_milestone, sizeof(this->transformed_last_milestone));
+
+    // now reset actuator correctly, NOTE this may lose a little precision
+    arm_solution->cartesian_to_actuator(this->last_milestone, actuator_pos);
+    for (int i = 0; i < 3; i++)
+        actuators[i]->change_last_milestone(actuator_pos[i]);
 }
 
 // Convert target from millimeters to steps, and append this to the planner