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
+
+rotary_delta_calibration.enable true # enable the calibration routines for rotary delta
#The steps per degree are calculated as:
#
#include "modules/tools/endstops/Endstops.h"
#include "modules/tools/zprobe/ZProbe.h"
#include "modules/tools/scaracal/SCARAcal.h"
+#include "RotaryDeltaCalibration.h"
#include "modules/tools/switch/SwitchPool.h"
#include "modules/tools/temperatureswitch/TemperatureSwitch.h"
#include "modules/tools/drillingcycles/Drillingcycles.h"
#ifndef NO_TOOLS_SCARACAL
kernel->add_module( new(AHB0) SCARAcal() );
#endif
+ #ifndef NO_TOOLS_ROTARYDELTACALIBRATION
+ kernel->add_module( new RotaryDeltaCalibration() );
+ #endif
#ifndef NONETWORK
kernel->add_module( new Network() );
#endif
}
// 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();
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]; }
uint8_t current_wcs{0}; // 0 means G54 is enabled this is persistent once saved with M500
wcs_t g92_offset;
wcs_t tool_offset; // used for multiple extruders, sets the tool offset for the current extruder applied first
- std::tuple<float, float, float, uint8_t> last_probe_position;
+ std::tuple<float, float, float, uint8_t> last_probe_position{0,0,0,0};
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
#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;
// 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();
}
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);
}
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);
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)
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
THEKERNEL->step_ticker->register_acceleration_tick_handler([this]() {acceleration_tick(); });
// Settings
- this->on_config_reload(this);
+ this->load_config();
}
// Get config
-void Endstops::on_config_reload(void *argument)
+void Endstops::load_config()
{
this->pins[0].from_string( THEKERNEL->config->value(alpha_min_endstop_checksum )->by_default("nc" )->as_string())->as_input();
this->pins[1].from_string( THEKERNEL->config->value(beta_min_endstop_checksum )->by_default("nc" )->as_string())->as_input();
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()) {
// 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
}
}
+void Endstops::set_homing_offset(Gcode *gcode)
+{
+ // Similar to M206 and G92 but sets Homing offsets based on current position
+ float cartesian[3];
+ THEKERNEL->robot->get_axis_position(cartesian); // get actual position from robot
+ if (gcode->has_letter('X')) {
+ home_offset[0] -= (cartesian[X_AXIS] - gcode->get_value('X'));
+ THEKERNEL->robot->reset_axis_position(gcode->get_value('X'), X_AXIS);
+ }
+ if (gcode->has_letter('Y')) {
+ home_offset[1] -= (cartesian[Y_AXIS] - gcode->get_value('Y'));
+ THEKERNEL->robot->reset_axis_position(gcode->get_value('Y'), Y_AXIS);
+ }
+ if (gcode->has_letter('Z')) {
+ home_offset[2] -= (cartesian[Z_AXIS] - gcode->get_value('Z'));
+ THEKERNEL->robot->reset_axis_position(gcode->get_value('Z'), Z_AXIS);
+ }
+
+ gcode->stream->printf("Homing Offset: X %5.3f Y %5.3f Z %5.3f\n", home_offset[0], home_offset[1], home_offset[2]);
+}
+
// Start homing sequences by response to GCode commands
void Endstops::on_gcode_received(void *argument)
{
break;
case 206: // M206 - set homing offset
- 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]);
+ if(is_rdelta) return; // RotaryDeltaCalibration module will handle this
- } 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]);
- }
+ 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]);
break;
- case 306:
- if(!is_rdelta) { // Similar to M206 and G92 but sets Homing offsets based on current position
- float cartesian[3];
- THEKERNEL->robot->get_axis_position(cartesian); // get actual position from robot
- if (gcode->has_letter('X')) {
- home_offset[0] -= (cartesian[X_AXIS] - gcode->get_value('X'));
- THEKERNEL->robot->reset_axis_position(gcode->get_value('X'), X_AXIS);
- }
- if (gcode->has_letter('Y')) {
- home_offset[1] -= (cartesian[Y_AXIS] - gcode->get_value('Y'));
- THEKERNEL->robot->reset_axis_position(gcode->get_value('Y'), Y_AXIS);
- }
- if (gcode->has_letter('Z')) {
- home_offset[2] -= (cartesian[Z_AXIS] - gcode->get_value('Z'));
- THEKERNEL->robot->reset_axis_position(gcode->get_value('Z'), Z_AXIS);
- }
-
- gcode->stream->printf("Homing Offset: X %5.3f Y %5.3f Z %5.3f\n", home_offset[0], home_offset[1], home_offset[2]);
+ case 306: // set homing offset based on current position
+ if(is_rdelta) return; // RotaryDeltaCalibration module will handle this
- } else {
- // 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()
- };
-
- //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);
- }
- 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);
- }
- 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);
- }
-
- gcode->stream->printf("Theta Offset: A %8.5f B %8.5f C %8.5f\n", home_offset[0], home_offset[1], home_offset[2]);
- }
+ set_homing_offset(gcode);
break;
case 500: // save settings
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();
Endstops();
void on_module_loaded();
void on_gcode_received(void* argument);
- void on_config_reload(void* argument);
void acceleration_tick(void);
private:
+ void load_config();
void home(char axes_to_move);
void do_homing_cartesian(char axes_to_move);
void do_homing_corexy(char axes_to_move);
void on_idle(void *argument);
bool debounced_get(int pin);
void process_home_command(Gcode* gcode);
+ void set_homing_offset(Gcode* gcode);
float homing_position[3];
float home_offset[3];
--- /dev/null
+#include "RotaryDeltaCalibration.h"
+#include "EndstopsPublicAccess.h"
+#include "Kernel.h"
+#include "Robot.h"
+#include "Config.h"
+#include "checksumm.h"
+#include "ConfigValue.h"
+#include "StreamOutput.h"
+#include "PublicDataRequest.h"
+#include "PublicData.h"
+#include "Gcode.h"
+#include "StepperMotor.h"
+
+#define rotarydelta_checksum CHECKSUM("rotary_delta_calibration")
+#define enable_checksum CHECKSUM("enable")
+
+void RotaryDeltaCalibration::on_module_loaded()
+{
+ // if the module is disabled -> do nothing
+ if(!THEKERNEL->config->value( rotarydelta_checksum, enable_checksum )->by_default(false)->as_bool()) {
+ // as this module is not needed free up the resource
+ delete this;
+ return;
+ }
+
+ // register event-handlers
+ register_for_event(ON_GCODE_RECEIVED);
+}
+
+float *RotaryDeltaCalibration::get_homing_offset()
+{
+ float *theta_offset; // points to theta offset in Endstop module
+ bool ok = PublicData::get_value( endstops_checksum, home_offset_checksum, &theta_offset );
+ return ok ? theta_offset : nullptr;
+}
+
+void RotaryDeltaCalibration::on_gcode_received(void *argument)
+{
+ Gcode *gcode = static_cast<Gcode *>(argument);
+
+ if( gcode->has_m) {
+ switch( gcode->m ) {
+ case 206: {
+ float *theta_offset= get_homing_offset(); // points to theta offset in Endstop module
+ if (theta_offset == nullptr) {
+ gcode->stream->printf("error:no endstop module found\n");
+ return;
+ }
+
+ // set theta offset, set directly in the Endstop module (bad practice really)
+ if (gcode->has_letter('A')) theta_offset[0] = gcode->get_value('A');
+ if (gcode->has_letter('B')) theta_offset[1] = gcode->get_value('B');
+ if (gcode->has_letter('C')) theta_offset[2] = gcode->get_value('C');
+
+ gcode->stream->printf("Theta offset set: A %8.5f B %8.5f C %8.5f\n", theta_offset[0], theta_offset[1], theta_offset[2]);
+
+ break;
+ }
+
+ case 306: {
+ // 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)
+
+ ActuatorCoordinates current_angle;
+ // 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
+ for (size_t i = 0; i < THEKERNEL->robot->actuators.size(); i++) {
+ current_angle[i]= THEKERNEL->robot->actuators[i]->get_current_position();
+ }
+
+ if (gcode->has_letter('L') && gcode->get_value('L') != 0) {
+ // specifying L1 it will take the last probe position (set by G30 or G38.x ) and set the home offset based on that
+ // this allows the use of G30 to find the angle tool
+ uint8_t ok;
+ std::tie(current_angle[0], current_angle[1], current_angle[2], ok) = THEKERNEL->robot->get_last_probe_position();
+ if(ok == 0) {
+ gcode->stream->printf("error:Nothing set as probe failed or not run\n");
+ return;
+ }
+ }
+
+ float *theta_offset= get_homing_offset(); // points to theta offset in Endstop module
+ if (theta_offset == nullptr) {
+ gcode->stream->printf("error:no endstop module found\n");
+ return;
+ }
+
+ int cnt= 0;
+
+ //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
+ theta_offset[0] -= (current_angle[0] - a);
+ current_angle[0]= a;
+ cnt++;
+ }
+ if (gcode->has_letter('B')) {
+ float b = gcode->get_value('B');
+ theta_offset[1] -= (current_angle[1] - b);
+ current_angle[1]= b;
+ cnt++;
+ }
+ if (gcode->has_letter('C')) {
+ float c = gcode->get_value('C');
+ theta_offset[2] -= (current_angle[2] - c);
+ current_angle[2]= c;
+ cnt++;
+ }
+
+ // reset the actuator positions (and machine position accordingly)
+ // But only if all three actuators have been specified at the same time
+ if(cnt == 3 || (gcode->has_letter('R') && gcode->get_value('R') != 0)) {
+ THEKERNEL->robot->reset_actuator_position(current_angle);
+ gcode->stream->printf("NOTE: actuator position reset\n");
+ }else{
+ gcode->stream->printf("NOTE: actuator position NOT reset\n");
+ }
+
+ gcode->stream->printf("Theta Offset: A %8.5f B %8.5f C %8.5f\n", theta_offset[0], theta_offset[1], theta_offset[2]);
+ }
+ }
+ }
+}
--- /dev/null
+#pragma once
+
+#include "Module.h"
+
+class Gcode;
+
+class RotaryDeltaCalibration : public Module
+{
+public:
+ RotaryDeltaCalibration(){};
+ virtual ~RotaryDeltaCalibration(){};
+
+ void on_module_loaded();
+
+private:
+ void on_gcode_received(void *argument);
+ float *get_homing_offset();
+};
#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")
+#define rdelta_homing_checksum CHECKSUM("rdelta_homing")
#define X_AXIS 0
#define Y_AXIS 1
// need to know if we need to use delta kinematics for homing
this->is_delta = THEKERNEL->config->value(delta_homing_checksum)->by_default(false)->as_bool();
+ this->is_rdelta = THEKERNEL->config->value(rdelta_homing_checksum)->by_default(false)->as_bool();
// default for backwards compatibility add DeltaCalibrationStrategy if a delta
// will be deprecated
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)
return false;
}
+ bool delta= is_delta || is_rdelta;
+
// if no stepper is moving, moves are finished and there was no touch
- if( !STEPPER[Z_AXIS]->is_moving() && (!is_delta || (!STEPPER[Y_AXIS]->is_moving() && !STEPPER[Z_AXIS]->is_moving())) ) {
+ if( !STEPPER[Z_AXIS]->is_moving() && (!delta || (!STEPPER[Y_AXIS]->is_moving() && !STEPPER[Z_AXIS]->is_moving())) ) {
return false;
}
- // if the touchprobe is active...
+ // if the probe is active...
if( this->pin.get() ) {
//...increase debounce counter...
if( debounce < debounce_count) {
steps= STEPPER[Z_AXIS]->get_stepped();
STEPPER[Z_AXIS]->move(0, 0);
}
- if(is_delta) {
+ if(delta) {
for( int i = X_AXIS; i <= Y_AXIS; i++ ) {
if ( STEPPER[i]->is_moving() ) {
STEPPER[i]->move(0, 0);
// 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++ ) {
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
- if(this->is_delta) {
+ 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
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
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);
STEPPER[Z_AXIS]->move(dir, steps, 0);
- if(this->is_delta) {
+ if(delta) {
STEPPER[X_AXIS]->move(dir, steps, 0);
STEPPER[Y_AXIS]->move(dir, steps, 0);
}
this->running = true;
- while(STEPPER[Z_AXIS]->is_moving() || (is_delta && (STEPPER[X_AXIS]->is_moving() || STEPPER[Y_AXIS]->is_moving())) ) {
+ while(STEPPER[Z_AXIS]->is_moving() || (delta && (STEPPER[X_AXIS]->is_moving() || STEPPER[Y_AXIS]->is_moving())) ) {
// wait for it to complete
THEKERNEL->call_event(ON_IDLE);
if(THEKERNEL->is_halted()){
}
if( gcode->g == 30 ) { // simple Z probe
- // NOTE currently this will not work for rotary deltas, use G38.2/3 Z instead
// first wait for an empty queue i.e. no moves left
THEKERNEL->conveyor->wait_for_empty_queue();
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) {
+ // the result is in actuator coordinates and raw steps
gcode->stream->printf("Z:%1.4f C:%d\n", zsteps_to_mm(steps), steps);
+
+ // set the last probe position to the current actuator units
+ THEKERNEL->robot->set_last_probe_position(std::make_tuple(
+ THEKERNEL->robot->actuators[X_AXIS]->get_current_position(),
+ THEKERNEL->robot->actuators[Y_AXIS]->get_current_position(),
+ THEKERNEL->robot->actuators[Z_AXIS]->get_current_position(),
+ 1));
+
// 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 to pre probe position
+ return_probe(steps, reverse);
}
+
} else {
gcode->stream->printf("ZProbe not triggered\n");
+ THEKERNEL->robot->set_last_probe_position(std::make_tuple(
+ THEKERNEL->robot->actuators[X_AXIS]->get_current_position(),
+ THEKERNEL->robot->actuators[Y_AXIS]->get_current_position(),
+ THEKERNEL->robot->actuators[Z_AXIS]->get_current_position(),
+ 0));
}
} else {
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();
}
}
if(!this->running) return; // nothing to do
if(STEPPER[Z_AXIS]->is_moving()) accelerate(Z_AXIS);
- if(is_delta) {
+ if(is_delta || is_rdelta) {
// deltas needs to move all actuators
for ( int c = X_AXIS; c <= Y_AXIS; c++ ) {
if( !STEPPER[c]->is_moving() ) continue;
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);
volatile struct {
volatile bool running:1;
bool is_delta:1;
+ bool is_rdelta:1;
bool probing:1;
+ bool reverse_z:1;
volatile bool probe_detected:1;
};
};
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());
// show free memory
void SimpleShell::mem_command( string parameters, StreamOutput *stream)
{
- bool verbose = shift_parameter( parameters ).find_first_of("Vv") != string::npos ;
+ bool verbose = shift_parameter( parameters ).find_first_of("Vv") != string::npos;
unsigned long heap = (unsigned long)_sbrk(0);
unsigned long m = g_maximumHeapAddress - heap;
stream->printf("Unused Heap: %lu bytes\r\n", m);
[TLO:0.000]
[PRB:0.000,0.000,0.000:0]
*/
+
+ bool verbose = shift_parameter( parameters ).find_first_of("Vv") != string::npos;
+
std::vector<Robot::wcs_t> v= THEKERNEL->robot->get_wcs_state();
+ if(verbose) {
+ char current_wcs= std::get<0>(v[0]);
+ stream->printf("[current WCS: %s]\n", wcs2gcode(current_wcs).c_str());
+ }
+
int n= std::get<1>(v[0]);
for (int i = 1; i <= n; ++i) {
stream->printf("[%s:%1.4f,%1.4f,%1.4f]\n", wcs2gcode(i-1).c_str(), std::get<0>(v[i]), std::get<1>(v[i]), std::get<2>(v[i]));
stream->printf("[G30:%1.4f,%1.4f,%1.4f]\n", 0.0F, 0.0F, 0.0F); // not implemented
stream->printf("[G92:%1.4f,%1.4f,%1.4f]\n", std::get<0>(v[n+1]), std::get<1>(v[n+1]), std::get<2>(v[n+1]));
- stream->printf("[TL0:%1.4f]\n", std::get<2>(v[n+2]));
+ if(verbose) {
+ stream->printf("[Tool Offset:%1.4f,%1.4f,%1.4f]\n", std::get<0>(v[n+2]), std::get<1>(v[n+2]), std::get<2>(v[n+2]));
+ }else{
+ stream->printf("[TL0:%1.4f]\n", std::get<2>(v[n+2]));
+ }
// this is the last probe position, updated when a probe completes, also stores the number of steps moved after a homing cycle
float px, py, pz;
} else if (what == "wcs") {
// print the wcs state
- std::vector<Robot::wcs_t> v= THEKERNEL->robot->get_wcs_state();
- char current_wcs= std::get<0>(v[0]);
- stream->printf("current WCS: %s\n", wcs2gcode(current_wcs).c_str());
- int n= std::get<1>(v[0]);
- for (int i = 1; i <= n; ++i) {
- stream->printf("%s: %f, %f, %f\n", wcs2gcode(i-1).c_str(), std::get<0>(v[i]), std::get<1>(v[i]), std::get<2>(v[i]));
- }
-
- stream->printf("G92: %f, %f, %f\n", std::get<0>(v[n+1]), std::get<1>(v[n+1]), std::get<2>(v[n+1]));
- stream->printf("ToolOffset: %f, %f, %f\n", std::get<0>(v[n+2]), std::get<1>(v[n+2]), std::get<2>(v[n+2]));
+ grblDP_command("-v", stream);
} else if (what == "state") {
// also $G
HCoop / clinton / Smoothieware.git / commitdiff