- char buf[32];
- int n = snprintf(buf, sizeof(buf), "C: X:%1.3f Y:%1.3f Z:%1.3f",
- from_millimeters(this->last_milestone[0]),
- from_millimeters(this->last_milestone[1]),
- from_millimeters(this->last_milestone[2]));
- gcode->txt_after_ok.append(buf, n);
- gcode->mark_as_taken();
+ // this 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
+ char buf[64];
+ int n = 0;
+ // current actuator position in mm
+ ActuatorCoordinates current_position{
+ actuators[X_AXIS]->get_current_position(),
+ actuators[Y_AXIS]->get_current_position(),
+ actuators[Z_AXIS]->get_current_position()
+ };
+
+ // get machine position from the actuator position using FK
+ float mpos[3];
+ arm_solution->actuator_to_cartesian(current_position, mpos);
+
+ if(gcode->subcode == 0) { // M114 print 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)));
+
+ } 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 == 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 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(n > 0)
+ gcode->txt_after_ok.append(buf, n);