From 932a399584cf44b82c81355a257970cc74a93155 Mon Sep 17 00:00:00 2001 From: Jim Morris Date: Fri, 19 Feb 2016 00:18:54 -0800 Subject: [PATCH] M306 for a rotary delta calculates the theta offset by comparing the current angle with the actual angle specified in the parameter A B or C --- src/modules/robot/Robot.cpp | 6 +-- src/modules/tools/endstops/Endstops.cpp | 52 ++++++++++++++++++++++--- 2 files changed, 49 insertions(+), 9 deletions(-) diff --git a/src/modules/robot/Robot.cpp b/src/modules/robot/Robot.cpp index f399bb03..9b491889 100644 --- a/src/modules/robot/Robot.cpp +++ b/src/modules/robot/Robot.cpp @@ -767,8 +767,8 @@ void Robot::reset_axis_position(float position, int axis) reset_axis_position(last_milestone[X_AXIS], last_milestone[Y_AXIS], last_milestone[Z_AXIS]); } -// similar to reset_actuator_position but directly sets the actuator positions in actuators units (eg mm for cartesian, degrees for rotary delta) -// then sets the axis postions to match. currently only called from G28.4 in Endstops.cpp +// similar to reset_axis_position but directly sets the actuator positions in actuators units (eg mm for cartesian, degrees for rotary delta) +// then sets the axis postions to match. currently only called from G28.4 and M306 in Endstops.cpp void Robot::reset_actuator_position(float a, float b, float c) { // NOTE this does NOT support the multiple actuator HACK. so if there are more than 3 actuators this will probably not work @@ -796,7 +796,7 @@ void Robot::reset_position_from_current_actuator_position() // now reset actuator::last_milestone, NOTE this may lose a little precision as FK is not always entirely accurate. // NOTE This is required to sync the machine position with the actuator position, we do a somewhat redundant cartesian_to_actuator() call - // to get everything in perfect sync. **This IS required as there is rounding to the next step as steps are integer** + // to get everything in perfect sync. arm_solution->cartesian_to_actuator(last_machine_position, actuator_pos); for (size_t i = 0; i < actuators.size(); i++) actuators[i]->change_last_milestone(actuator_pos[i]); diff --git a/src/modules/tools/endstops/Endstops.cpp b/src/modules/tools/endstops/Endstops.cpp index f3140cf5..3e42d5bd 100644 --- a/src/modules/tools/endstops/Endstops.cpp +++ b/src/modules/tools/endstops/Endstops.cpp @@ -787,10 +787,19 @@ void Endstops::on_gcode_received(void *argument) break; case 206: // M206 - set homing offset - if (gcode->has_letter('X')) home_offset[0] = gcode->get_value('X'); - if (gcode->has_letter('Y')) home_offset[1] = gcode->get_value('Y'); - if (gcode->has_letter('Z')) home_offset[2] = gcode->get_value('Z'); - gcode->stream->printf("X %5.3f Y %5.3f Z %5.3f\n", home_offset[0], home_offset[1], home_offset[2]); + if(!is_rdelta) { + if (gcode->has_letter('X')) home_offset[0] = gcode->get_value('X'); + if (gcode->has_letter('Y')) home_offset[1] = gcode->get_value('Y'); + if (gcode->has_letter('Z')) home_offset[2] = gcode->get_value('Z'); + gcode->stream->printf("X %5.3f Y %5.3f Z %5.3f\n", home_offset[0], home_offset[1], home_offset[2]); + + }else{ + // set theta offset + if (gcode->has_letter('A')) home_offset[0] = gcode->get_value('A'); + if (gcode->has_letter('B')) home_offset[1] = gcode->get_value('B'); + if (gcode->has_letter('C')) home_offset[2] = gcode->get_value('C'); + gcode->stream->printf("Theta offset A %8.5f B %8.5f C %8.5f\n", home_offset[0], home_offset[1], home_offset[2]); + } break; case 306: @@ -813,13 +822,44 @@ void Endstops::on_gcode_received(void *argument) gcode->stream->printf("Homing Offset: X %5.3f Y %5.3f Z %5.3f\n", home_offset[0], home_offset[1], home_offset[2]); }else{ - gcode->stream->printf("//M306 is not supported on rotary delta, use G92 Zxxx or G5x to set z height\n"); + // for a rotary delta M306 calibrates the homing angle + // by doing M306 A-56.17 it will calculate the M206 A value (the theta offset for actuator A) based on the difference + // between what it thinks is the current angle and what the current angle actually is specified by A (ditto for B and C) + + // get the current angle for each actuator + ActuatorCoordinates current_angle{ + THEKERNEL->robot->actuators[X_AXIS]->get_current_position(), + THEKERNEL->robot->actuators[Y_AXIS]->get_current_position(), + THEKERNEL->robot->actuators[Z_AXIS]->get_current_position() + }; + + if (gcode->has_letter('A')) { + float a= gcode->get_value('A'); + home_offset[0] -= (current_angle[0] - a); + THEKERNEL->robot->reset_actuator_position(a, NAN, NAN); + } + if (gcode->has_letter('B')) { + float b= gcode->get_value('B'); + home_offset[1] -= (current_angle[1] - b); + THEKERNEL->robot->reset_actuator_position(NAN, b, NAN); + } + if (gcode->has_letter('C')) { + float c= gcode->get_value('C'); + home_offset[2] -= (current_angle[2] - c); + THEKERNEL->robot->reset_actuator_position(NAN, NAN, c); + } + + gcode->stream->printf("Theta Offset: A %8.5f B %8.5f C %8.5f\n", home_offset[0], home_offset[1], home_offset[2]); } break; case 500: // save settings case 503: // print settings - gcode->stream->printf(";Home offset (mm):\nM206 X%1.2f Y%1.2f Z%1.2f\n", home_offset[0], home_offset[1], home_offset[2]); + if(!is_rdelta) + gcode->stream->printf(";Home offset (mm):\nM206 X%1.2f Y%1.2f Z%1.2f\n", home_offset[0], home_offset[1], home_offset[2]); + else + gcode->stream->printf(";Theta offset (degrees):\nM206 A%1.5f B%1.5f C%1.5f\n", home_offset[0], home_offset[1], home_offset[2]); + if (this->is_delta || this->is_scara) { gcode->stream->printf(";Trim (mm):\nM666 X%1.3f Y%1.3f Z%1.3f\n", trim_mm[0], trim_mm[1], trim_mm[2]); gcode->stream->printf(";Max Z\nM665 Z%1.3f\n", this->homing_position[2]); -- 2.20.1