}
}
-void Robot::print_position(Gcode *gcode) const
+std::vector<Robot::wcs_t> Robot::get_wcs_state() const
+{
+ std::vector<wcs_t> v;
+ v.push_back(wcs_t(current_wcs, k_max_wcs, 0));
+ for(auto& i : wcs_offsets) {
+ v.push_back(i);
+ }
+ v.push_back(g92_offset);
+ v.push_back(tool_offset);
+ return v;
+}
+
+int Robot::print_position(uint8_t subcode, char *buf, size_t bufsize) const
{
// M114.1 is a new way to do this (similar to how GRBL does it).
// it returns the realtime position based on the current step position of the actuators.
// this does require a FK to get a machine position from the actuator position
// and then invert all the transforms to get a workspace position from machine position
// M114 just does it the old way uses last_milestone and does inversse tranfroms to get the requested position
- char buf[64];
int n = 0;
- if(gcode->subcode == 0) { // M114 print WCS
+ if(subcode == 0) { // M114 print WCS
wcs_t pos= mcs2wcs(last_milestone);
- 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)));
+ n = snprintf(buf, bufsize, "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 == 4) { // 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(subcode == 4) { // 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, bufsize, "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 == 5) { // 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]);
+ } else if(subcode == 5) { // 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, bufsize, "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]);
} else {
// get real time positions
float mpos[3];
arm_solution->actuator_to_cartesian(current_position, mpos);
- if(gcode->subcode == 1) { // M114.1 print realtime WCS
+ if(subcode == 1) { // M114.1 print realtime WCS
// 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)));
+ n = snprintf(buf, bufsize, "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 == 2) { // M114.1 print realtime 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(subcode == 2) { // M114.1 print realtime Machine coordinate system
+ n = snprintf(buf, bufsize, "MPOS: X:%1.3f Y:%1.3f Z:%1.3f", mpos[X_AXIS], mpos[Y_AXIS], mpos[Z_AXIS]);
- } else if(gcode->subcode == 3) { // M114.2 print realtime 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(subcode == 3) { // M114.2 print realtime actuator position
+ n = snprintf(buf, bufsize, "APOS: A:%1.3f B:%1.3f C:%1.3f", current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS]);
}
}
- if(n > 0)
- gcode->txt_after_ok.append(buf, n);
+ return n;
+}
+
+// converts current last milestone (machine position without compensation transform) to work coordinate system (inverse transform)
+Robot::wcs_t Robot::mcs2wcs(const float *pos) const
+{
+ return std::make_tuple(
+ 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)
+ );
}
// this does a sanity check that actuator speeds do not exceed steps rate capability
}
}
-// converts current last milestone (machine position without compensation transform) to work coordinate system (inverse transform)
-Robot::wcs_t Robot::mcs2wcs(const float *pos) const
-{
- return std::make_tuple(
- 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
//See if the current Gcode line has some orders for us
void Robot::on_gcode_received(void *argument)
std::tie(x, y, z) = wcs_offsets[n];
if(gcode->get_int('L') == 20) {
// 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]; }
+ // get current position in WCS
+ wcs_t pos= mcs2wcs(last_milestone);
+
+ if(gcode->has_letter('X')){
+ x -= to_millimeters(gcode->get_value('X')) - std::get<X_AXIS>(pos);
+ }
+
+ if(gcode->has_letter('Y')){
+ y -= to_millimeters(gcode->get_value('Y')) - std::get<Y_AXIS>(pos);
+ }
+ if(gcode->has_letter('Z')) {
+ z -= to_millimeters(gcode->get_value('Z')) - std::get<Z_AXIS>(pos);
+ }
+
} else {
// the value is the offset from machine zero
if(gcode->has_letter('X')) x = to_millimeters(gcode->get_value('X'));
if(gcode->has_letter('X')){
x += to_millimeters(gcode->get_value('X')) - std::get<X_AXIS>(pos);
}
-
- if(gcode->has_letter('Y')) y = to_millimeters(gcode->get_value('Y')) - last_milestone[Y_AXIS] - std::get<Y_AXIS>(wcs_offsets[current_wcs]) - std::get<Y_AXIS>(tool_offset);
- if(gcode->has_letter('Z')) z = to_millimeters(gcode->get_value('Z')) - last_milestone[Z_AXIS] - std::get<Z_AXIS>(wcs_offsets[current_wcs]) - std::get<Z_AXIS>(tool_offset);
+ if(gcode->has_letter('Y')){
+ y += to_millimeters(gcode->get_value('Y')) - std::get<Y_AXIS>(pos);
+ }
+ if(gcode->has_letter('Z')) {
+ z += to_millimeters(gcode->get_value('Z')) - std::get<Z_AXIS>(pos);
+ }
g92_offset = wcs_t(x, y, z);
}
check_max_actuator_speeds();
return;
- case 114:
- print_position(gcode);
+ case 114:{
+ char buf[64];
+ int n= print_position(gcode->subcode, buf, sizeof buf);
+ if(n > 0) gcode->txt_after_ok.append(buf, n);
return;
+ }
case 120: // push state
push_state();
}
// reset the machine position for all axis. Used for homing.
-// NOTE this sets the last_milestone which is machine position before compensation transform
+// During homing compensation is turned off (actually not used as it drives steppers directly)
+// once homed and reset_axis called compensation is used for the move to origin and back off home if enabled,
+// so in those cases the final position is compensated.
void Robot::reset_axis_position(float x, float y, float z)
{
+ // these are set to the same as compensation was not used to get to the current position
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_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;
arm_solution->cartesian_to_actuator(this->last_machine_position, actuator_pos);
if (segments > 1) {
// A vector to keep track of the endpoint of each segment
float segment_delta[3];
- float segment_end[3];
+ float segment_end[3]{last_milestone[X_AXIS], last_milestone[Y_AXIS], last_milestone[Z_AXIS]};
// How far do we move each segment?
for (int i = X_AXIS; i <= Z_AXIS; i++)
for (int i = 1; i < segments; i++) {
if(THEKERNEL->is_halted()) return false; // don't queue any more segments
for(int axis = X_AXIS; axis <= Z_AXIS; axis++ )
- segment_end[axis] = last_milestone[axis] + segment_delta[axis];
+ segment_end[axis] += segment_delta[axis];
// Append the end of this segment to the queue
bool b= this->append_milestone(gcode, segment_end, rate_mm_s);
HCoop / clinton / Smoothieware.git / blobdiff