Add M665 to adjust arm length and arm radius for kossel arm solution

Add M666 as replacement for M206
Add M665 Znnn to adjust gamma_max for delta Z

diff --git a/src/modules/robot/Robot.cpp b/src/modules/robot/Robot.cpp
index c0c2ca3..d876e5c 100644 (file)
--- a/src/modules/robot/Robot.cpp
+++ b/src/modules/robot/Robot.cpp
@@ -236,6 +236,26 @@ void Robot::on_gcode_received(void * argument){
                     seconds_per_minute = factor * 0.6;
                 }
                 break;
+
+            case 665: // M665 set optional arm solution variables based on arm solution
+                gcode->mark_as_taken();
+                // the parameter args could be any letter so try each one
+                for(char c='A';c<='Z';c++) {
+                    double v;
+                    bool supported= arm_solution->get_optional(c, &v); // retrieve current value if supported
+
+                    if(supported && gcode->has_letter(c)) { // set new value if supported
+                        v= gcode->get_value(c);
+                        arm_solution->set_optional(c, v);
+                    }
+                    if(supported) { // print all current values of supported options
+                        char buf[16];
+                        int n= snprintf(buf, sizeof(buf), "%c:%8.3f ", c, v);
+                        gcode->txt_after_ok.append(buf, n);
+                    }
+                }
+                break;
+
         }
    }
     if( this->motion_mode < 0)

diff --git a/src/modules/robot/arm_solutions/BaseSolution.h b/src/modules/robot/arm_solutions/BaseSolution.h
index 802d366..ebf29f1 100644 (file)
--- a/src/modules/robot/arm_solutions/BaseSolution.h
+++ b/src/modules/robot/arm_solutions/BaseSolution.h
@@ -11,6 +11,9 @@ class BaseSolution {
 
         virtual void set_steps_per_millimeter( double steps[] ) = 0;
         virtual void get_steps_per_millimeter( double steps[] ) = 0;
+
+        virtual bool set_optional(char parameter, double value) { return false; };
+        virtual bool get_optional(char parameter, double *value) { return false; };
 };
 
 #endif

diff --git a/src/modules/robot/arm_solutions/JohannKosselSolution.cpp b/src/modules/robot/arm_solutions/JohannKosselSolution.cpp
index 9433d29..0fa8283 100644 (file)
--- a/src/modules/robot/arm_solutions/JohannKosselSolution.cpp
+++ b/src/modules/robot/arm_solutions/JohannKosselSolution.cpp
@@ -2,6 +2,7 @@
 #include <fastmath.h>
 
 #define PIOVER180       0.01745329251994329576923690768489F
+#define SQ(x) powf(x, 2)
 
 JohannKosselSolution::JohannKosselSolution(Config* passed_config) : config(passed_config){
     this->alpha_steps_per_mm = this->config->value(alpha_steps_per_mm_checksum)->as_number();
@@ -13,8 +14,12 @@ JohannKosselSolution::JohannKosselSolution(Config* passed_config) : config(passe
     // arm_radius is the horizontal distance from hinge to hinge when the effector is centered
     this->arm_radius         = this->config->value(arm_radius_checksum)->by_default(124.0)->as_number();
 
-    this->arm_length_squared = powf(this->arm_length, 2);
-    
+    init();
+}
+
+void JohannKosselSolution::init() {
+    this->arm_length_squared = SQ(this->arm_length);
+
     // Effective X/Y positions of the three vertical towers.
     float DELTA_RADIUS= this->arm_radius;
     float SIN_60= 0.8660254037844386F;
@@ -27,8 +32,6 @@ JohannKosselSolution::JohannKosselSolution(Config* passed_config) : config(passe
     DELTA_TOWER3_Y= DELTA_RADIUS;
 }
 
-#define sq(x) powf((x), 2)
-
 void JohannKosselSolution::millimeters_to_steps( double millimeters[], int steps[] ){
     float cartesian[3];
     // convert input to float
@@ -37,25 +40,25 @@ void JohannKosselSolution::millimeters_to_steps( double millimeters[], int steps
     cartesian[2]= millimeters[2];
 
     float delta_x = sqrtf(this->arm_length_squared
-                         - sq(DELTA_TOWER1_X-cartesian[0])
-                         - sq(DELTA_TOWER1_Y-cartesian[1])
+                         - SQ(DELTA_TOWER1_X-cartesian[0])
+                         - SQ(DELTA_TOWER1_Y-cartesian[1])
                         ) + cartesian[2];
     float delta_y = sqrtf(this->arm_length_squared
-                         - sq(DELTA_TOWER2_X-cartesian[0])
-                         - sq(DELTA_TOWER2_Y-cartesian[1])
+                         - SQ(DELTA_TOWER2_X-cartesian[0])
+                         - SQ(DELTA_TOWER2_Y-cartesian[1])
                         ) + cartesian[2];
     float delta_z = sqrtf(this->arm_length_squared
-                         - sq(DELTA_TOWER3_X-cartesian[0])
-                         - sq(DELTA_TOWER3_Y-cartesian[1])
+                         - SQ(DELTA_TOWER3_X-cartesian[0])
+                         - SQ(DELTA_TOWER3_Y-cartesian[1])
                         ) + cartesian[2];
 
-   
+
     steps[ALPHA_STEPPER] = lround( delta_x * this->alpha_steps_per_mm );
     steps[BETA_STEPPER ] = lround( delta_y * this->beta_steps_per_mm );
     steps[GAMMA_STEPPER] = lround( delta_z * this->gamma_steps_per_mm );
 }
 
-void JohannKosselSolution::steps_to_millimeters( int steps[], double millimeters[] ){} 
+void JohannKosselSolution::steps_to_millimeters( int steps[], double millimeters[] ){}
 
 void JohannKosselSolution::set_steps_per_millimeter( double steps[] )
 {
@@ -71,3 +74,36 @@ void JohannKosselSolution::get_steps_per_millimeter( double steps[] )
     steps[2] = this->gamma_steps_per_mm;
 }
 
+bool JohannKosselSolution::set_optional(char parameter, double value) {
+
+    switch(parameter) {
+        case 'L': // sets arm_length
+            this->arm_length= value;
+            init();
+            break;
+        case 'R': // sets arm_radius
+            this->arm_radius= value;
+            init();
+            break;
+        default:
+            return false;
+    }
+    return true;
+}
+
+bool JohannKosselSolution::get_optional(char parameter, double *value) {
+    if(value == NULL) return false;
+
+    switch(parameter) {
+        case 'L': // get arm_length
+            *value= this->arm_length;
+            break;
+        case 'R': // get arm_radius
+            *value= this->arm_radius;
+            break;
+        default:
+            return false;
+    }
+
+    return true;
+};

diff --git a/src/modules/robot/arm_solutions/JohannKosselSolution.h b/src/modules/robot/arm_solutions/JohannKosselSolution.h
index c77da47..e258d91 100644 (file)
--- a/src/modules/robot/arm_solutions/JohannKosselSolution.h
+++ b/src/modules/robot/arm_solutions/JohannKosselSolution.h
@@ -22,8 +22,12 @@ class JohannKosselSolution : public BaseSolution {
 
         void set_steps_per_millimeter( double steps[] );
         void get_steps_per_millimeter( double steps[] );
+        bool set_optional(char parameter, double value);
+        bool get_optional(char parameter, double *value);
 
     private:
+        void init();
+
         Config* config;
         float alpha_steps_per_mm;
         float beta_steps_per_mm;

diff --git a/src/modules/tools/endstops/Endstops.cpp b/src/modules/tools/endstops/Endstops.cpp
index e96a7a0..25458fd 100644 (file)
--- a/src/modules/tools/endstops/Endstops.cpp
+++ b/src/modules/tools/endstops/Endstops.cpp
@@ -361,7 +361,20 @@ void Endstops::on_gcode_received(void* argument)
                 }
                 break;
 
-            case 206: // M206 - set trim for each axis in mm
+            case 665: { // M665 - set max gamma/z height
+                    gcode->mark_as_taken();
+                    double gamma_max= this->homing_position[2];
+                    if(gcode->has_letter('Z')) {
+                        this->homing_position[2]= gamma_max= gcode->get_value('Z');
+                    }
+                    char buf[16];
+                    int n= snprintf(buf, sizeof(buf), "Max Z:%8.3f ", gamma_max);
+                    gcode->txt_after_ok.append(buf, n);
+                }
+                break;
+
+            case 206: // M206 - set trim for each axis in mm (TODO to be deprecated)
+            case 666: // M666 - set trim for each axis in mm
                 {
                     int dirx= (this->home_direction[0] ? 1 : -1);
                     int diry= (this->home_direction[1] ? 1 : -1);