support M92 set axis steps per mm

diff --git a/src/modules/robot/Robot.cpp b/src/modules/robot/Robot.cpp
index 176afb0..21fdf18 100644 (file)
--- a/src/modules/robot/Robot.cpp
+++ b/src/modules/robot/Robot.cpp
@@ -127,7 +127,22 @@ void Robot::execute_gcode(Gcode* gcode){
        }
    }else if( gcode->has_letter('M')){
      switch( (int) gcode->get_value('M') ){
-         case 114: gcode->stream->printf("C: X:%1.3f Y:%1.3f Z:%1.3f\n",
+            case 92: // M92 - set steps per mm
+                double steps[3];
+                this->arm_solution->get_steps_per_millimeter(steps);
+                if (gcode->has_letter('X'))
+                    steps[0] = this->to_millimeters(gcode->get_value('X'));
+                if (gcode->has_letter('Y'))
+                    steps[1] = this->to_millimeters(gcode->get_value('Y'));
+                if (gcode->has_letter('Z'))
+                    steps[2] = this->to_millimeters(gcode->get_value('Z'));
+                this->arm_solution->set_steps_per_millimeter(steps);
+                // update current position in steps
+                this->arm_solution->millimeters_to_steps(this->current_position, this->kernel->planner->position);
+                gcode->stream->printf("X:%g Y:%g Z:%g ", steps[0], steps[1], steps[2]);
+                gcode->add_nl = true;
+                return;
+            case 114: gcode->stream->printf("C: X:%1.3f Y:%1.3f Z:%1.3f\n",
                                                  this->current_position[0],
                                                  this->current_position[1],
                                                  this->current_position[2]);

diff --git a/src/modules/robot/arm_solutions/BaseSolution.h b/src/modules/robot/arm_solutions/BaseSolution.h
index acb068b..fb6602f 100644 (file)
--- a/src/modules/robot/arm_solutions/BaseSolution.h
+++ b/src/modules/robot/arm_solutions/BaseSolution.h
@@ -7,6 +7,9 @@ class BaseSolution {
         BaseSolution();
         virtual void millimeters_to_steps( double millimeters[], int steps[] ) = 0;
         virtual void steps_to_millimeters( int steps[], double millimeters[] ) = 0;
+
+        virtual void set_steps_per_millimeter( double steps[] ) = 0;
+        virtual void get_steps_per_millimeter( double steps[] ) = 0;
 };
 
 #endif

diff --git a/src/modules/robot/arm_solutions/CartesianSolution.cpp b/src/modules/robot/arm_solutions/CartesianSolution.cpp
index f903d8d..3c61f5f 100644 (file)
--- a/src/modules/robot/arm_solutions/CartesianSolution.cpp
+++ b/src/modules/robot/arm_solutions/CartesianSolution.cpp
@@ -18,3 +18,17 @@ void CartesianSolution::steps_to_millimeters( int steps[], double millimeters[]
     millimeters[BETA_STEPPER ] = steps[Y_AXIS] / this->beta_steps_per_mm;
     millimeters[GAMMA_STEPPER] = steps[X_AXIS] / this->gamma_steps_per_mm;
 }
+
+void CartesianSolution::set_steps_per_millimeter( double steps[] )
+{
+    this->alpha_steps_per_mm = steps[0];
+    this->beta_steps_per_mm  = steps[1];
+    this->gamma_steps_per_mm = steps[2];
+}
+
+void CartesianSolution::get_steps_per_millimeter( double steps[] )
+{
+    steps[0] = this->alpha_steps_per_mm;
+    steps[1] = this->beta_steps_per_mm;
+    steps[2] = this->gamma_steps_per_mm;
+}

diff --git a/src/modules/robot/arm_solutions/CartesianSolution.h b/src/modules/robot/arm_solutions/CartesianSolution.h
index 3dc9b9e..aaa5507 100644 (file)
--- a/src/modules/robot/arm_solutions/CartesianSolution.h
+++ b/src/modules/robot/arm_solutions/CartesianSolution.h
@@ -17,6 +17,9 @@ class CartesianSolution : public BaseSolution {
         void millimeters_to_steps( double millimeters[], int steps[] );
         void steps_to_millimeters( int steps[], double millimeters[] );
 
+        void set_steps_per_millimeter( double steps[] );
+        void get_steps_per_millimeter( double steps[] );
+
         Config* config;
         double alpha_steps_per_mm;
         double beta_steps_per_mm;

diff --git a/src/modules/tools/extruder/Extruder.cpp b/src/modules/tools/extruder/Extruder.cpp
index c61c71b..622aa78 100644 (file)
--- a/src/modules/tools/extruder/Extruder.cpp
+++ b/src/modules/tools/extruder/Extruder.cpp
@@ -101,6 +101,16 @@ void Extruder::on_gcode_execute(void* argument){
         if( gcode->m == 82 ){ this->absolute_mode = true; }
         if( gcode->m == 83 ){ this->absolute_mode = false; }
         if( gcode->m == 84 ){ this->en_pin->set(1); }
+        if (gcode->m == 92 )
+        {
+            if (gcode->has_letter('E'))
+            {
+                this->steps_per_millimeter = gcode->get_value('E');
+                this->current_steps = int(floor(this->steps_per_millimeter * this->current_position));
+            }
+            gcode->stream->printf("E:%g ", this->steps_per_millimeter);
+            gcode->add_nl = true;
+        }
     }
 
     // The mode is OFF by default, and SOLO or FOLLOW only if we need to extrude