mirror/Grbl_Esp32
1
0
Fork 0
mirror of https://github.com/bdring/Grbl_Esp32.git synced 2025-09-02 10:53:01 +02:00
Code Issues Releases Wiki Activity

MPos now has forward kinematics

Browse Source
This commit is contained in:
bdring
2020-12-19 07:11:28 -06:00
parent 4ced81ad06
commit e6faee455b
2 changed files with 14 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files

18
Grbl_Esp32/Custom/CoreXY.cpp
View File

@@ -292,7 +292,7 @@ void inverse_kinematics(float* target, plan_line_data_t* pl_data, float* positio
} }
// motors -> cartesian // motors -> cartesian
void forward_kinematics(float* position) { void forward_kinematics(float* position) { // Note: the Position value is not used. sys_position is used
float calc_fwd[MAX_N_AXIS]; float calc_fwd[MAX_N_AXIS];
float wco[MAX_N_AXIS]; float wco[MAX_N_AXIS];
float print_position[N_AXIS]; float print_position[N_AXIS];
@@ -301,13 +301,19 @@ void forward_kinematics(float* position) {
memcpy(current_position, sys_position, sizeof(sys_position)); memcpy(current_position, sys_position, sizeof(sys_position));
system_convert_array_steps_to_mpos(print_position, current_position); system_convert_array_steps_to_mpos(print_position, current_position);
// determine the Work Coordinate Offsets for each axis // If we want to see WPos, apply the offsets
auto n_axis = number_axis->get(); auto n_axis = number_axis->get();
for (int axis = 0; axis < n_axis; axis++) { for (int axis = 0; axis < n_axis; axis++) {
// Apply work coordinate offsets and tool length offset to current position. // determine the Work Coordinate Offsets for each axis
wco[axis] = gc_state.coord_system[axis] + gc_state.coord_offset[axis]; if (bit_isfalse(status_mask->get(), RtStatus::Position)) {
if (axis == TOOL_LENGTH_OFFSET_AXIS) { // Apply work coordinate offsets and tool length offset to current position.
wco[axis] += gc_state.tool_length_offset; wco[axis] = gc_state.coord_system[axis] + gc_state.coord_offset[axis];
if (axis == TOOL_LENGTH_OFFSET_AXIS) {
wco[axis] += gc_state.tool_length_offset;
}
} else { // MPos
wco[axis] = 0.0;
} }
} }

3
Grbl_Esp32/src/Report.cpp
View File

@@ -655,11 +655,12 @@ void report_realtime_status(uint8_t client) {
} }
} }
} }
// allow a custom machine to apply forward kinematics
forward_kinematics(print_position); // a weak definition does nothing. Users can provide strong version
// Report machine position // Report machine position
if (bit_istrue(status_mask->get(), RtStatus::Position)) { if (bit_istrue(status_mask->get(), RtStatus::Position)) {
strcat(status, "|MPos:"); strcat(status, "|MPos:");
} else { } else {
forward_kinematics(print_position); // a weak definition does nothing. Users can provide strong version
strcat(status, "|WPos:"); strcat(status, "|WPos:");
} }
report_util_axis_values(print_position, temp); report_util_axis_values(print_position, temp);