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

Core XY fixes (#756)

Browse Source 
* Updates for CoreXY

* Delete fystec_ant.h
This commit is contained in:
bdring
2021-02-05 14:53:02 -06:00
committed by GitHub
parent 8ddec618cd
commit 447d1739f3
6 changed files with 67 additions and 32 deletions
Download Patch File Download Diff File Expand all files Collapse all files

71
Grbl_Esp32/Custom/CoreXY.cpp
View File

@@ -69,7 +69,8 @@ bool user_defined_homing(uint8_t cycle_mask) {
// check for multi axis homing per cycle ($Homing/Cycle0=XY type)...not allowed in CoreXY
bool setting_error = false;
for (int cycle = 0; cycle < 3; cycle++) {
auto n_axis = number_axis->get();
for (int cycle = 0; cycle < n_axis; cycle++) {
if (numberOfSetBits(homing_cycle[cycle]->get()) > 1) {
grbl_msg_sendf(CLIENT_SERIAL,
MsgLevel::Info,
@@ -90,7 +91,7 @@ bool user_defined_homing(uint8_t cycle_mask) {
pl_data->motion.systemMotion = 1;
pl_data->motion.noFeedOverride = 1;
uint8_t cycle_count = (cycle_mask == 0) ? 3 : 1; // if we have a cycle_mask, we are only going to do one axis
uint8_t cycle_count = (cycle_mask == 0) ? n_axis : 1; // if we have a cycle_mask, we are only going to do one axis
AxisMask mask = 0;
for (int cycle = 0; cycle < cycle_count; cycle++) {
@@ -105,7 +106,7 @@ bool user_defined_homing(uint8_t cycle_mask) {
mask = motors_set_homing_mode(mask, true); // non standard homing motors will do their own thing and get removed from the mask
for (uint8_t axis = X_AXIS; axis <= Z_AXIS; axis++) {
for (uint8_t axis = X_AXIS; axis <= n_axis; axis++) {
if (bit(axis) == mask) {
// setup for the homing of this axis
bool approach = true;
@@ -129,7 +130,9 @@ bool user_defined_homing(uint8_t cycle_mask) {
approach ? target[axis] = max_travel : target[axis] = -max_travel;
}
target[Z_AXIS] = system_convert_axis_steps_to_mpos(sys_position, Z_AXIS);
for (int axis = Z_AXIS; axis < n_axis; axis++) {
target[axis] = system_convert_axis_steps_to_mpos(sys_position, axis);
}
// convert back to motor steps
inverse_kinematics(target);
@@ -217,7 +220,10 @@ bool user_defined_homing(uint8_t cycle_mask) {
last_cartesian[X_AXIS] = target[X_AXIS];
last_cartesian[Y_AXIS] = target[Y_AXIS];
last_cartesian[Z_AXIS] = system_convert_axis_steps_to_mpos(sys_position, Z_AXIS);
for (int axis = Z_AXIS; axis < n_axis; axis++) {
last_cartesian[axis] = system_convert_axis_steps_to_mpos(sys_position, axis);
}
// convert to motors
inverse_kinematics(target);
@@ -239,15 +245,15 @@ bool user_defined_homing(uint8_t cycle_mask) {
// This function is used by Grbl convert Cartesian to motors
// this does not do any motion control
void inverse_kinematics(float* position) {
float motors[3];
float motors[MAX_N_AXIS];
motors[X_AXIS] = geometry_factor * position[X_AXIS] + position[Y_AXIS];
motors[Y_AXIS] = geometry_factor * position[X_AXIS] - position[Y_AXIS];
motors[Z_AXIS] = position[Z_AXIS];
position[0] = motors[0];
position[1] = motors[1];
position[2] = motors[2];
position[X_AXIS] = motors[X_AXIS];
position[Y_AXIS] = motors[Y_AXIS];
// Z and higher just pass through unchanged
}
// Inverse Kinematics calculates motor positions from real world cartesian positions
@@ -268,7 +274,11 @@ void inverse_kinematics(float* target, plan_line_data_t* pl_data, float* positio
motors[X_AXIS] = geometry_factor * target[X_AXIS] + target[Y_AXIS];
motors[Y_AXIS] = geometry_factor * target[X_AXIS] - target[Y_AXIS];
motors[Z_AXIS] = target[Z_AXIS];
auto n_axis = number_axis->get();
for (uint8_t axis = Z_AXIS; axis <= n_axis; axis++) {
motors[axis] = target[axis];
}
float motor_distance = three_axis_dist(motors, last_motors);
@@ -283,14 +293,36 @@ void inverse_kinematics(float* target, plan_line_data_t* pl_data, float* positio
// motors -> cartesian
void forward_kinematics(float* position) {
float calc_fwd[MAX_N_AXIS];
float calc_fwd[MAX_N_AXIS];
float wco[MAX_N_AXIS];
float print_position[N_AXIS];
int32_t current_position[N_AXIS]; // Copy current state of the system position variable
memcpy(current_position, sys_position, sizeof(sys_position));
system_convert_array_steps_to_mpos(print_position, current_position);
// determine the Work Coordinate Offsets for each axis
auto n_axis = number_axis->get();
for (int axis = 0; axis < n_axis; axis++) {
// Apply work coordinate offsets and tool length offset to current position.
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;
}
}
// apply the forward kinemetics to the machine coordinates
// https://corexy.com/theory.html
calc_fwd[X_AXIS] = 0.5 / geometry_factor * (position[X_AXIS] + position[Y_AXIS]);
calc_fwd[Y_AXIS] = 0.5 * (position[X_AXIS] - position[Y_AXIS]);
//calc_fwd[X_AXIS] = 0.5 / geometry_factor * (position[X_AXIS] + position[Y_AXIS]);
calc_fwd[X_AXIS] = ((0.5 * (print_position[X_AXIS] + print_position[Y_AXIS]) * geometry_factor) - wco[X_AXIS]);
calc_fwd[Y_AXIS] = ((0.5 * (print_position[X_AXIS] - print_position[Y_AXIS])) - wco[Y_AXIS]);
position[X_AXIS] = calc_fwd[X_AXIS];
position[Y_AXIS] = calc_fwd[Y_AXIS];
for (int axis = 0; axis < n_axis; axis++) {
if (axis > Y_AXIS) { // for axes above Y there is no kinematics
calc_fwd[axis] = print_position[axis] - wco[axis];
}
position[axis] = calc_fwd[axis]; // this is the value returned to reporting
}
}
bool kinematics_pre_homing(uint8_t cycle_mask) {
@@ -298,9 +330,10 @@ bool kinematics_pre_homing(uint8_t cycle_mask) {
}
void kinematics_post_homing() {
gc_state.position[X_AXIS] = last_cartesian[X_AXIS];
gc_state.position[Y_AXIS] = last_cartesian[Y_AXIS];
gc_state.position[Z_AXIS] = last_cartesian[Z_AXIS];
auto n_axis = number_axis->get();
for (uint8_t axis = X_AXIS; axis <= n_axis; axis++) {
gc_state.position[axis] = last_cartesian[axis];
}
}
// this is used used by Grbl soft limits to see if the range of the machine is exceeded.

7
Grbl_Esp32/src/Grbl.cpp
View File

@@ -42,10 +42,7 @@ void grbl_init() {
init_motors();
system_ini(); // Configure pinout pins and pin-change interrupt (Renamed due to conflict with esp32 files)
memset(sys_position, 0, sizeof(sys_position)); // Clear machine position.
#ifdef USE_MACHINE_INIT
machine_init(); // user supplied function for special initialization
#endif
machine_init(); // weak definition in Grbl.cpp does nothing
// Initialize system state.
#ifdef FORCE_INITIALIZATION_ALARM
// Force Grbl into an ALARM state upon a power-cycle or hard reset.
@@ -117,6 +114,8 @@ void run_once() {
protocol_main_loop();
}
void __attribute__((weak)) machine_init() {}
/*
setup() and loop() in the Arduino .ino implements this control flow:

7
Grbl_Esp32/src/Grbl.h
View File

@@ -23,7 +23,7 @@
// Grbl versioning system
const char* const GRBL_VERSION = "1.3a";
const char* const GRBL_VERSION_BUILD = "20210203";
const char* const GRBL_VERSION_BUILD = "20210204";
//#include <sdkconfig.h>
#include <Arduino.h>
@@ -94,8 +94,7 @@ void run_once();
// Called if USE_MACHINE_INIT is defined
void machine_init();
// Called if USE_CUSTOM_HOMING is defined
bool user_defined_homing(uint8_t cycle_mask);
bool user_defined_homing(uint8_t cycle_mask); // weak definition in Limits.cpp
// Called if USE_KINEMATICS is defined
@@ -106,7 +105,7 @@ uint8_t kinematic_limits_check(float* target);
// Called if USE_FWD_KINEMATICS is defined
void inverse_kinematics(float* position); // used to return a converted value
void forward_kinematics(float* position);
void forward_kinematics(float* position); // weak definition in Report.cpp
// Called if MACRO_BUTTON_0_PIN or MACRO_BUTTON_1_PIN or MACRO_BUTTON_2_PIN is defined
void user_defined_macro(uint8_t index);

4
Grbl_Esp32/src/Limits.cpp
View File

@@ -404,3 +404,7 @@ bool limitsCheckTravel(float* target) {
}
return false;
}
bool __attribute__((weak)) user_defined_homing(uint8_t cycle_mask) {
return false;
}

4
Grbl_Esp32/src/MotionControl.cpp
View File

@@ -283,11 +283,11 @@ static bool axis_is_squared(uint8_t axis_mask) {
// executing the homing cycle. This prevents incorrect buffered plans after homing.
void mc_homing_cycle(uint8_t cycle_mask) {
bool no_cycles_defined = true;
#ifdef USE_CUSTOM_HOMING
if (user_defined_homing(cycle_mask)) {
return;
}
#endif
// This give kinematics a chance to do something before normal homing
// if it returns true, the homing is canceled.
#ifdef USE_KINEMATICS

6
Grbl_Esp32/src/Report.cpp
View File

@@ -659,13 +659,11 @@ void report_realtime_status(uint8_t client) {
}
}
}
forward_kinematics(print_position); // a weak definition does nothing. Users can provide strong version
// Report machine position
if (bit_istrue(status_mask->get(), RtStatus::Position)) {
strcat(status, "|MPos:");
} else {
#ifdef USE_FWD_KINEMATICS
forward_kinematics(print_position);
#endif
strcat(status, "|WPos:");
}
report_util_axis_values(print_position, temp);
@@ -957,3 +955,5 @@ void reportTaskStackSize(UBaseType_t& saved) {
}
#endif
}
void __attribute__((weak)) forward_kinematics(float* position) {} // This version does nothing. Make your own to do something with it