split off the M206/M306 for rotary delta into its own calibration module

diff --git a/ConfigSamples/rotary.delta/config b/ConfigSamples/rotary.delta/config
index fa8432d..671db5c 100644 (file)
--- a/ConfigSamples/rotary.delta/config
+++ b/ConfigSamples/rotary.delta/config
@@ -23,6 +23,8 @@ delta_tool_offset 30.500       # Distance between end effector ball joint plane
 
 delta_mirror_xy   true         # true for firepick
 
+rotary_delta_calibration.enable  true  # enable the calibration routines for rotary delta
+
 #The steps per degree are calculated as:
 #
 # (1) First determine the circumference of the big pulley, which has a smooth surface, and calculate the 'teeth it would have'

diff --git a/src/modules/tools/endstops/Endstops.cpp b/src/modules/tools/endstops/Endstops.cpp
index 6cde60b..4cd6cf5 100644 (file)
--- a/src/modules/tools/endstops/Endstops.cpp
+++ b/src/modules/tools/endstops/Endstops.cpp
@@ -142,11 +142,11 @@ void Endstops::on_module_loaded()
     THEKERNEL->step_ticker->register_acceleration_tick_handler([this]() {acceleration_tick(); });
 
     // Settings
-    this->on_config_reload(this);
+    this->load_config();
 }
 
 // Get config
-void Endstops::on_config_reload(void *argument)
+void Endstops::load_config()
 {
     this->pins[0].from_string( THEKERNEL->config->value(alpha_min_endstop_checksum          )->by_default("nc" )->as_string())->as_input();
     this->pins[1].from_string( THEKERNEL->config->value(beta_min_endstop_checksum           )->by_default("nc" )->as_string())->as_input();
@@ -791,79 +791,23 @@ void Endstops::process_home_command(Gcode* gcode)
 
 void Endstops::set_homing_offset(Gcode *gcode)
 {
-    if(!is_rdelta) {
-        // Similar to M206 and G92 but sets Homing offsets based on current position
-        float cartesian[3];
-        THEKERNEL->robot->get_axis_position(cartesian);    // get actual position from robot
-        if (gcode->has_letter('X')) {
-            home_offset[0] -= (cartesian[X_AXIS] - gcode->get_value('X'));
-            THEKERNEL->robot->reset_axis_position(gcode->get_value('X'), X_AXIS);
-        }
-        if (gcode->has_letter('Y')) {
-            home_offset[1] -= (cartesian[Y_AXIS] - gcode->get_value('Y'));
-            THEKERNEL->robot->reset_axis_position(gcode->get_value('Y'), Y_AXIS);
-        }
-        if (gcode->has_letter('Z')) {
-            home_offset[2] -= (cartesian[Z_AXIS] - gcode->get_value('Z'));
-            THEKERNEL->robot->reset_axis_position(gcode->get_value('Z'), Z_AXIS);
-        }
-
-        gcode->stream->printf("Homing Offset: X %5.3f Y %5.3f Z %5.3f\n", home_offset[0], home_offset[1], home_offset[2]);
-
-    } else {
-        // for a rotary delta M306 calibrates the homing angle
-        // by doing M306 A-56.17 it will calculate the M206 A value (the theta offset for actuator A) based on the difference
-        // between what it thinks is the current angle and what the current angle actually is specified by A (ditto for B and C)
-
-        ActuatorCoordinates current_angle;
-        // get the current angle for each actuator, NOTE we only deal with  ABC so if there are more than 3 actuators this will probably go wonky
-        for (size_t i = 0; i < THEKERNEL->robot->actuators.size(); i++) {
-            current_angle[i]= THEKERNEL->robot->actuators[i]->get_current_position();
-        }
-
-        if (gcode->has_letter('L') && gcode->get_value('L') != 0) {
-            // specifying L1 it will take the last probe position (set by G30 or G38.x ) and set the home offset based on that
-            // this allows the use of G30 to find the angle tool
-            uint8_t ok;
-            std::tie(current_angle[0], current_angle[1], current_angle[2], ok) = THEKERNEL->robot->get_last_probe_position();
-            if(ok == 0) {
-                gcode->stream->printf("error:Nothing set as probe failed or not run\n");
-                return;
-            }
-        }
-
-        int cnt= 0;
-        //figure out what home_offset needs to be to correct the homing_position
-        if (gcode->has_letter('A')) {
-            float a = gcode->get_value('A'); // what actual angle is
-            home_offset[0] -= (current_angle[0] - a);
-            current_angle[0]= a;
-            cnt++;
-        }
-        if (gcode->has_letter('B')) {
-            float b = gcode->get_value('B');
-            home_offset[1] -= (current_angle[1] - b);
-            current_angle[1]= b;
-            cnt++;
-        }
-        if (gcode->has_letter('C')) {
-            float c = gcode->get_value('C');
-            home_offset[2] -= (current_angle[2] - c);
-            current_angle[2]= c;
-            cnt++;
-        }
-
-        // reset the actuator positions (and machine position accordingly)
-        // But only if all three actuators have been specified at the same time
-        if(cnt == 3 || (gcode->has_letter('R') && gcode->get_value('R') != 0)) {
-            THEKERNEL->robot->reset_actuator_position(current_angle);
-            gcode->stream->printf("NOTE: actuator position reset\n");
-        }else{
-            gcode->stream->printf("NOTE: actuator position NOT reset\n");
-        }
-
-        gcode->stream->printf("Theta Offset: A %8.5f B %8.5f C %8.5f\n", home_offset[0], home_offset[1], home_offset[2]);
+    // Similar to M206 and G92 but sets Homing offsets based on current position
+    float cartesian[3];
+    THEKERNEL->robot->get_axis_position(cartesian);    // get actual position from robot
+    if (gcode->has_letter('X')) {
+        home_offset[0] -= (cartesian[X_AXIS] - gcode->get_value('X'));
+        THEKERNEL->robot->reset_axis_position(gcode->get_value('X'), X_AXIS);
     }
+    if (gcode->has_letter('Y')) {
+        home_offset[1] -= (cartesian[Y_AXIS] - gcode->get_value('Y'));
+        THEKERNEL->robot->reset_axis_position(gcode->get_value('Y'), Y_AXIS);
+    }
+    if (gcode->has_letter('Z')) {
+        home_offset[2] -= (cartesian[Z_AXIS] - gcode->get_value('Z'));
+        THEKERNEL->robot->reset_axis_position(gcode->get_value('Z'), Z_AXIS);
+    }
+
+    gcode->stream->printf("Homing Offset: X %5.3f Y %5.3f Z %5.3f\n", home_offset[0], home_offset[1], home_offset[2]);
 }
 
 // Start homing sequences by response to GCode commands
@@ -887,22 +831,16 @@ void Endstops::on_gcode_received(void *argument)
             break;
 
             case 206: // M206 - set homing offset
-                if(!is_rdelta) {
-                    if (gcode->has_letter('X')) home_offset[0] = gcode->get_value('X');
-                    if (gcode->has_letter('Y')) home_offset[1] = gcode->get_value('Y');
-                    if (gcode->has_letter('Z')) home_offset[2] = gcode->get_value('Z');
-                    gcode->stream->printf("X %5.3f Y %5.3f Z %5.3f\n", home_offset[0], home_offset[1], home_offset[2]);
+                if(is_rdelta) break; // RotaryDeltaCalibration module will handle this
 
-                } else {
-                    // set theta offset
-                    if (gcode->has_letter('A')) home_offset[0] = gcode->get_value('A');
-                    if (gcode->has_letter('B')) home_offset[1] = gcode->get_value('B');
-                    if (gcode->has_letter('C')) home_offset[2] = gcode->get_value('C');
-                    gcode->stream->printf("Theta offset A %8.5f B %8.5f C %8.5f\n", home_offset[0], home_offset[1], home_offset[2]);
-                }
+                if (gcode->has_letter('X')) home_offset[0] = gcode->get_value('X');
+                if (gcode->has_letter('Y')) home_offset[1] = gcode->get_value('Y');
+                if (gcode->has_letter('Z')) home_offset[2] = gcode->get_value('Z');
+                gcode->stream->printf("X %5.3f Y %5.3f Z %5.3f\n", home_offset[0], home_offset[1], home_offset[2]);
                 break;
 
             case 306: // set homing offset based on current position
+                if(is_rdelta) break; // RotaryDeltaCalibration module will handle this
                 set_homing_offset(gcode);
                 break;
 

diff --git a/src/modules/tools/endstops/Endstops.h b/src/modules/tools/endstops/Endstops.h
index 125534a..96173c4 100644 (file)
--- a/src/modules/tools/endstops/Endstops.h
+++ b/src/modules/tools/endstops/Endstops.h
@@ -21,10 +21,10 @@ class Endstops : public Module{
         Endstops();
         void on_module_loaded();
         void on_gcode_received(void* argument);
-        void on_config_reload(void* argument);
         void acceleration_tick(void);
 
     private:
+        void load_config();
         void home(char axes_to_move);
         void do_homing_cartesian(char axes_to_move);
         void do_homing_corexy(char axes_to_move);

diff --git a/src/modules/tools/rotarydeltacalibration/RotaryDeltaCalibration.cpp b/src/modules/tools/rotarydeltacalibration/RotaryDeltaCalibration.cpp
new file mode 100644 (file)
index 0000000..610fd2f
--- /dev/null
+++ b/src/modules/tools/rotarydeltacalibration/RotaryDeltaCalibration.cpp
@@ -0,0 +1,118 @@
+#include "RotaryDeltaCalibration.h"
+#include "EndstopsPublicAccess.h"
+#include "Kernel.h"
+#include "Robot.h"
+#include "Config.h"
+#include "checksumm.h"
+#include "ConfigValue.h"
+#include "StreamOutput.h"
+#include "PublicDataRequest.h"
+#include "PublicData.h"
+#include "Gcode.h"
+#include "StepperMotor.h"
+
+#define rotarydelta_checksum CHECKSUM("rotary_delta_calibration")
+#define enable_checksum CHECKSUM("enable")
+
+void RotaryDeltaCalibration::on_module_loaded()
+{
+    // if the module is disabled -> do nothing
+    if(!THEKERNEL->config->value( rotarydelta_checksum, enable_checksum )->by_default(false)->as_bool()) {
+        // as this module is not needed free up the resource
+        delete this;
+        return;
+    }
+
+    // register event-handlers
+    register_for_event(ON_GCODE_RECEIVED);
+}
+
+void RotaryDeltaCalibration::on_gcode_received(void *argument)
+{
+    Gcode *gcode = static_cast<Gcode *>(argument);
+
+    if( gcode->has_m) {
+        switch( gcode->m ) {
+            case 206: {
+                float *theta_offset; // points to theta offset in Endstop module
+                bool ok = PublicData::get_value( endstops_checksum, home_offset_checksum, &theta_offset );
+                if (!ok) {
+                    gcode->stream->printf("error:no endstop module found\n");
+                    return;
+                }
+
+                // set theta offset, set directly in the Endstop module (bad practice really)
+                if (gcode->has_letter('A')) theta_offset[0] = gcode->get_value('A');
+                if (gcode->has_letter('B')) theta_offset[1] = gcode->get_value('B');
+                if (gcode->has_letter('C')) theta_offset[2] = gcode->get_value('C');
+
+                gcode->stream->printf("Theta offset set: A %8.5f B %8.5f C %8.5f\n", theta_offset[0], theta_offset[1], theta_offset[2]);
+
+                break;
+            }
+
+            case 306: {
+                // for a rotary delta M306 calibrates the homing angle
+                // by doing M306 A-56.17 it will calculate the M206 A value (the theta offset for actuator A) based on the difference
+                // between what it thinks is the current angle and what the current angle actually is specified by A (ditto for B and C)
+
+                ActuatorCoordinates current_angle;
+                // get the current angle for each actuator, NOTE we only deal with  ABC so if there are more than 3 actuators this will probably go wonky
+                for (size_t i = 0; i < THEKERNEL->robot->actuators.size(); i++) {
+                    current_angle[i]= THEKERNEL->robot->actuators[i]->get_current_position();
+                }
+
+                if (gcode->has_letter('L') && gcode->get_value('L') != 0) {
+                    // specifying L1 it will take the last probe position (set by G30 or G38.x ) and set the home offset based on that
+                    // this allows the use of G30 to find the angle tool
+                    uint8_t ok;
+                    std::tie(current_angle[0], current_angle[1], current_angle[2], ok) = THEKERNEL->robot->get_last_probe_position();
+                    if(ok == 0) {
+                        gcode->stream->printf("error:Nothing set as probe failed or not run\n");
+                        return;
+                    }
+                }
+
+                float *theta_offset; // points to theta offset in Endstop module
+                bool ok = PublicData::get_value( endstops_checksum, home_offset_checksum, &theta_offset );
+                if (!ok) {
+                    gcode->stream->printf("error:no endstop module found\n");
+                    return;
+                }
+
+                int cnt= 0;
+
+                //figure out what home_offset needs to be to correct the homing_position
+                if (gcode->has_letter('A')) {
+                    float a = gcode->get_value('A'); // what actual angle is
+                    theta_offset[0] -= (current_angle[0] - a);
+                    current_angle[0]= a;
+                    cnt++;
+                }
+                if (gcode->has_letter('B')) {
+                    float b = gcode->get_value('B');
+                    theta_offset[1] -= (current_angle[1] - b);
+                    current_angle[1]= b;
+                    cnt++;
+                }
+                if (gcode->has_letter('C')) {
+                    float c = gcode->get_value('C');
+                    theta_offset[2] -= (current_angle[2] - c);
+                    current_angle[2]= c;
+                    cnt++;
+                }
+
+                // reset the actuator positions (and machine position accordingly)
+                // But only if all three actuators have been specified at the same time
+                if(cnt == 3 || (gcode->has_letter('R') && gcode->get_value('R') != 0)) {
+                    THEKERNEL->robot->reset_actuator_position(current_angle);
+                    gcode->stream->printf("NOTE: actuator position reset\n");
+                }else{
+                    gcode->stream->printf("NOTE: actuator position NOT reset\n");
+                }
+
+                gcode->stream->printf("Theta Offset: A %8.5f B %8.5f C %8.5f\n", theta_offset[0], theta_offset[1], theta_offset[2]);
+            }
+        }
+    }
+}

diff --git a/src/modules/tools/rotarydeltacalibration/RotaryDeltaCalibration.h b/src/modules/tools/rotarydeltacalibration/RotaryDeltaCalibration.h
new file mode 100644 (file)
index 0000000..90f2ec3
--- /dev/null
+++ b/src/modules/tools/rotarydeltacalibration/RotaryDeltaCalibration.h
@@ -0,0 +1,18 @@
+#pragma once
+
+#include "Module.h"
+
+class Gcode;
+
+class RotaryDeltaCalibration : public Module
+{
+public:
+    RotaryDeltaCalibration(){};
+    virtual ~RotaryDeltaCalibration(){};
+
+    void on_module_loaded();
+
+private:
+    void on_gcode_received(void *argument);
+
+};