add M1910.2 to move by the specified number of actuator units

add reset_actuator_position() to to allow manula homing and setting actuator specific positions
add G28.4 to allow manual setting of homing to actuator units
(optimize set lround to lroundf wherever it was used)

diff --git a/src/libs/StepperMotor.cpp b/src/libs/StepperMotor.cpp
index 83cbbfe..013f0f4 100644 (file)
--- a/src/libs/StepperMotor.cpp
+++ b/src/libs/StepperMotor.cpp
@@ -197,19 +197,19 @@ StepperMotor* StepperMotor::set_speed( float speed )
 void StepperMotor::change_steps_per_mm(float new_steps)
 {
     steps_per_mm = new_steps;
-    last_milestone_steps = lround(last_milestone_mm * steps_per_mm);
+    last_milestone_steps = lroundf(last_milestone_mm * steps_per_mm);
     current_position_steps = last_milestone_steps;
 }
 
 void StepperMotor::change_last_milestone(float new_milestone)
 {
     last_milestone_mm = new_milestone;
-    last_milestone_steps = lround(last_milestone_mm * steps_per_mm);
+    last_milestone_steps = lroundf(last_milestone_mm * steps_per_mm);
     current_position_steps = last_milestone_steps;
 }
 
 int  StepperMotor::steps_to_target(float target)
 {
-    int target_steps = lround(target * steps_per_mm);
+    int target_steps = lroundf(target * steps_per_mm);
     return target_steps - last_milestone_steps;
 }

diff --git a/src/modules/robot/Robot.cpp b/src/modules/robot/Robot.cpp
index 0bc9667..f399bb0 100644 (file)
--- a/src/modules/robot/Robot.cpp
+++ b/src/modules/robot/Robot.cpp
@@ -767,6 +767,19 @@ void Robot::reset_axis_position(float position, int axis)
     reset_axis_position(last_milestone[X_AXIS], last_milestone[Y_AXIS], last_milestone[Z_AXIS]);
 }
 
+// similar to reset_actuator_position but directly sets the actuator positions in actuators units (eg mm for cartesian, degrees for rotary delta)
+// then sets the axis postions to match. currently only called from G28.4 in Endstops.cpp
+void Robot::reset_actuator_position(float a, float b, float c)
+{
+    // NOTE this does NOT support the multiple actuator HACK. so if there are more than 3 actuators this will probably not work
+    if(!isnan(a)) actuators[0]->change_last_milestone(a);
+    if(!isnan(b)) actuators[1]->change_last_milestone(b);
+    if(!isnan(c)) actuators[2]->change_last_milestone(c);
+
+    // now correct axis positions then recorrect actuator to account for rounding
+    reset_position_from_current_actuator_position();
+}
+
 // Use FK to find out where actuator is and reset to match
 void Robot::reset_position_from_current_actuator_position()
 {
@@ -783,7 +796,7 @@ void Robot::reset_position_from_current_actuator_position()
 
     // now reset actuator::last_milestone, NOTE this may lose a little precision as FK is not always entirely accurate.
     // NOTE This is required to sync the machine position with the actuator position, we do a somewhat redundant cartesian_to_actuator() call
-    // to get everything in perfect sync.
+    // to get everything in perfect sync. **This IS required as there is rounding to the next step as steps are integer**
     arm_solution->cartesian_to_actuator(last_machine_position, actuator_pos);
     for (size_t i = 0; i < actuators.size(); i++)
         actuators[i]->change_last_milestone(actuator_pos[i]);

diff --git a/src/modules/robot/Robot.h b/src/modules/robot/Robot.h
index fb60ca8..5993cf0 100644 (file)
--- a/src/modules/robot/Robot.h
+++ b/src/modules/robot/Robot.h
@@ -33,6 +33,7 @@ class Robot : public Module {
 
         void reset_axis_position(float position, int axis);
         void reset_axis_position(float x, float y, float z);
+        void reset_actuator_position(float a, float b, float c);
         void reset_position_from_current_actuator_position();
         float get_seconds_per_minute() const { return seconds_per_minute; }
         float get_z_maxfeedrate() const { return this->max_speeds[2]; }

diff --git a/src/modules/tools/endstops/Endstops.cpp b/src/modules/tools/endstops/Endstops.cpp
index 4273a7e..234f2ad 100644 (file)
--- a/src/modules/tools/endstops/Endstops.cpp
+++ b/src/modules/tools/endstops/Endstops.cpp
@@ -642,13 +642,22 @@ void Endstops::on_gcode_received(void *argument)
             if(gcode->get_num_args() == 0) {
                 THEKERNEL->robot->reset_axis_position(0, 0, 0);
             } else {
-                // do a manual homing based on current position, no endstops required
+                // do a manual homing based on given coordinates, no endstops required
                 if(gcode->has_letter('X')) THEKERNEL->robot->reset_axis_position(gcode->get_value('X'), X_AXIS);
                 if(gcode->has_letter('Y')) THEKERNEL->robot->reset_axis_position(gcode->get_value('Y'), Y_AXIS);
                 if(gcode->has_letter('Z')) THEKERNEL->robot->reset_axis_position(gcode->get_value('Z'), Z_AXIS);
             }
             return;
 
+       } else if(gcode->subcode == 4) { // G28.4 is a smoothie special it sets manual homing based on the actuator position (used for rotary delta)
+            // do a manual homing based on given coordinates, no endstops required
+            float a=NAN, b=NAN, c=NAN;
+            if(gcode->has_letter('A')) a=  gcode->get_value('A');
+            if(gcode->has_letter('B')) b=  gcode->get_value('B');
+            if(gcode->has_letter('C')) c=  gcode->get_value('C');
+            THEKERNEL->robot->reset_actuator_position(a, b, c);
+            return;
+
         } else if(THEKERNEL->is_grbl_mode()) {
             gcode->stream->printf("error:Unsupported command\n");
             return;
@@ -831,26 +840,33 @@ void Endstops::on_gcode_received(void *argument)
                 break;
 
             // NOTE this is to test accuracy of lead screws etc.
-            case 1910: { // M1910 - move specific number of raw steps
-                if(gcode->subcode == 0) {
+            case 1910: {
+                // M1910.0 - move specific number of raw steps
+                // M1910.1 - stop any moves
+                // M1910.2 - move specific number of actuator coordinates (usually mm but is degrees for a rotary delta)
+                if(gcode->subcode == 0 || gcode->subcode == 2) {
                     // Enable the motors
                     THEKERNEL->stepper->turn_enable_pins_on();
 
-                    int x = 0, y = 0 , z = 0, f = 200 * 16;
+                    int32_t x = 0, y = 0 , z = 0, f = 200 * 16;
                     if (gcode->has_letter('F')) f = gcode->get_value('F');
+
                     if (gcode->has_letter('X')) {
                         x = gcode->get_value('X');
+                        if(gcode->subcode == 2) x= lroundf(x * STEPS_PER_MM(X_AXIS));
                         STEPPER[X_AXIS]->move(x < 0, abs(x), f);
                     }
                     if (gcode->has_letter('Y')) {
                         y = gcode->get_value('Y');
+                        if(gcode->subcode == 2) y= lroundf(y * STEPS_PER_MM(Y_AXIS));
                         STEPPER[Y_AXIS]->move(y < 0, abs(y), f);
                     }
                     if (gcode->has_letter('Z')) {
                         z = gcode->get_value('Z');
+                        if(gcode->subcode == 2) z= lroundf(z * STEPS_PER_MM(Z_AXIS));
                         STEPPER[Z_AXIS]->move(z < 0, abs(z), f);
                     }
-                    gcode->stream->printf("Moving X %d Y %d Z %d steps at F %d steps/sec\n", x, y, z, f);
+                    gcode->stream->printf("Moving X %ld Y %ld Z %ld steps at F %ld steps/sec\n", x, y, z, f);
 
                 } else if(gcode->subcode == 1) {
                     // stop any that are moving

diff --git a/src/modules/utils/panel/screens/WatchScreen.cpp b/src/modules/utils/panel/screens/WatchScreen.cpp
index 10fba78..f1ecc7d 100644 (file)
--- a/src/modules/utils/panel/screens/WatchScreen.cpp
+++ b/src/modules/utils/panel/screens/WatchScreen.cpp
@@ -65,7 +65,7 @@ void WatchScreen::on_enter()
     get_current_status();
     get_current_pos(this->pos);
     get_sd_play_info();
-    this->current_speed = lround(get_current_speed());
+    this->current_speed = lroundf(get_current_speed());
     this->refresh_screen(false);
     THEPANEL->enter_control_mode(1, 0.5);
     THEPANEL->set_control_value(this->current_speed);
@@ -122,7 +122,7 @@ void WatchScreen::on_refresh()
             this->speed_changed = false;
         } else if (!this->issue_change_speed) { // change still queued
             // read it in case it was changed via M220
-            this->current_speed = lround(get_current_speed());
+            this->current_speed = lroundf(get_current_speed());
             THEPANEL->set_control_value(this->current_speed);
             THEPANEL->reset_counter();
         }

diff --git a/src/modules/utils/simpleshell/SimpleShell.cpp b/src/modules/utils/simpleshell/SimpleShell.cpp
index e4008fe..ae360d6 100644 (file)
--- a/src/modules/utils/simpleshell/SimpleShell.cpp
+++ b/src/modules/utils/simpleshell/SimpleShell.cpp
@@ -26,6 +26,7 @@
 #include "ToolManagerPublicAccess.h"
 #include "GcodeDispatch.h"
 #include "BaseSolution.h"
+#include "StepperMotor.h"
 
 #include "TemperatureControlPublicAccess.h"
 #include "EndstopsPublicAccess.h"
@@ -710,21 +711,25 @@ void SimpleShell::get_command( string parameters, StreamOutput *stream)
         }
 
         std::vector<float> v= parse_number_list(p.c_str());
-        if(p.empty() || v.size() != 3) {
-            stream->printf("error:usage: get [fk|ik] [-m] x,y,z\n");
+        if(p.empty() || v.size() < 1) {
+            stream->printf("error:usage: get [fk|ik] [-m] x[,y,z]\n");
             return;
         }
 
         float x= v[0];
-        float y= v[1];
-        float z= v[2];
+        float y= (v.size() > 1) ? v[1] : x;
+        float z= (v.size() > 2) ? v[2] : y;
 
         if(what == "fk") {
             // do forward kinematics on the given actuator position and display the cartesian coordinates
             ActuatorCoordinates apos{x, y, z};
             float pos[3];
             THEKERNEL->robot->arm_solution->actuator_to_cartesian(apos, pos);
-            stream->printf("cartesian= X %f, Y %f, Z %f\n", pos[0], pos[1], pos[2]);
+            stream->printf("cartesian= X %f, Y %f, Z %f, Steps= A %lu, B %lu, C %lu\n",
+                pos[0], pos[1], pos[2],
+                lroundf(x*THEKERNEL->robot->actuators[0]->get_steps_per_mm()),
+                lroundf(y*THEKERNEL->robot->actuators[1]->get_steps_per_mm()),
+                lroundf(z*THEKERNEL->robot->actuators[2]->get_steps_per_mm()));
             x= pos[0];
             y= pos[1];
             z= pos[2];