process wcs offsets immediately.
add in tool offset to that.
this->motion_mode = MOTION_MODE_SEEK;
this->select_plane(X_AXIS, Y_AXIS, Z_AXIS);
clear_vector(this->last_milestone);
this->motion_mode = MOTION_MODE_SEEK;
this->select_plane(X_AXIS, Y_AXIS, Z_AXIS);
clear_vector(this->last_milestone);
- clear_vector(this->machine_position);
+ clear_vector(this->last_machine_position);
this->arm_solution = NULL;
seconds_per_minute = 60.0F;
this->clearToolOffset();
this->arm_solution = NULL;
seconds_per_minute = 60.0F;
this->clearToolOffset();
-// converts current machine position to work coordinate system
-Robot::wcs_t Robot::mcs2wcs()
+// converts current last milestone (machine position without compensation transform) to work coordinate system (inverse transform)
+Robot::wcs_t Robot::mcs2wcs(const float *pos) const
- // FIXME this will be incorrect if there is a compensation transform in effect
+ // FIXME this will be incorrect if there is a compensation transform in effect and pos is machine_position instead of last_transform
- machine_position[X_AXIS] - (std::get<X_AXIS>(wcs_offsets[current_wcs]) - std::get<X_AXIS>(g92_offset)),
- machine_position[Y_AXIS] - (std::get<Y_AXIS>(wcs_offsets[current_wcs]) - std::get<Y_AXIS>(g92_offset)),
- machine_position[Z_AXIS] - (std::get<Z_AXIS>(wcs_offsets[current_wcs]) - std::get<Z_AXIS>(g92_offset)));
+ pos[X_AXIS] - std::get<X_AXIS>(wcs_offsets[current_wcs]) + std::get<X_AXIS>(g92_offset) - std::get<X_AXIS>(tool_offset),
+ pos[Y_AXIS] - std::get<Y_AXIS>(wcs_offsets[current_wcs]) + std::get<Y_AXIS>(g92_offset) - std::get<Y_AXIS>(tool_offset),
+ pos[Z_AXIS] - std::get<Z_AXIS>(wcs_offsets[current_wcs]) + std::get<Z_AXIS>(g92_offset) - std::get<Z_AXIS>(tool_offset)
+ );
}
//A GCode has been received
}
//A GCode has been received
- //G-letter Gcodes are mostly what the Robot module is interrested in, other modules also catch the gcode event and do stuff accordingly
if( gcode->has_g) {
switch( gcode->g ) {
if( gcode->has_g) {
switch( gcode->g ) {
- case 0: this->motion_mode = MOTION_MODE_SEEK; break;
- case 1: this->motion_mode = MOTION_MODE_LINEAR; break;
- case 2: this->motion_mode = MOTION_MODE_CW_ARC; break;
- case 3: this->motion_mode = MOTION_MODE_CCW_ARC; break;
- case 4: {
+ case 0: this->motion_mode = MOTION_MODE_SEEK; break;
+ case 1: this->motion_mode = MOTION_MODE_LINEAR; break;
+ case 2: this->motion_mode = MOTION_MODE_CW_ARC; break;
+ case 3: this->motion_mode = MOTION_MODE_CCW_ARC; break;
+ case 4: { // G4 pause
uint32_t delay_ms = 0;
if (gcode->has_letter('P')) {
delay_ms = gcode->get_int('P');
uint32_t delay_ms = 0;
if (gcode->has_letter('P')) {
delay_ms = gcode->get_int('P');
uint32_t start = us_ticker_read(); // mbed call
while ((us_ticker_read() - start) < delay_ms * 1000) {
THEKERNEL->call_event(ON_IDLE, this);
uint32_t start = us_ticker_read(); // mbed call
while ((us_ticker_read() - start) < delay_ms * 1000) {
THEKERNEL->call_event(ON_IDLE, this);
+ if(THEKERNEL->is_halted()) return;
float x, y, z;
std::tie(x, y, z) = wcs_offsets[n];
if(gcode->get_int('L') == 20) {
float x, y, z;
std::tie(x, y, z) = wcs_offsets[n];
if(gcode->get_int('L') == 20) {
- // this makes the current machine position the offset
- if(gcode->has_letter('X')) { x = to_millimeters(gcode->get_value('X')) - machine_position[X_AXIS]; }
- if(gcode->has_letter('Y')) { x = to_millimeters(gcode->get_value('Y')) - machine_position[Y_AXIS]; }
- if(gcode->has_letter('Z')) { x = to_millimeters(gcode->get_value('Z')) - machine_position[Z_AXIS]; }
+ // this makes the current machine position (less compensation transform) the offset
+ if(gcode->has_letter('X')) { x = to_millimeters(gcode->get_value('X')) - last_milestone[X_AXIS]; }
+ if(gcode->has_letter('Y')) { x = to_millimeters(gcode->get_value('Y')) - last_milestone[Y_AXIS]; }
+ if(gcode->has_letter('Z')) { x = to_millimeters(gcode->get_value('Z')) - last_milestone[Z_AXIS]; }
} else {
// the value is the offset from machine zero
if(gcode->has_letter('X')) x = to_millimeters(gcode->get_value('X'));
} else {
// the value is the offset from machine zero
if(gcode->has_letter('X')) x = to_millimeters(gcode->get_value('X'));
case 91: this->absolute_mode = false; break;
case 92: {
case 91: this->absolute_mode = false; break;
case 92: {
- wcs_t old = g92_offset;
-
- if(gcode->subcode == 1 || gcode->subcode == 2 || gcode->get_num_args() == 0) {
+ if(gcode->subcode == 1 || gcode->subcode == 2 || gcode->get_num_args() == 0) {
// reset G92 offsets to 0
g92_offset = wcs_t(0, 0, 0);
// reset G92 offsets to 0
g92_offset = wcs_t(0, 0, 0);
// standard setting of the g92 offsets, making current machine position whatever the coordinate arguments are
float x, y, z;
std::tie(x, y, z) = g92_offset;
// standard setting of the g92 offsets, making current machine position whatever the coordinate arguments are
float x, y, z;
std::tie(x, y, z) = g92_offset;
- if(gcode->has_letter('X')) x = to_millimeters(gcode->get_value('X')) - machine_position[X_AXIS];
- if(gcode->has_letter('Y')) y = to_millimeters(gcode->get_value('Y')) - machine_position[Y_AXIS];
- if(gcode->has_letter('Z')) z = to_millimeters(gcode->get_value('Z')) - machine_position[Z_AXIS];
+ if(gcode->has_letter('X')) x = to_millimeters(gcode->get_value('X')) - last_milestone[X_AXIS];
+ if(gcode->has_letter('Y')) y = to_millimeters(gcode->get_value('Y')) - last_milestone[Y_AXIS];
+ if(gcode->has_letter('Z')) z = to_millimeters(gcode->get_value('Z')) - last_milestone[Z_AXIS];
g92_offset = wcs_t(x, y, z);
}
g92_offset = wcs_t(x, y, z);
}
- if(old != g92_offset) {
- // as it changed we need to update the last_milestone to reflect the new coordinate system
- std::tie(this->last_milestone[X_AXIS], this->last_milestone[Y_AXIS], this->last_milestone[Z_AXIS]) = mcs2wcs();
- }
arm_solution->actuator_to_cartesian(current_position, mpos);
if(gcode->subcode == 0) { // M114 print WCS
arm_solution->actuator_to_cartesian(current_position, mpos);
if(gcode->subcode == 0) { // M114 print WCS
- // Note this is workspace coordinates after any bed level compensation has been applied, currently there is no way to get
- // the position we were asked for (although it should be last_milestone) without doing an inverse compensation, which is probably not a big deal.
- n = snprintf(buf, sizeof(buf), "C: X:%1.3f Y:%1.3f Z:%1.3f",
- from_millimeters(mpos[X_AXIS] - (std::get<X_AXIS>(wcs_offsets[current_wcs]) - std::get<X_AXIS>(g92_offset))),
- from_millimeters(mpos[Y_AXIS] - (std::get<Y_AXIS>(wcs_offsets[current_wcs]) - std::get<Y_AXIS>(g92_offset))),
- from_millimeters(mpos[Z_AXIS] - (std::get<Z_AXIS>(wcs_offsets[current_wcs]) - std::get<Z_AXIS>(g92_offset))) );
-
+ // FIXME this currently includes the compensation transform which is incorrect so will be slightly off if it is in effect (but by very little)
+ wcs_t pos= mcs2wcs(mpos);
+ n = snprintf(buf, sizeof(buf), "C: X:%1.3f Y:%1.3f Z:%1.3f", from_millimeters(std::get<X_AXIS>(pos)), from_millimeters(std::get<Y_AXIS>(pos)), from_millimeters(std::get<Z_AXIS>(pos)));
} else if(gcode->subcode == 1) { // M114.1 print Machine coordinate system
n = snprintf(buf, sizeof(buf), "MPOS: X:%1.3f Y:%1.3f Z:%1.3f", mpos[X_AXIS], mpos[Y_AXIS], mpos[Z_AXIS]);
} else if(gcode->subcode == 1) { // M114.1 print Machine coordinate system
n = snprintf(buf, sizeof(buf), "MPOS: X:%1.3f Y:%1.3f Z:%1.3f", mpos[X_AXIS], mpos[Y_AXIS], mpos[Z_AXIS]);
} else if(gcode->subcode == 2) { // M114.2 print actuator position
n = snprintf(buf, sizeof(buf), "APOS: A:%1.3f B:%1.3f C:%1.3f", current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS]);
} else if(gcode->subcode == 2) { // M114.2 print actuator position
n = snprintf(buf, sizeof(buf), "APOS: A:%1.3f B:%1.3f C:%1.3f", current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS]);
- } else if(gcode->subcode == 3) { // M114.3 print last milestone (which should be the same as M114 if axis are not moving and no level compensation)
- n = snprintf(buf, sizeof(buf), "LWCS: X:%1.3f Y:%1.3f Z:%1.3f", last_milestone[X_AXIS], last_milestone[Y_AXIS], last_milestone[Z_AXIS]);
+ } else if(gcode->subcode == 3) { // M114.3 print last milestone (which should be the same as machine position if axis are not moving and no level compensation)
+ n = snprintf(buf, sizeof(buf), "LMS: X:%1.3f Y:%1.3f Z:%1.3f", last_milestone[X_AXIS], last_milestone[Y_AXIS], last_milestone[Z_AXIS]);
- } else if(gcode->subcode == 4) { // M114.4 print last machins position (which should be the same as M114.1 if axis are not moving)
- n = snprintf(buf, sizeof(buf), "LMCS: X:%1.3f Y:%1.3f Z:%1.3f", machine_position[X_AXIS], machine_position[Y_AXIS], machine_position[Z_AXIS]);
+ } else if(gcode->subcode == 4) { // M114.4 print last machine position (which should be the same as M114.1 if axis are not moving and no level compensation)
+ n = snprintf(buf, sizeof(buf), "LMCS: X:%1.3f Y:%1.3f Z:%1.3f", last_machine_position[X_AXIS], last_machine_position[Y_AXIS], last_machine_position[Z_AXIS]);
if(gcode->m == 503) {
// just print the G92 setting as it is not saved
if(gcode->m == 503) {
// just print the G92 setting as it is not saved
+ // TODO linuxcnc does seem to save G92, so maybe we should here too
if(g92_offset != wcs_t(0, 0, 0)) {
float x, y, z;
std::tie(x, y, z) = g92_offset;
if(g92_offset != wcs_t(0, 0, 0)) {
float x, y, z;
std::tie(x, y, z) = g92_offset;
- if( this->motion_mode < 0) {
- next_command_is_MCS = false; // must be on same line as G0 or G1
- return;
+ if( this->motion_mode >= 0) {
+ process_move(gcode);
- // Get parameters
- float param[3], target[3], offset[3];
+ next_command_is_MCS = false; // must be on same line as G0 or G1
+}
+void Robot::process_move(Gcode *gcode)
+{
+ // we have a G0/G1/G2/G3 so extract parameters and apply offsets to get machine coordinate target
+ float param[3]{NAN, NAN, NAN};
for(int i= X_AXIS; i <= Z_AXIS; ++i) {
char letter= 'X'+i;
if( gcode->has_letter(letter) ) {
param[i] = this->to_millimeters(gcode->get_value(letter));
for(int i= X_AXIS; i <= Z_AXIS; ++i) {
char letter= 'X'+i;
if( gcode->has_letter(letter) ) {
param[i] = this->to_millimeters(gcode->get_value(letter));
- }else{
- param[i] = next_command_is_MCS ? machine_position[i] : last_milestone[i];
+ float offset[3]{0,0,0};
for(char letter = 'I'; letter <= 'K'; letter++) {
if( gcode->has_letter(letter) ) {
offset[letter - 'I'] = this->to_millimeters(gcode->get_value(letter));
}
}
for(char letter = 'I'; letter <= 'K'; letter++) {
if( gcode->has_letter(letter) ) {
offset[letter - 'I'] = this->to_millimeters(gcode->get_value(letter));
}
}
+ // calculate target in machine coordinates (less compensation transform which needs to be done after segmentation)
+ float target[3]{last_milestone[X_AXIS], last_milestone[Y_AXIS], last_milestone[Z_AXIS]};
if(!next_command_is_MCS) {
if(this->absolute_mode) {
if(!next_command_is_MCS) {
if(this->absolute_mode) {
- // apply wcs offsets and g92 offset
- target[X_AXIS]= param[X_AXIS] + std::get<X_AXIS>(wcs_offsets[current_wcs]) - std::get<X_AXIS>(g92_offset);
- target[Y_AXIS]= param[Y_AXIS] + std::get<Y_AXIS>(wcs_offsets[current_wcs]) - std::get<Y_AXIS>(g92_offset);
- target[Z_AXIS]= param[Z_AXIS] + std::get<Z_AXIS>(wcs_offsets[current_wcs]) - std::get<Z_AXIS>(g92_offset);
+ // apply wcs offsets and g92 offset and tool offset
+ if(!isnan(param[X_AXIS])) {
+ target[X_AXIS]= param[X_AXIS] + std::get<X_AXIS>(wcs_offsets[current_wcs]) - std::get<X_AXIS>(g92_offset) + std::get<X_AXIS>(tool_offset);
+ }
+
+ if(!isnan(param[Y_AXIS])) {
+ target[Y_AXIS]= param[Y_AXIS] + std::get<Y_AXIS>(wcs_offsets[current_wcs]) - std::get<Y_AXIS>(g92_offset) + std::get<Y_AXIS>(tool_offset);
+ }
+
+ if(!isnan(param[Z_AXIS])) {
+ target[Z_AXIS]= param[Z_AXIS] + std::get<Z_AXIS>(wcs_offsets[current_wcs]) - std::get<Z_AXIS>(g92_offset) + std::get<Z_AXIS>(tool_offset);
+ }
}else{
// they are deltas from the last_milestone if specified
for(int i= X_AXIS; i <= Z_AXIS; ++i) {
}else{
// they are deltas from the last_milestone if specified
for(int i= X_AXIS; i <= Z_AXIS; ++i) {
- if(gcode->has_letter('X'+i)) target[i] = param[i] + last_milestone[i];
- else target[i] = param[i];
+ if(!isnan(param[i])) target[i] = param[i] + last_milestone[i];
- // already in machine coordinates
- memcpy(target, param, sizeof target);
+ // already in machine coordinates, we do not add tool offset for that
+ for(int i= X_AXIS; i <= Z_AXIS; ++i) {
+ if(!isnan(param[i])) target[i] = param[i];
+ }
}
if( gcode->has_letter('F') ) {
}
if( gcode->has_letter('F') ) {
- if(!moved) return; // do not update last_milestone if no movement was queued
-
- // Update the last_milestone to the current target for the next time we use last_milestone, use the requested target not the adjusted one
- if(next_command_is_MCS) {
- // as the target was in machine coordinates we need to add inverse wcs offsets and g92 offset to get a reasonable equivalent last_milestone
- std::tie(this->last_milestone[X_AXIS], this->last_milestone[Y_AXIS], this->last_milestone[Z_AXIS]) = mcs2wcs();
-
- } else {
- // set last_milestome to the requested position, or what it was before
- memcpy(this->last_milestone, param, sizeof(this->last_milestone)); // this->last_milestone[] = params[];
+ if(moved) {
+ // set last_milestone to the calculated target
+ memcpy(this->last_milestone, target, sizeof(this->last_milestone));
}
// reset the machine position for all axis. Used for homing.
}
// reset the machine position for all axis. Used for homing.
-// we need to also set the last_milestone by applying the inverse offsets
+// NOTE this sets the last_milestone which is machine position before compensation transform
void Robot::reset_axis_position(float x, float y, float z)
{
void Robot::reset_axis_position(float x, float y, float z)
{
- this->machine_position[X_AXIS] = x;
- this->machine_position[Y_AXIS] = y;
- this->machine_position[Z_AXIS] = z;
+ last_machine_position[X_AXIS]= last_milestone[X_AXIS] = x;
+ last_machine_position[Y_AXIS]= last_milestone[Y_AXIS] = y;
+ last_machine_position[Z_AXIS]= last_milestone[Z_AXIS] = z;
- // calculate what the last milestone would be
- std::tie(this->last_milestone[X_AXIS], this->last_milestone[Y_AXIS], this->last_milestone[Z_AXIS]) = mcs2wcs();
+ // calculate what the last_machine_position would be by applying compensation transform if enabled
+ // otherwise it is the same as last_milestone
+ if(compensationTransform) {
+ compensationTransform(last_machine_position);
+ }
// now set the actuator positions to match
ActuatorCoordinates actuator_pos;
// now set the actuator positions to match
ActuatorCoordinates actuator_pos;
- arm_solution->cartesian_to_actuator(this->machine_position, actuator_pos);
+ arm_solution->cartesian_to_actuator(this->last_machine_position, actuator_pos);
for (size_t i = 0; i < actuators.size(); i++)
actuators[i]->change_last_milestone(actuator_pos[i]);
}
for (size_t i = 0; i < actuators.size(); i++)
actuators[i]->change_last_milestone(actuator_pos[i]);
}
// Reset the position for an axis (used in homing)
void Robot::reset_axis_position(float position, int axis)
{
// Reset the position for an axis (used in homing)
void Robot::reset_axis_position(float position, int axis)
{
- machine_position[axis] = position;
- reset_axis_position(machine_position[X_AXIS], machine_position[Y_AXIS], machine_position[Z_AXIS]);
+ last_milestone[axis] = position;
+ reset_axis_position(last_milestone[X_AXIS], last_milestone[Y_AXIS], last_milestone[Z_AXIS]);
}
// Use FK to find out where actuator is and reset to match
}
// Use FK to find out where actuator is and reset to match
for (size_t i = 0; i < actuators.size(); i++) {
actuator_pos[i] = actuators[i]->get_current_position();
}
for (size_t i = 0; i < actuators.size(); i++) {
actuator_pos[i] = actuators[i]->get_current_position();
}
- arm_solution->actuator_to_cartesian(actuator_pos, machine_position);
-
- std::tie(this->last_milestone[X_AXIS], this->last_milestone[Y_AXIS], this->last_milestone[Z_AXIS]) = mcs2wcs();
+ arm_solution->actuator_to_cartesian(actuator_pos, last_machine_position);
+ // FIXME problem is this includes any compensation transform, and without an inverse compensation we cannot get a correct last_milestone
+ memcpy(last_milestone, last_machine_position, sizeof last_milestone);
// now reset actuator correctly, NOTE this may lose a little precision
// NOTE I do not recall why this was necessary, maybe to correct for small errors in FK
// now reset actuator correctly, NOTE this may lose a little precision
// NOTE I do not recall why this was necessary, maybe to correct for small errors in FK
- // arm_solution->cartesian_to_actuator(machine_position, actuator_pos);
+ // 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]);
}
// for (size_t i = 0; i < actuators.size(); i++)
// actuators[i]->change_last_milestone(actuator_pos[i]);
}
-// Convert target from millimeters to steps, and append this to the planner
+// Convert target (in machine coordinates) from millimeters to steps, and append this to the planner
+// target is in machine coordinates without the compensation transform, however we save a last_machine_position that includes
+// all transforms and is what we actually convert to actuator positions
bool Robot::append_milestone(Gcode * gcode, const float target[], float rate_mm_s)
{
float deltas[3];
bool Robot::append_milestone(Gcode * gcode, const float target[], float rate_mm_s)
{
float deltas[3];
// find distance moved by each axis, use transformed target from the current machine position
for (int axis = X_AXIS; axis <= Z_AXIS; axis++) {
// find distance moved by each axis, use transformed target from the current machine position
for (int axis = X_AXIS; axis <= Z_AXIS; axis++) {
- deltas[axis] = transformed_target[axis] - machine_position[axis];
+ deltas[axis] = transformed_target[axis] - last_machine_position[axis];
}
// Compute how long this move moves, so we can attach it to the block for later use
}
// Compute how long this move moves, so we can attach it to the block for later use
if(millimeters_of_travel < 0.00001F) return false;
// this is the machine position
if(millimeters_of_travel < 0.00001F) return false;
// this is the machine position
- memcpy(this->machine_position, transformed_target, sizeof(this->machine_position));
-
+ memcpy(this->last_machine_position, transformed_target, sizeof(this->last_machine_position));
// find distance unit vector
for (int i = 0; i < 3; i++)
// find distance unit vector
for (int i = 0; i < 3; i++)
- // find actuator position given cartesian position, use actual adjusted target
- arm_solution->cartesian_to_actuator( transformed_target, actuator_pos );
+ // find actuator position given the machine position, use actual adjusted target
+ arm_solution->cartesian_to_actuator( this->last_machine_position, actuator_pos );
float isecs = rate_mm_s / millimeters_of_travel;
// check per-actuator speed limits
float isecs = rate_mm_s / millimeters_of_travel;
// check per-actuator speed limits
}
// Append a move to the queue ( cutting it into segments if needed )
}
// Append a move to the queue ( cutting it into segments if needed )
-bool Robot::append_line(Gcode * gcode, const float target[], float rate_mm_s )
+bool Robot::append_line(Gcode *gcode, const float target[], float rate_mm_s )
{
float last_target[]{last_milestone[X_AXIS], last_milestone[Y_AXIS], last_milestone[Z_AXIS]};
{
float last_target[]{last_milestone[X_AXIS], last_milestone[Y_AXIS], last_milestone[Z_AXIS]};
- if(next_command_is_MCS) {
- // we are in machine coordinates
- memcpy(last_target, machine_position, sizeof(last_target));
- }
-
- // Find out the distance for this move in WCS
+ // Find out the distance for this move in MCS
// NOTE we need to do sqrt here as this setting of millimeters_of_travel is used by extruder and other modules even if there is no XYZ move
gcode->millimeters_of_travel = sqrtf(powf( target[X_AXIS] - last_target[X_AXIS], 2 ) + powf( target[Y_AXIS] - last_target[Y_AXIS], 2 ) + powf( target[Z_AXIS] - last_target[Z_AXIS], 2 ));
// NOTE we need to do sqrt here as this setting of millimeters_of_travel is used by extruder and other modules even if there is no XYZ move
gcode->millimeters_of_travel = sqrtf(powf( target[X_AXIS] - last_target[X_AXIS], 2 ) + powf( target[Y_AXIS] - last_target[Y_AXIS], 2 ) + powf( target[Z_AXIS] - last_target[Z_AXIS], 2 ));
- // We cut the line into smaller segments. This is only needed on a cartesian robot for zgrid, but always necessary for robots with rotational axes.
- // In cartesian robot, a high "mm_per_line_segment" setting will prevent waste.
+ // We cut the line into smaller segments. This is only needed on a cartesian robot for zgrid, but always necessary for robots with rotational axes like Deltas.
// In delta robots either mm_per_line_segment can be used OR delta_segments_per_second
// The latter is more efficient and avoids splitting fast long lines into very small segments, like initial z move to 0, it is what Johanns Marlin delta port does
uint16_t segments;
// In delta robots either mm_per_line_segment can be used OR delta_segments_per_second
// The latter is more efficient and avoids splitting fast long lines into very small segments, like initial z move to 0, it is what Johanns Marlin delta port does
uint16_t segments;
void Robot::clearToolOffset()
{
void Robot::clearToolOffset()
{
- memset(this->toolOffset, 0, sizeof(this->toolOffset));
+ this->tool_offset= wcs_t(0,0,0);
}
void Robot::setToolOffset(const float offset[3])
{
}
void Robot::setToolOffset(const float offset[3])
{
- memcpy(this->toolOffset, offset, sizeof(this->toolOffset));
+ this->tool_offset= wcs_t(offset[0], offset[1], offset[2]);
bool append_line( Gcode* gcode, const float target[], float rate_mm_s);
bool append_arc( Gcode* gcode, const float target[], const float offset[], float radius, bool is_clockwise );
bool compute_arc(Gcode* gcode, const float offset[], const float target[]);
bool append_line( Gcode* gcode, const float target[], float rate_mm_s);
bool append_arc( Gcode* gcode, const float target[], const float offset[], float radius, bool is_clockwise );
bool compute_arc(Gcode* gcode, const float offset[], const float target[]);
+ void process_move(Gcode *gcode);
float theta(float x, float y);
void select_plane(uint8_t axis_0, uint8_t axis_1, uint8_t axis_2);
float theta(float x, float y);
void select_plane(uint8_t axis_0, uint8_t axis_1, uint8_t axis_2);
// Workspace coordinate systems
using wcs_t= std::tuple<float, float, float>;
// Workspace coordinate systems
using wcs_t= std::tuple<float, float, float>;
+ wcs_t mcs2wcs(const float *pos) const;
+ wcs_t mcs2wcs() const { return mcs2wcs(last_machine_position); }
+
static const size_t k_max_wcs= 9; // setup 9 WCS offsets
std::array<wcs_t, k_max_wcs> wcs_offsets; // these are persistent once set
uint8_t current_wcs{0}; // 0 means G54 in enabled this is persistent
wcs_t g92_offset;
static const size_t k_max_wcs= 9; // setup 9 WCS offsets
std::array<wcs_t, k_max_wcs> wcs_offsets; // these are persistent once set
uint8_t current_wcs{0}; // 0 means G54 in enabled this is persistent
wcs_t g92_offset;
+ wcs_t tool_offset; // used for multiple extruders, sets the tool offset for the current extruder applied first
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
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
- float last_milestone[3]; // Last requested position, in millimeters, which is what we were requested to move to in the gcode
- float machine_position[3]; // Last machine position, which is th eposition before converting to actuator coordinates
+ float last_milestone[3]; // Last requested position, in millimeters, which is what we were requested to move to in the gcode after offsets applied but before compensation transform
+ float last_machine_position[3]; // Last machine position, which is the position before converting to actuator coordinates (includes compensation transform)
int8_t motion_mode; // Motion mode for the current received Gcode
uint8_t plane_axis_0, plane_axis_1, plane_axis_2; // Current plane ( XY, XZ, YZ )
float seek_rate; // Current rate for seeking moves ( mm/s )
int8_t motion_mode; // Motion mode for the current received Gcode
uint8_t plane_axis_0, plane_axis_1, plane_axis_2; // Current plane ( XY, XZ, YZ )
float seek_rate; // Current rate for seeking moves ( mm/s )
int arc_correction; // Setting : how often to rectify arc computation
float max_speeds[3]; // Setting : max allowable speed in mm/m for each axis
int arc_correction; // Setting : how often to rectify arc computation
float max_speeds[3]; // Setting : max allowable speed in mm/m for each axis
// Used by Stepper, Planner
friend class Planner;
friend class Stepper;
// Used by Stepper, Planner
friend class Planner;
friend class Stepper;
return this->inch_mode ? value/25.4F : value;
}
inline void Robot::get_axis_position(float position[]){
return this->inch_mode ? value/25.4F : value;
}
inline void Robot::get_axis_position(float position[]){
- memcpy(position, this->last_milestone, sizeof(float)*3 );
+ memcpy(position, this->last_milestone, sizeof this->last_milestone);
#include "libs/StreamOutput.h"
#include "FileStream.h"
#include "libs/StreamOutput.h"
#include "FileStream.h"
-#define return_error_on_unhandled_gcode_checksum CHECKSUM("return_error_on_unhandled_gcode")
-
-ToolManager::ToolManager(){
+ToolManager::ToolManager()
+{
active_tool = 0;
current_tool_name = CHECKSUM("hotend");
}
active_tool = 0;
current_tool_name = CHECKSUM("hotend");
}
-void ToolManager::on_module_loaded(){
- this->on_config_reload(this);
+void ToolManager::on_module_loaded()
+{
this->register_for_event(ON_GCODE_RECEIVED);
this->register_for_event(ON_GET_PUBLIC_DATA);
this->register_for_event(ON_SET_PUBLIC_DATA);
}
this->register_for_event(ON_GCODE_RECEIVED);
this->register_for_event(ON_GET_PUBLIC_DATA);
this->register_for_event(ON_SET_PUBLIC_DATA);
}
-void ToolManager::on_config_reload(void *argument){
- return_error_on_unhandled_gcode = THEKERNEL->config->value( return_error_on_unhandled_gcode_checksum )->by_default(false)->as_bool();
-}
-
-void ToolManager::on_gcode_received(void *argument){
+void ToolManager::on_gcode_received(void *argument)
+{
Gcode *gcode = static_cast<Gcode*>(argument);
Gcode *gcode = static_cast<Gcode*>(argument);
- if( gcode->has_letter('T') ){
+ if( gcode->has_letter('T') ) {
int new_tool = gcode->get_value('T');
int new_tool = gcode->get_value('T');
- if(new_tool >= (int)this->tools.size() || new_tool < 0){
+ if(new_tool >= (int)this->tools.size() || new_tool < 0) {
- if( return_error_on_unhandled_gcode ) {
- char buf[32]; // should be big enough for any status
- int n= snprintf(buf, sizeof(buf), "T%d invalid tool ", new_tool);
- gcode->txt_after_ok.append(buf, n);
- }
+ char buf[32]; // should be big enough for any status
+ int n = snprintf(buf, sizeof(buf), "T%d invalid tool ", new_tool);
+ gcode->txt_after_ok.append(buf, n);
+
- if(new_tool != this->active_tool){
+ if(new_tool != this->active_tool) {
// We must wait for an empty queue before we can disable the current extruder
THEKERNEL->conveyor->wait_for_empty_queue();
this->tools[active_tool]->disable();
// We must wait for an empty queue before we can disable the current extruder
THEKERNEL->conveyor->wait_for_empty_queue();
this->tools[active_tool]->disable();
-void ToolManager::on_get_public_data(void* argument){
+void ToolManager::on_get_public_data(void* argument)
+{
PublicDataRequest* pdr = static_cast<PublicDataRequest*>(argument);
if(!pdr->starts_with(tool_manager_checksum)) return;
if(!pdr->second_element_is(is_active_tool_checksum)) return;
// check that we control the given tool
PublicDataRequest* pdr = static_cast<PublicDataRequest*>(argument);
if(!pdr->starts_with(tool_manager_checksum)) return;
if(!pdr->second_element_is(is_active_tool_checksum)) return;
// check that we control the given tool
- uint16_t n= t->get_name();
- if(pdr->third_element_is(n)){
- managed= true;
+ uint16_t n = t->get_name();
+ if(pdr->third_element_is(n)) {
+ managed = true;
-void ToolManager::on_set_public_data(void* argument){
+void ToolManager::on_set_public_data(void* argument)
+{
PublicDataRequest* pdr = static_cast<PublicDataRequest*>(argument);
if(!pdr->starts_with(tool_manager_checksum)) return;
PublicDataRequest* pdr = static_cast<PublicDataRequest*>(argument);
if(!pdr->starts_with(tool_manager_checksum)) return;
}
// Add a tool to the tool list
}
// Add a tool to the tool list
-void ToolManager::add_tool(Tool* tool_to_add){
- if(this->tools.size() == 0){
+void ToolManager::add_tool(Tool* tool_to_add)
+{
+ if(this->tools.size() == 0) {
tool_to_add->enable();
this->current_tool_name = tool_to_add->get_name();
//send new_tool_offsets to robot
tool_to_add->enable();
this->current_tool_name = tool_to_add->get_name();
//send new_tool_offsets to robot
void on_module_loaded();
void on_gcode_received(void *);
void on_module_loaded();
void on_gcode_received(void *);
- void on_config_reload(void *);
void on_get_public_data(void *argument);
void on_set_public_data(void *argument);
void add_tool(Tool *tool_to_add);
void on_get_public_data(void *argument);
void on_set_public_data(void *argument);
void add_tool(Tool *tool_to_add);
int active_tool;
uint16_t current_tool_name;
int active_tool;
uint16_t current_tool_name;
- bool return_error_on_unhandled_gcode;