Change the way tool offset is handled in robot

diff --git a/src/modules/robot/Robot.cpp b/src/modules/robot/Robot.cpp
index d2eab55..80e389a 100644 (file)
--- a/src/modules/robot/Robot.cpp
+++ b/src/modules/robot/Robot.cpp
@@ -101,7 +101,7 @@ Robot::Robot()
     clear_vector(this->last_milestone);
     this->arm_solution = NULL;
     seconds_per_minute = 60.0F;
-    this->setToolOffset(0.0F, 0.0F, 0.0F);
+    this->clearToolOffset();
 }
 
 //Called when the module has just been loaded
@@ -274,8 +274,6 @@ void Robot::on_gcode_received(void *argument)
 {
     Gcode *gcode = static_cast<Gcode *>(argument);
 
-    //Temp variables, constant properties are stored in the object
-    uint8_t next_action = NEXT_ACTION_DEFAULT;
     this->motion_mode = -1;
 
     //G-letter Gcodes are mostly what the Robot module is interrested in, other modules also catch the gcode event and do stuff accordingly
@@ -483,12 +481,6 @@ void Robot::on_gcode_received(void *argument)
         }
     }
 
-    // add in the tooloffset to target, we do not want this set to last milestone though
-    float offset_target[3];
-    for (int i = X_AXIS; i <= Z_AXIS; ++i) {
-        offset_target[i]= target[i] + toolOffset[i];
-    }
-
     if( gcode->has_letter('F') ) {
         if( this->motion_mode == MOTION_MODE_SEEK )
             this->seek_rate = this->to_millimeters( gcode->get_value('F') );
@@ -497,21 +489,15 @@ void Robot::on_gcode_received(void *argument)
     }
 
     //Perform any physical actions
-    switch( next_action ) {
-        case NEXT_ACTION_DEFAULT:
-            switch(this->motion_mode) {
-                case MOTION_MODE_CANCEL: break;
-                case MOTION_MODE_SEEK  : this->append_line(gcode, offset_target, this->seek_rate / seconds_per_minute ); break;
-                case MOTION_MODE_LINEAR: this->append_line(gcode, offset_target, this->feed_rate / seconds_per_minute ); break;
-                case MOTION_MODE_CW_ARC: case MOTION_MODE_CCW_ARC: this->compute_arc(gcode, offset, offset_target ); break;
-            }
-            break;
+    switch(this->motion_mode) {
+        case MOTION_MODE_CANCEL: break;
+        case MOTION_MODE_SEEK  : this->append_line(gcode, target, this->seek_rate / seconds_per_minute ); break;
+        case MOTION_MODE_LINEAR: this->append_line(gcode, target, this->feed_rate / seconds_per_minute ); break;
+        case MOTION_MODE_CW_ARC:
+        case MOTION_MODE_CCW_ARC: this->compute_arc(gcode, offset, target ); break;
     }
 
-    // As far as the parser is concerned, the position is now == target. In reality the
-    // motion control system might still be processing the action and the real tool position
-    // in any intermediate location.
-    memcpy(this->last_milestone, target, sizeof(this->last_milestone)); // this->position[] = target[];
+    // last_milestone was set to target in append_milestone, no need to do it again
 
 }
 
@@ -801,10 +787,19 @@ void Robot::select_plane(uint8_t axis_0, uint8_t axis_1, uint8_t axis_2)
     this->plane_axis_2 = axis_2;
 }
 
-void Robot::setToolOffset(float offsetx, float offsety, float offsetz)
+void Robot::clearToolOffset()
 {
-    this->toolOffset[0] = offsetx;
-    this->toolOffset[1] = offsety;
-    this->toolOffset[2] = offsetz;
+    memset(this->toolOffset, 0, sizeof(this->toolOffset));
+}
+
+void Robot::setToolOffset(const float offset[3])
+{
+    // this is ugly, but it is basically how Marlin does it...
+    // we remove the last tooloffset from last_milestone, and add the new one in
+    // this has the effect of offseting all future moves, but in effect lying about the last_milestone before the next move
+    for (int i = X_AXIS; i <= Z_AXIS; ++i) {
+        this->last_milestone[i]= this->last_milestone[i] - this->toolOffset[i] + offset[i];
+    }
+    memcpy(this->toolOffset, offset, sizeof(this->toolOffset));
 }
 

diff --git a/src/modules/robot/Robot.h b/src/modules/robot/Robot.h
index f2fb8b2..c103d6e 100644 (file)
--- a/src/modules/robot/Robot.h
+++ b/src/modules/robot/Robot.h
@@ -55,7 +55,7 @@ class Robot : public Module {
 
         BaseSolution* arm_solution;                           // Selected Arm solution ( millimeters to step calculation )
         bool absolute_mode;                                   // true for absolute mode ( default ), false for relative mode
-               void setToolOffset(float offsetx, float offsety, float offsetz);
+               void setToolOffset(const float offset[3]);
 
     private:
         void distance_in_gcode_is_known(Gcode* gcode);
@@ -89,7 +89,7 @@ class Robot : public Module {
         float max_speeds[3];                                 // Setting : max allowable speed in mm/m for each axis
 
                float toolOffset[3];
-               //void clearToolOffset();
+               void clearToolOffset();
 
     // Used by Stepper
     public:

diff --git a/src/modules/tools/toolmanager/ToolManager.cpp b/src/modules/tools/toolmanager/ToolManager.cpp
index 17b6940..a1cb855 100644 (file)
--- a/src/modules/tools/toolmanager/ToolManager.cpp
+++ b/src/modules/tools/toolmanager/ToolManager.cpp
@@ -72,7 +72,7 @@ void ToolManager::on_gcode_received(void *argument){
 
                 //send new_tool_offsets to robot
                 const float *new_tool_offset = tools[new_tool]->get_offset();
-                THEKERNEL->robot->setToolOffset(new_tool_offset[0], new_tool_offset[1], new_tool_offset[2]);
+                THEKERNEL->robot->setToolOffset(new_tool_offset);
             }
         }
     }
@@ -110,7 +110,7 @@ void ToolManager::add_tool(Tool* tool_to_add){
         this->current_tool_name = tool_to_add->get_name();
         //send new_tool_offsets to robot
         const float *new_tool_offset = tool_to_add->get_offset();
-        THEKERNEL->robot->setToolOffset(new_tool_offset[0], new_tool_offset[1], new_tool_offset[2]);
+        THEKERNEL->robot->setToolOffset(new_tool_offset);
     } else {
         tool_to_add->disable();
     }