Merge branch 'edge' into multitool

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

diff --git a/src/modules/robot/Robot.cpp b/src/modules/robot/Robot.cpp
index 391a808..eedfd0c 100644 (file)
--- a/src/modules/robot/Robot.cpp
+++ b/src/modules/robot/Robot.cpp
@@ -100,6 +100,7 @@ Robot::Robot(){
     clear_vector(this->last_milestone);
     this->arm_solution = NULL;
     seconds_per_minute = 60.0F;
+       this->setToolOffset(0.0, 0.0, 0.0);
 }
 
 //Called when the module has just been loaded
@@ -208,6 +209,8 @@ void Robot::on_config_reload(void* argument){
     arm_solution->cartesian_to_actuator(last_milestone, actuator_pos);
     for (int i = 0; i < 3; i++)
         actuators[i]->change_last_milestone(actuator_pos[i]);
+
+    //this->clearToolOffset();
 }
 
 void Robot::on_get_public_data(void* argument){
@@ -246,6 +249,19 @@ void Robot::on_set_public_data(void* argument){
         this->seconds_per_minute = t / 0.6F; // t * 60 / 100
         pdr->set_taken();
     }
+    else if(pdr->second_element_is(current_position_checksum)) {
+        float *t= static_cast<float*>(pdr->get_data_ptr());
+        for (int i = 0; i < 3; i++){
+            this->last_milestone[i] = this->to_millimeters(t[i]);
+        }
+
+        float actuator_pos[3];
+        arm_solution->cartesian_to_actuator(last_milestone, actuator_pos);
+        for (int i = 0; i < 3; i++)
+            actuators[i]->change_last_milestone(actuator_pos[i]);
+
+        pdr->set_taken();
+    }
 }
 
 //A GCode has been received
@@ -463,7 +479,7 @@ void Robot::on_gcode_received(void * argument){
     }
     for(char letter = 'X'; letter <= 'Z'; letter++){
         if( gcode->has_letter(letter) ){
-            target[letter-'X'] = this->to_millimeters(gcode->get_value(letter)) + ( this->absolute_mode ? 0 : target[letter-'X']);
+            target[letter - 'X'] = this->to_millimeters(gcode->get_value(letter)) + (this->absolute_mode ? 0 : target[letter - 'X']) + this->toolOffset[letter - 'X']; 
         }
     }
 
@@ -762,4 +778,9 @@ 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) {
+        this->toolOffset[0] = offsetx;
+       this->toolOffset[1] = offsety;
+       this->toolOffset[2] = offsetz;
+}