From 93f20a8c66e479c5460af133007783a8a5d68a3f Mon Sep 17 00:00:00 2001 From: Jim Morris Date: Sat, 27 Feb 2016 01:24:33 -0800 Subject: [PATCH] fix rotary delta FK to be mirrored like the IK add config option to mirror or not mirror the XY on rotary delta fix homing on rotarary delta Allow zprobe to probe in reverse direction allow ok as a command and just echo ok. --- ConfigSamples/rotary.delta/config | 4 +- src/modules/robot/Robot.cpp | 10 ++-- src/modules/robot/Robot.h | 2 +- .../arm_solutions/RotaryDeltaSolution.cpp | 33 +++++++++--- .../robot/arm_solutions/RotaryDeltaSolution.h | 5 +- src/modules/tools/endstops/Endstops.cpp | 43 ++++++++-------- src/modules/tools/zprobe/ZProbe.cpp | 51 ++++++++----------- src/modules/tools/zprobe/ZProbe.h | 7 +-- src/modules/utils/simpleshell/SimpleShell.cpp | 5 +- 9 files changed, 92 insertions(+), 68 deletions(-) diff --git a/ConfigSamples/rotary.delta/config b/ConfigSamples/rotary.delta/config index a7fe1e58..fa8432d0 100644 --- a/ConfigSamples/rotary.delta/config +++ b/ConfigSamples/rotary.delta/config @@ -19,7 +19,9 @@ delta_rf 90.000 # Servo horn length delta_z_offset 268.0 # Distance from big pulley shaft to table/bed delta_ee_offs 15.000 # Ball joint plane to bottom of end effector surface -tool_offset 30.500 # Distance between end effector ball joint plane and tip of tool (PnP) +delta_tool_offset 30.500 # Distance between end effector ball joint plane and tip of tool (PnP) + +delta_mirror_xy true # true for firepick #The steps per degree are calculated as: # diff --git a/src/modules/robot/Robot.cpp b/src/modules/robot/Robot.cpp index d4d93359..3e7dae67 100644 --- a/src/modules/robot/Robot.cpp +++ b/src/modules/robot/Robot.cpp @@ -772,13 +772,11 @@ void Robot::reset_axis_position(float position, int axis) } // 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) +// then sets the axis positions to match. currently only called from Endstops.cpp +void Robot::reset_actuator_position(const ActuatorCoordinates &ac) { - // NOTE this does NOT support the multiple actuator HACK. so if there are more than 3 actuators this will probably not work - if(!isnan(a)) actuators[0]->change_last_milestone(a); - if(!isnan(b)) actuators[1]->change_last_milestone(b); - if(!isnan(c)) actuators[2]->change_last_milestone(c); + for (size_t i = 0; i < actuators.size(); i++) + actuators[i]->change_last_milestone(ac[i]); // now correct axis positions then recorrect actuator to account for rounding reset_position_from_current_actuator_position(); diff --git a/src/modules/robot/Robot.h b/src/modules/robot/Robot.h index 7832a133..54fd14c5 100644 --- a/src/modules/robot/Robot.h +++ b/src/modules/robot/Robot.h @@ -33,7 +33,7 @@ class Robot : public Module { void reset_axis_position(float position, int axis); void reset_axis_position(float x, float y, float z); - void reset_actuator_position(float a, float b, float c); + void reset_actuator_position(const ActuatorCoordinates &ac); void reset_position_from_current_actuator_position(); float get_seconds_per_minute() const { return seconds_per_minute; } float get_z_maxfeedrate() const { return this->max_speeds[2]; } diff --git a/src/modules/robot/arm_solutions/RotaryDeltaSolution.cpp b/src/modules/robot/arm_solutions/RotaryDeltaSolution.cpp index fbbb3b7c..07aa6074 100644 --- a/src/modules/robot/arm_solutions/RotaryDeltaSolution.cpp +++ b/src/modules/robot/arm_solutions/RotaryDeltaSolution.cpp @@ -17,7 +17,9 @@ #define delta_z_offset_checksum CHECKSUM("delta_z_offset") #define delta_ee_offs_checksum CHECKSUM("delta_ee_offs") -#define tool_offset_checksum CHECKSUM("tool_offset") +#define tool_offset_checksum CHECKSUM("delta_tool_offset") + +#define delta_mirror_xy_checksum CHECKSUM("delta_mirror_xy") const static float pi = 3.14159265358979323846; // PI const static float two_pi = 2 * pi; @@ -51,6 +53,10 @@ RotaryDeltaSolution::RotaryDeltaSolution(Config *config) // Distance between end effector ball joint plane and tip of tool (PnP) tool_offset = config->value(tool_offset_checksum)->by_default(30.500F)->as_number(); + // mirror the XY axis + mirror_xy= config->value(delta_mirror_xy_checksum)->by_default(true)->as_bool(); + + debug_flag= false; init(); } @@ -130,7 +136,6 @@ int RotaryDeltaSolution::delta_calcForward(float theta1, float theta2, float the void RotaryDeltaSolution::init() { - //these are calculated here and not in the config() as these variables can be fine tuned by the user. z_calc_offset = -(delta_z_offset - tool_offset - delta_ee_offs); } @@ -142,10 +147,16 @@ void RotaryDeltaSolution::cartesian_to_actuator(const float cartesian_mm[], Actu float beta_theta = 0.0F; float gamma_theta = 0.0F; - //Code from Trossen Robotics tutorial, note we put the X axis at the back and not the front of the robot. + //Code from Trossen Robotics tutorial, has X in front Y to the right and Z to the left + // firepick is X at the back and negates X0 X0 + // selected by a config option + float x0 = cartesian_mm[X_AXIS]; + float y0 = cartesian_mm[Y_AXIS]; + if(mirror_xy) { + x0= -x0; + y0= -y0; + } - float x0 = -cartesian_mm[X_AXIS]; - float y0 = -cartesian_mm[Y_AXIS]; float z_with_offset = cartesian_mm[Z_AXIS] + z_calc_offset; //The delta calculation below places zero at the top. Subtract the Z offset to make zero at the bottom. int status = delta_calcAngleYZ(x0, y0, z_with_offset, alpha_theta); @@ -187,8 +198,18 @@ void RotaryDeltaSolution::cartesian_to_actuator(const float cartesian_mm[], Actu void RotaryDeltaSolution::actuator_to_cartesian(const ActuatorCoordinates &actuator_mm, float cartesian_mm[] ) { + float x, y, z; //Use forward kinematics - delta_calcForward(actuator_mm[ALPHA_STEPPER], actuator_mm[BETA_STEPPER ], actuator_mm[GAMMA_STEPPER], cartesian_mm[X_AXIS], cartesian_mm[Y_AXIS], cartesian_mm[Z_AXIS]); + delta_calcForward(actuator_mm[ALPHA_STEPPER], actuator_mm[BETA_STEPPER ], actuator_mm[GAMMA_STEPPER], x, y, z); + if(mirror_xy) { + cartesian_mm[X_AXIS]= -x; + cartesian_mm[Y_AXIS]= -y; + cartesian_mm[Z_AXIS]= z; + }else{ + cartesian_mm[X_AXIS]= x; + cartesian_mm[Y_AXIS]= y; + cartesian_mm[Z_AXIS]= z; + } } bool RotaryDeltaSolution::set_optional(const arm_options_t &options) diff --git a/src/modules/robot/arm_solutions/RotaryDeltaSolution.h b/src/modules/robot/arm_solutions/RotaryDeltaSolution.h index dd475f7a..52334fab 100644 --- a/src/modules/robot/arm_solutions/RotaryDeltaSolution.h +++ b/src/modules/robot/arm_solutions/RotaryDeltaSolution.h @@ -30,6 +30,9 @@ class RotaryDeltaSolution : public BaseSolution { float tool_offset; // Distance between end effector ball joint plane and tip of tool float z_calc_offset; - bool debug_flag{false}; + struct { + bool debug_flag:1; + bool mirror_xy:1; + }; }; #endif // RotaryDeltaSolution_H diff --git a/src/modules/tools/endstops/Endstops.cpp b/src/modules/tools/endstops/Endstops.cpp index b0ee3bd6..7f5746d2 100644 --- a/src/modules/tools/endstops/Endstops.cpp +++ b/src/modules/tools/endstops/Endstops.cpp @@ -650,12 +650,12 @@ void Endstops::process_home_command(Gcode* gcode) return; } else if(gcode->subcode == 4) { // G28.4 is a smoothie special it sets manual homing based on the actuator position (used for rotary delta) - // do a manual homing based on given coordinates, no endstops required - float a = NAN, b = NAN, c = NAN; - if(gcode->has_letter('A')) a = gcode->get_value('A'); - if(gcode->has_letter('B')) b = gcode->get_value('B'); - if(gcode->has_letter('C')) c = gcode->get_value('C'); - THEKERNEL->robot->reset_actuator_position(a, b, c); + // do a manual homing based on given coordinates, no endstops required, NOTE does not support the multi actuator hack + ActuatorCoordinates ac; + if(gcode->has_letter('A')) ac[0] = gcode->get_value('A'); + if(gcode->has_letter('B')) ac[1] = gcode->get_value('B'); + if(gcode->has_letter('C')) ac[2] = gcode->get_value('C'); + THEKERNEL->robot->reset_actuator_position(ac); return; } else if(THEKERNEL->is_grbl_mode()) { @@ -755,7 +755,8 @@ void Endstops::process_home_command(Gcode* gcode) // without endstop trim, real_position == ideal_position if(is_rdelta) { // with a rotary delta we set the actuators angle then use the FK to calculate the resulting cartesian coordinates - THEKERNEL->robot->reset_actuator_position(ideal_position[0], ideal_position[1], ideal_position[2]); + ActuatorCoordinates real_actuator_position = {ideal_position[0], ideal_position[1], ideal_position[2]}; + THEKERNEL->robot->reset_actuator_position(real_actuator_position); } else { // Reset the actuator positions to correspond our real position @@ -848,30 +849,32 @@ void Endstops::on_gcode_received(void *argument) // 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() - }; + // get the current angle for each actuator, NOTE we only deal with ABC so if there are more than 3 actuators this will probably go wonky + ActuatorCoordinates current_angle; + for (size_t i = 0; i < THEKERNEL->robot->actuators.size(); i++) { + current_angle[i]= THEKERNEL->robot->actuators[i]->get_current_position(); + } //figure out what home_offset needs to be to correct the homing_position if (gcode->has_letter('A')) { float a = gcode->get_value('A'); // what actual angle is - home_offset[0] = (a - current_angle[0]); - THEKERNEL->robot->reset_actuator_position(a, NAN, NAN); + home_offset[0] += (current_angle[0] - a); + current_angle[0]= a; } if (gcode->has_letter('B')) { float b = gcode->get_value('B'); - home_offset[1] = (b - current_angle[1]); - THEKERNEL->robot->reset_actuator_position(NAN, b, NAN); + home_offset[1] += (current_angle[1] - b); + current_angle[1]= b; } if (gcode->has_letter('C')) { float c = gcode->get_value('C'); - home_offset[2] = (c - current_angle[2]); - THEKERNEL->robot->reset_actuator_position(NAN, NAN, c); + home_offset[2] += (current_angle[2] - c); + current_angle[2]= c; } + // reset the actuator positions (and machine position accordingly) + THEKERNEL->robot->reset_actuator_position(current_angle); + 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; @@ -919,7 +922,7 @@ void Endstops::on_gcode_received(void *argument) case 1910: { // M1910.0 - move specific number of raw steps // M1910.1 - stop any moves - // M1910.2 - move specific number of actuator coordinates (usually mm but is degrees for a rotary delta) + // M1910.2 - move specific number of actuator units (usually mm but is degrees for a rotary delta) if(gcode->subcode == 0 || gcode->subcode == 2) { // Enable the motors THEKERNEL->stepper->turn_enable_pins_on(); diff --git a/src/modules/tools/zprobe/ZProbe.cpp b/src/modules/tools/zprobe/ZProbe.cpp index d3d24540..14abf69f 100644 --- a/src/modules/tools/zprobe/ZProbe.cpp +++ b/src/modules/tools/zprobe/ZProbe.cpp @@ -41,6 +41,7 @@ #define return_feedrate_checksum CHECKSUM("return_feedrate") #define probe_height_checksum CHECKSUM("probe_height") #define gamma_max_checksum CHECKSUM("gamma_max") +#define reverse_z_direction_checksum CHECKSUM("reverse_z") // from endstop section #define delta_homing_checksum CHECKSUM("delta_homing") @@ -134,6 +135,7 @@ void ZProbe::on_config_reload(void *argument) this->fast_feedrate = THEKERNEL->config->value(zprobe_checksum, fast_feedrate_checksum)->by_default(100)->as_number(); // feedrate in mm/sec this->return_feedrate = THEKERNEL->config->value(zprobe_checksum, return_feedrate_checksum)->by_default(0)->as_number(); // feedrate in mm/sec this->max_z = THEKERNEL->config->value(gamma_max_checksum)->by_default(500)->as_number(); // maximum zprobe distance + this->reverse_z = THEKERNEL->config->value(reverse_z_direction_checksum)->by_default(false)->as_bool(); // Z probe moves in reverse direction (upside down rdelta) } bool ZProbe::wait_for_probe(int& steps) @@ -183,7 +185,7 @@ bool ZProbe::wait_for_probe(int& steps) // single probe with custom feedrate // returns boolean value indicating if probe was triggered -bool ZProbe::run_probe_feed(int& steps, float feedrate, float max_dist) +bool ZProbe::run_probe(int& steps, float feedrate, float max_dist, bool reverse) { // not a block move so disable the last tick setting for ( int c = X_AXIS; c <= Z_AXIS; c++ ) { @@ -196,11 +198,13 @@ bool ZProbe::run_probe_feed(int& steps, float feedrate, float max_dist) float maxz= max_dist < 0 ? this->max_z*2 : max_dist; // move Z down - STEPPER[Z_AXIS]->move(true, maxz * Z_STEPS_PER_MM, 0); // always probes down, no more than 2*maxz + bool dir= !reverse_z; + if(reverse) dir= !dir; // specified to move in opposite Z direction + STEPPER[Z_AXIS]->move(dir, maxz * Z_STEPS_PER_MM, 0); // probe in specified direction, no more than maxz if(this->is_delta || this->is_rdelta) { // for delta need to move all three actuators - STEPPER[X_AXIS]->move(true, maxz * STEPS_PER_MM(X_AXIS), 0); - STEPPER[Y_AXIS]->move(true, maxz * STEPS_PER_MM(Y_AXIS), 0); + STEPPER[X_AXIS]->move(dir, maxz * STEPS_PER_MM(X_AXIS), 0); + STEPPER[Y_AXIS]->move(dir, maxz * STEPS_PER_MM(Y_AXIS), 0); } // start acceleration processing @@ -214,15 +218,7 @@ bool ZProbe::run_probe_feed(int& steps, float feedrate, float max_dist) return r; } -// single probe with either fast or slow feedrate -// returns boolean value indicating if probe was triggered -bool ZProbe::run_probe(int& steps, bool fast) -{ - float feedrate = (fast ? this->fast_feedrate : this->slow_feedrate); - return run_probe_feed(steps, feedrate); -} - -bool ZProbe::return_probe(int steps) +bool ZProbe::return_probe(int steps, bool reverse) { // move probe back to where it was @@ -236,6 +232,7 @@ bool ZProbe::return_probe(int steps) this->current_feedrate = fr * Z_STEPS_PER_MM; // feedrate in steps/sec bool dir= steps < 0; + if(reverse) dir= !dir; steps= abs(steps); bool delta= (this->is_delta || this->is_rdelta); @@ -306,20 +303,18 @@ void ZProbe::on_gcode_received(void *argument) int steps; bool probe_result; - if(gcode->has_letter('F')) { - probe_result = run_probe_feed(steps, gcode->get_value('F') / 60); - } else { - probe_result = run_probe(steps); - } + bool reverse= (gcode->has_letter('R') && gcode->get_value('R') != 0); // specify to probe in reverse direction + float rate= gcode->has_letter('F') ? gcode->get_value('F') / 60 : this->slow_feedrate; + probe_result = run_probe(steps, rate, -1, reverse); if(probe_result) { gcode->stream->printf("Z:%1.4f C:%d\n", zsteps_to_mm(steps), steps); // move back to where it started, unless a Z is specified - if(gcode->has_letter('Z')) { + if(gcode->has_letter('Z') && !is_rdelta) { // set Z to the specified value, and leave probe where it is THEKERNEL->robot->reset_axis_position(gcode->get_value('Z'), Z_AXIS); } else { - return_probe(steps); + return_probe(steps, reverse); } } else { gcode->stream->printf("ZProbe not triggered\n"); @@ -488,16 +483,14 @@ void ZProbe::probe_XYZ(Gcode *gcode, int axis) gcode->stream->printf("[PRB:%1.3f,%1.3f,%1.3f:%d]\n", pos[X_AXIS], pos[Y_AXIS], pos[Z_AXIS], probeok); THEKERNEL->robot->set_last_probe_position(std::make_tuple(pos[X_AXIS], pos[Y_AXIS], pos[Z_AXIS], probeok)); - if(!probeok) { - if(gcode->subcode == 2) { - // issue error if probe was not triggered and subcode == 2 - gcode->stream->printf("ALARM:Probe fail\n"); - THEKERNEL->call_event(ON_HALT, nullptr); + if(!probeok && gcode->subcode == 2) { + // issue error if probe was not triggered and subcode == 2 + gcode->stream->printf("ALARM:Probe fail\n"); + THEKERNEL->call_event(ON_HALT, nullptr); - }else{ - // if the probe stopped the move we need to correct the last_milestone as it did not reach where it thought - THEKERNEL->robot->reset_position_from_current_actuator_position(); - } + }else if(probeok){ + // if the probe stopped the move we need to correct the last_milestone as it did not reach where it thought + THEKERNEL->robot->reset_position_from_current_actuator_position(); } } diff --git a/src/modules/tools/zprobe/ZProbe.h b/src/modules/tools/zprobe/ZProbe.h index ef5decca..4113ba34 100644 --- a/src/modules/tools/zprobe/ZProbe.h +++ b/src/modules/tools/zprobe/ZProbe.h @@ -34,9 +34,9 @@ public: void acceleration_tick(void); bool wait_for_probe(int& steps); - bool run_probe(int& steps, bool fast= false); - bool run_probe_feed(int& steps, float feedrate, float max_dist= -1); - bool return_probe(int steps); + bool run_probe(int& steps, float feedrate, float max_dist= -1, bool reverse= false); + bool run_probe(int& steps, bool fast= false) { return run_probe(steps, fast ? this->fast_feedrate : this->slow_feedrate); } + bool return_probe(int steps, bool reverse= false); bool doProbeAt(int &steps, float x, float y); float probeDistance(float x, float y); @@ -71,6 +71,7 @@ private: bool is_delta:1; bool is_rdelta:1; bool probing:1; + bool reverse_z:1; volatile bool probe_detected:1; }; }; diff --git a/src/modules/utils/simpleshell/SimpleShell.cpp b/src/modules/utils/simpleshell/SimpleShell.cpp index 3a1b1ce7..38680f14 100644 --- a/src/modules/utils/simpleshell/SimpleShell.cpp +++ b/src/modules/utils/simpleshell/SimpleShell.cpp @@ -266,7 +266,10 @@ void SimpleShell::on_console_line_received( void *argument ) THEKERNEL->configurator->config_load_command( possible_command, new_message.stream ); } else if (cmd == "play" || cmd == "progress" || cmd == "abort" || cmd == "suspend" || cmd == "resume") { - // handled in the Player.cpp module + + } else if (cmd == "ok") { + // probably an echo so reply ok + new_message.stream->printf("ok\n"); }else if(!parse_command(cmd.c_str(), possible_command, new_message.stream)) { new_message.stream->printf("error:Unsupported command - %s\n", cmd.c_str()); -- 2.20.1