#define z_axis_max_speed_checksum CHECKSUM("z_axis_max_speed")
#define segment_z_moves_checksum CHECKSUM("segment_z_moves")
#define save_g92_checksum CHECKSUM("save_g92")
+#define set_g92_checksum CHECKSUM("set_g92")
// arm solutions
#define arm_solution_checksum CHECKSUM("arm_solution")
this->segment_z_moves = THEKERNEL->config->value(segment_z_moves_checksum )->by_default(true)->as_bool();
this->save_g92 = THEKERNEL->config->value(save_g92_checksum )->by_default(false)->as_bool();
+ string g92 = THEKERNEL->config->value(set_g92_checksum )->by_default("")->as_string();
+ if(!g92.empty()) {
+ // optional setting for a fixed G92 offset
+ std::vector<float> t= parse_number_list(g92.c_str());
+ if(t.size() == 3) {
+ g92_offset = wcs_t(t[0], t[1], t[2]);
+ }
+ }
// default s value for laser
this->s_value = THEKERNEL->config->value(laser_module_default_power_checksum)->by_default(0.8F)->as_number();
return v;
}
+void Robot::get_current_machine_position(float *pos) const
+{
+ // get real time 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
+ arm_solution->actuator_to_cartesian(current_position, pos);
+}
+
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).
} else {
// get real time positions
- // 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);
+ get_current_machine_position(mpos);
+
// current_position/mpos includes the compensation transform so we need to get the inverse to get actual position
if(compensationTransform) compensationTransform(mpos, true); // get inverse compensation transform
if(subcode == 1) { // M114.1 print realtime WCS
wcs_t pos= mcs2wcs(mpos);
- n = snprintf(buf, bufsize, "WPOS: X:%1.4f Y:%1.4f Z:%1.4f", 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, "WCS: X:%1.4f Y:%1.4f Z:%1.4f", from_millimeters(std::get<X_AXIS>(pos)), from_millimeters(std::get<Y_AXIS>(pos)), from_millimeters(std::get<Z_AXIS>(pos)));
} else if(subcode == 2) { // M114.2 print realtime Machine coordinate system
- n = snprintf(buf, bufsize, "MPOS: X:%1.4f Y:%1.4f Z:%1.4f", mpos[X_AXIS], mpos[Y_AXIS], mpos[Z_AXIS]);
+ n = snprintf(buf, bufsize, "MCS: X:%1.4f Y:%1.4f Z:%1.4f", mpos[X_AXIS], mpos[Y_AXIS], mpos[Z_AXIS]);
} else if(subcode == 3) { // M114.3 print realtime actuator position
+ // get real time 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()
+ };
n = snprintf(buf, bufsize, "APOS: X:%1.4f Y:%1.4f Z:%1.4f", current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS]);
}
}
}
// reset the machine position for all axis. Used for homing.
-// after homing we need to set the actuator position at the position they would be at for the compensated XYZ
-// So we need to apply the compensation transform to the last_milestone we are given to get the compensated
-// last_machine_position which we then convert to actuator position.
+// after homing we supply the cartesian coordinates that the head is at when homed,
+// however for Z this is the compensated Z position (if enabled)
+// So we need to apply the inverse compensation transform to the supplied coordinates to get the correct last milestone
// this will make the results from M114 and ? consistent after homing.
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
+ // set both the same initially
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;
if(compensationTransform) {
- compensationTransform(last_machine_position, false);
+ // apply inverse transform to get last_milestone
+ compensationTransform(last_milestone, true);
}
- // now set the actuator positions to match
+ // now set the actuator positions based on the supplied compensated position
ActuatorCoordinates actuator_pos;
arm_solution->cartesian_to_actuator(this->last_machine_position, actuator_pos);
for (size_t i = X_AXIS; i <= Z_AXIS; i++)
// Reset the position for an axis (used in homing, and to reset extruder after suspend)
void Robot::reset_axis_position(float position, int axis)
{
- last_milestone[axis] = position;
+ last_machine_position[axis] = position;
if(axis <= Z_AXIS) {
- reset_axis_position(last_milestone[X_AXIS], last_milestone[Y_AXIS], last_milestone[Z_AXIS]);
+ reset_axis_position(last_machine_position[X_AXIS], last_machine_position[Y_AXIS], last_machine_position[Z_AXIS]);
#if MAX_ROBOT_ACTUATORS > 3
}else{
// extruders need to be set not calculated
// then sets the axis positions to match. currently only called from Endstops.cpp and RotaryDeltaCalibration.cpp
void Robot::reset_actuator_position(const ActuatorCoordinates &ac)
{
- for (size_t i = X_AXIS; i <= Z_AXIS; i++)
- actuators[i]->change_last_milestone(ac[i]);
+ for (size_t i = X_AXIS; i <= Z_AXIS; i++) {
+ if(!isnan(ac[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();
HCoop / clinton / Smoothieware.git / blobdiff