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:
#
}
// 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]; }
#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
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
// 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;
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();
#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")
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)
// 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
+ 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);
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");
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();
}
}
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);
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());
HCoop / clinton / Smoothieware.git / commitdiff