Make G92 work as expected based on current WCS position

[clinton/Smoothieware.git] / src / modules / robot / Robot.h

diff --git a/src/modules/robot/Robot.h b/src/modules/robot/Robot.h
index b45af92..508af57 100644 (file)
--- a/src/modules/robot/Robot.h
+++ b/src/modules/robot/Robot.h
@@ -15,6 +15,7 @@ using std::string;
 #include <stack>
 
 #include "libs/Module.h"
+#include "ActuatorCoordinates.h"
 
 class Gcode;
 class BaseSolution;
@@ -24,15 +25,11 @@ class Robot : public Module {
     public:
         Robot();
         void on_module_loaded();
-        void on_config_reload(void* argument);
         void on_gcode_received(void* argument);
 
         void reset_axis_position(float position, int axis);
         void reset_axis_position(float x, float y, float z);
         void reset_position_from_current_actuator_position();
-        void get_axis_position(float position[]);
-        float to_millimeters(float value);
-        float from_millimeters(float value);
         float get_seconds_per_minute() const { return seconds_per_minute; }
         float get_z_maxfeedrate() const { return this->max_speeds[2]; }
         void setToolOffset(const float offset[3]);
@@ -40,11 +37,14 @@ class Robot : public Module {
         void  push_state();
         void  pop_state();
         void check_max_actuator_speeds();
+        float to_millimeters( float value ) const { return this->inch_mode ? value * 25.4F : value; }
+        float from_millimeters( float value) const { return this->inch_mode ? value/25.4F : value;  }
+        void get_axis_position(float position[]) const { memcpy(position, this->last_milestone, sizeof this->last_milestone); }
 
         BaseSolution* arm_solution;                           // Selected Arm solution ( millimeters to step calculation )
 
         // gets accessed by Panel, Endstops, ZProbe
-        std::vector<StepperMotor*> actuators;
+        std::array<StepperMotor*, k_max_actuators> actuators;
 
         // set by a leveling strategy to transform the target of a move according to the current plan
         std::function<void(float[3])> compensationTransform;
@@ -52,23 +52,38 @@ class Robot : public Module {
         struct {
             bool inch_mode:1;                                 // true for inch mode, false for millimeter mode ( default )
             bool absolute_mode:1;                             // true for absolute mode ( default ), false for relative mode
+            bool next_command_is_MCS:1;                       // set by G53
         };
 
     private:
+        void load_config();
         void distance_in_gcode_is_known(Gcode* gcode);
-        void append_milestone( Gcode *gcode, float target[], float rate_mm_s);
-        void append_line( Gcode* gcode, float target[], float rate_mm_s);
-        void append_arc( Gcode* gcode, float target[], float offset[], float radius, bool is_clockwise );
-        void compute_arc(Gcode* gcode, float offset[], float target[]);
+        bool append_milestone( Gcode *gcode, const float target[], float rate_mm_s);
+        bool append_line( Gcode* gcode, const float target[], float rate_mm_s);
+        bool append_arc( Gcode* gcode, const float target[], const float offset[], float radius, bool is_clockwise );
+        bool compute_arc(Gcode* gcode, const float offset[], const float target[]);
+        void process_move(Gcode *gcode);
 
         float theta(float x, float y);
         void select_plane(uint8_t axis_0, uint8_t axis_1, uint8_t axis_2);
         void clearToolOffset();
+        void print_position(Gcode *gcode) const;
 
-        typedef std::tuple<float, float, bool, bool> saved_state_t; // save current feedrate and absolute mode, inch mode
+        // Workspace coordinate systems
+        using wcs_t= std::tuple<float, float, float>;
+        wcs_t mcs2wcs(const float *pos) const;
+
+        static const size_t k_max_wcs= 9; // setup 9 WCS offsets
+        std::array<wcs_t, k_max_wcs> wcs_offsets; // these are persistent once set
+        uint8_t current_wcs{0}; // 0 means G54 in enabled this is persistent
+        wcs_t g92_offset;
+        wcs_t tool_offset; // used for multiple extruders, sets the tool offset for the current extruder applied first
+
+        using saved_state_t= std::tuple<float, float, bool, bool, uint8_t>; // save current feedrate and absolute mode, inch mode, current_wcs
         std::stack<saved_state_t> state_stack;               // saves state from M120
-        float last_milestone[3];                             // Last position, in millimeters
-        float transformed_last_milestone[3];                 // Last transformed position
+
+        float last_milestone[3]; // Last requested position, in millimeters, which is what we were requested to move to in the gcode after offsets applied but before compensation transform
+        float last_machine_position[3]; // Last machine position, which is the position before converting to actuator coordinates (includes compensation transform)
         int8_t motion_mode;                                  // Motion mode for the current received Gcode
         uint8_t plane_axis_0, plane_axis_1, plane_axis_2;    // Current plane ( XY, XZ, YZ )
         float seek_rate;                                     // Current rate for seeking moves ( mm/s )
@@ -86,26 +101,10 @@ class Robot : public Module {
         int arc_correction;                                   // Setting : how often to rectify arc computation
         float max_speeds[3];                                 // Setting : max allowable speed in mm/m for each axis
 
-        float toolOffset[3];
-
         // Used by Stepper, Planner
         friend class Planner;
         friend class Stepper;
-
-        StepperMotor* alpha_stepper_motor;
-        StepperMotor* beta_stepper_motor;
-        StepperMotor* gamma_stepper_motor;
 };
 
-// Convert from inches to millimeters ( our internal storage unit ) if needed
-inline float Robot::to_millimeters( float value ){
-    return this->inch_mode ? value * 25.4 : value;
-}
-inline float Robot::from_millimeters( float value){
-    return this->inch_mode ? value/25.4 : value;
-}
-inline void Robot::get_axis_position(float position[]){
-    memcpy(position, this->last_milestone, sizeof(float)*3 );
-}
 
 #endif