diff --git a/Grbl_Esp32/Custom/CoreXY.cpp b/Grbl_Esp32/Custom/CoreXY.cpp
index 63e0eaa5..39488722 100644
--- a/Grbl_Esp32/Custom/CoreXY.cpp
+++ b/Grbl_Esp32/Custom/CoreXY.cpp
@@ -43,7 +43,6 @@ const float geometry_factor = 1.0;
const float geometry_factor = 2.0;
#endif
-static float last_motors[MAX_N_AXIS] = { 0.0 }; // A place to save the previous motor angles for distance/feed rate calcs
static float last_cartesian[MAX_N_AXIS] = {};
// prototypes for helper functions
@@ -144,7 +143,7 @@ bool user_defined_homing(uint8_t cycle_mask) {
}
for (int axis = Z_AXIS; axis < n_axis; axis++) {
- target[axis] = system_convert_axis_steps_to_mpos(sys_position, axis);
+ target[axis] = sys_position[axis] / axis_settings[axis]->steps_per_mm->get();
}
// convert back to motor steps
@@ -235,7 +234,7 @@ bool user_defined_homing(uint8_t cycle_mask) {
last_cartesian[Y_AXIS] = target[Y_AXIS];
for (int axis = Z_AXIS; axis < n_axis; axis++) {
- last_cartesian[axis] = system_convert_axis_steps_to_mpos(sys_position, axis);
+ last_cartesian[axis] = sys_position[axis] / axis_settings[axis]->steps_per_mm->get();
}
// convert to motors
@@ -255,15 +254,21 @@ bool user_defined_homing(uint8_t cycle_mask) {
return true;
}
-// Inverse Kinematics calculates motor positions from real world cartesian positions
-// position is the current position
-// Breaking into segments is not needed with CoreXY, because it is a linear system.
-bool inverse_kinematics(float* target, plan_line_data_t* pl_data, float* position) //The target and position are provided in MPos
-{
- float dx, dy, dz; // distances in each cartesian axis
- float motors[MAX_N_AXIS];
+static void transform_cartesian_to_motors(float* motors, float* cartesian) {
+ motors[X_AXIS] = geometry_factor * cartesian[X_AXIS] + cartesian[Y_AXIS];
+ motors[Y_AXIS] = geometry_factor * cartesian[X_AXIS] - cartesian[Y_AXIS];
- float feed_rate = pl_data->feed_rate; // save original feed rate
+ auto n_axis = number_axis->get();
+ for (uint8_t axis = Z_AXIS; axis <= n_axis; axis++) {
+ motors[axis] = cartesian[axis];
+ }
+}
+
+// Inverse Kinematics calculates motor positions from real world cartesian positions
+// position is the old machine position, target the new machine position
+// Breaking into segments is not needed with CoreXY, because it is a linear system.
+bool cartesian_to_motors(float* target, plan_line_data_t* pl_data, float* position) {
+ float dx, dy, dz; // distances in each cartesian axis
// calculate cartesian move distance for each axis
dx = target[X_AXIS] - position[X_AXIS];
@@ -271,56 +276,30 @@ bool inverse_kinematics(float* target, plan_line_data_t* pl_data, float* positio
dz = target[Z_AXIS] - position[Z_AXIS];
float dist = sqrt((dx * dx) + (dy * dy) + (dz * dz));
- motors[X_AXIS] = geometry_factor * target[X_AXIS] + target[Y_AXIS];
- motors[Y_AXIS] = geometry_factor * target[X_AXIS] - target[Y_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);
+ float motors[n_axis];
+ transform_cartesian_to_motors(motors, target);
if (!pl_data->motion.rapidMotion) {
- pl_data->feed_rate *= (motor_distance / dist);
+ float last_motors[n_axis];
+ transform_cartesian_to_motors(last_motors, position);
+ pl_data->feed_rate *= (three_axis_dist(motors, last_motors) / dist);
}
- memcpy(last_motors, motors, sizeof(motors));
-
return mc_line(motors, pl_data);
}
// motors -> cartesian
-void forward_kinematics(float* position) {
- 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;
- }
- }
-
+void motors_to_cartesian(float* cartesian, float* motors, int n_axis) {
// 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[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]);
+ cartesian[X_AXIS] = 0.5 * (motors[X_AXIS] + motors[Y_AXIS]) / geometry_factor;
+ cartesian[Y_AXIS] = 0.5 * (motors[X_AXIS] - motors[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
+ for (int axis = Z_AXIS; axis < n_axis; axis++) {
+ cartesian[axis] = motors[axis];
}
}
@@ -330,9 +309,7 @@ bool kinematics_pre_homing(uint8_t cycle_mask) {
void kinematics_post_homing() {
auto n_axis = number_axis->get();
- for (uint8_t axis = X_AXIS; axis <= n_axis; axis++) {
- gc_state.position[axis] = last_cartesian[axis];
- }
+ memcpy(gc_state.position, last_cartesian, n_axis * sizeof(last_cartesian[0]));
}
// this is used used by Limits.cpp to see if the range of the machine is exceeded.
diff --git a/Grbl_Esp32/Custom/atari_1020.cpp b/Grbl_Esp32/Custom/atari_1020.cpp
index 27a55021..8f27b4da 100644
--- a/Grbl_Esp32/Custom/atari_1020.cpp
+++ b/Grbl_Esp32/Custom/atari_1020.cpp
@@ -75,15 +75,12 @@ void machine_init() {
// this task tracks the Z position and sets the solenoid
void solenoidSyncTask(void* pvParameters) {
int32_t current_position[N_AXIS]; // copy of current location
- float m_pos[N_AXIS]; // machine position in mm
TickType_t xLastWakeTime;
const TickType_t xSolenoidFrequency = SOLENOID_TASK_FREQ; // in ticks (typically ms)
xLastWakeTime = xTaskGetTickCount(); // Initialise the xLastWakeTime variable with the current time.
while (true) {
// don't ever return from this or the task dies
- memcpy(current_position, sys_position, sizeof(sys_position)); // get current position in step
- system_convert_array_steps_to_mpos(m_pos, current_position); // convert to millimeters
- calc_solenoid(m_pos[Z_AXIS]); // calculate kinematics and move the servos
+ calc_solenoid(system_get_mpos()[Z_AXIS]); // calculate kinematics and move the servos
vTaskDelayUntil(&xLastWakeTime, xSolenoidFrequency);
}
}
diff --git a/Grbl_Esp32/Custom/custom_code_template.cpp b/Grbl_Esp32/Custom/custom_code_template.cpp
index 6725e59d..54d5f6ec 100644
--- a/Grbl_Esp32/Custom/custom_code_template.cpp
+++ b/Grbl_Esp32/Custom/custom_code_template.cpp
@@ -24,35 +24,38 @@
=======================================================================
This is a template for user-defined C++ code functions. Grbl can be
-configured to call some optional functions, enabled by #define statements
-in the machine definition .h file. Implement the functions thus enabled
-herein. The possible functions are listed and described below.
+configured to call some optional functions. These functions have weak definitions
+in the main code. If you create your own version they will be used instead
-To use this file, copy it to a name of your own choosing, and also copy
-Machines/template.h to a similar name.
+Put all of your functions in a .cpp file in the "Custom" folder.
+Add this to your machine definition file
+#define CUSTOM_CODE_FILENAME "../Custom/YourFile.cpp"
-Example:
-Machines/my_machine.h
-Custom/my_machine.cpp
-
-Edit machine.h to include your Machines/my_machine.h file
-
-Edit Machines/my_machine.h according to the instructions therein.
-
-Fill in the function definitions below for the functions that you have
-enabled with USE_ defines in Machines/my_machine.h
+Be careful to return the correct values
===============================================================================
+Below are all the current weak function
+
*/
-#ifdef USE_MACHINE_INIT
/*
-machine_init() is called when Grbl_ESP32 first starts. You can use it to do any
-special things your machine needs at startup.
+This function is used as a one time setup for your machine.
*/
void machine_init() {}
-#endif
+
+/*
+This is used to initialize a display.
+*/
+void display_init() {}
+
+/*
+ limitsCheckTravel() is called to check soft limits
+ It returns true if the motion is outside the limit values
+*/
+bool limitsCheckTravel() {
+ return false;
+}
/*
user_defined_homing(uint8_t cycle_mask) is called at the begining of the normal Grbl_ESP32 homing
@@ -72,8 +75,7 @@ bool user_defined_homing(uint8_t cycle_mask) {
*/
/*
- inverse_kinematics() looks at incoming move commands and modifies
- them before Grbl_ESP32 puts them in the motion planner.
+ cartesian_to_motors() converts from cartesian coordinates to motor space.
Grbl_ESP32 processes arcs by converting them into tiny little line segments.
Kinematics in Grbl_ESP32 works the same way. Search for this function across
@@ -83,7 +85,7 @@ bool user_defined_homing(uint8_t cycle_mask) {
pl_data = planner data (see the definition of this type to see what it is)
position = an N_AXIS array of where the machine is starting from for this move
*/
-bool inverse_kinematics(float* target, plan_line_data_t* pl_data, float* position) {
+bool cartesian_to_motors(float* target, plan_line_data_t* pl_data, float* position) {
// this simply moves to the target. Replace with your kinematics.
return mc_line(target, pl_data);
}
@@ -104,54 +106,32 @@ bool kinematics_pre_homing(uint8_t cycle_mask) {
void kinematics_post_homing() {}
/*
- limitsCheckTravel() is called to check soft limits
- It returns true if the motion is outside the limit values
-*/
-bool limitsCheckTravel() {
- return false;
-}
-
-/*
- The status command uses forward_kinematics() to convert
+ The status command uses motors_to_cartesian() to convert
your motor positions to cartesian X,Y,Z... coordinates.
Convert the N_AXIS array of motor positions to cartesian in your code.
*/
-void forward_kinematics(float* position) {
+void motors_to_cartesian(float* cartesian, float* motors, int n_axis) {
// position[X_AXIS] =
// position[Y_AXIS] =
}
-#ifdef USE_TOOL_CHANGE
/*
user_tool_change() is called when tool change gcode is received,
to perform appropriate actions for your machine.
*/
void user_tool_change(uint8_t new_tool) {}
-#endif
-#if defined(MACRO_BUTTON_0_PIN) || defined(MACRO_BUTTON_1_PIN) || defined(MACRO_BUTTON_2_PIN)
/*
options. user_defined_macro() is called with the button number to
perform whatever actions you choose.
*/
void user_defined_macro(uint8_t index) {}
-#endif
-#ifdef USE_M30
/*
user_m30() is called when an M30 gcode signals the end of a gcode file.
*/
void user_m30() {}
-#endif
-
-#ifdef USE_MACHINE_TRINAMIC_INIT
-/*
- machine_triaminic_setup() replaces the normal setup of trinamic
- drivers with your own code. For example, you could setup StallGuard
-*/
-void machine_trinamic_setup() {}
-#endif
// If you add any additional functions specific to your machine that
// require calls from common code, guard their calls in the common code with
diff --git a/Grbl_Esp32/Custom/mpcnc_laser_module.cpp b/Grbl_Esp32/Custom/mpcnc_laser_module.cpp
index e65ce629..52b816c4 100644
--- a/Grbl_Esp32/Custom/mpcnc_laser_module.cpp
+++ b/Grbl_Esp32/Custom/mpcnc_laser_module.cpp
@@ -21,7 +21,6 @@
along with Grbl. If not, see .
*/
-#ifdef USE_MACHINE_INIT
/*
machine_init() is called when Grbl_ESP32 first starts. You can use it to do any
special things your machine needs at startup.
@@ -32,4 +31,3 @@ void machine_init() {
pinMode(LVL_SHIFT_ENABLE, OUTPUT);
digitalWrite(LVL_SHIFT_ENABLE, HIGH);
}
-#endif
diff --git a/Grbl_Esp32/Custom/oled_basic.cpp b/Grbl_Esp32/Custom/oled_basic.cpp
index fedc543e..b1e2862b 100644
--- a/Grbl_Esp32/Custom/oled_basic.cpp
+++ b/Grbl_Esp32/Custom/oled_basic.cpp
@@ -120,7 +120,6 @@ void draw_checkbox(int16_t x, int16_t y, int16_t width, int16_t height, bool che
void displayDRO() {
uint8_t oled_y_pos;
- float print_position[MAX_N_AXIS];
//float wco[MAX_N_AXIS];
display.setTextAlignment(TEXT_ALIGN_LEFT);
@@ -135,10 +134,14 @@ void displayDRO() {
ControlPins ctrl_pin_state = system_control_get_state();
bool prb_pin_state = probe_get_state();
+ display.setTextAlignment(TEXT_ALIGN_RIGHT);
+
+ float* print_position = system_get_mpos();
if (bit_istrue(status_mask->get(), RtStatus::Position)) {
- calc_mpos(print_position);
+ display.drawString(60, 14, "M Pos");
} else {
- calc_wpos(print_position);
+ display.drawString(60, 14, "W Pos");
+ mpos_to_wpos(print_position);
}
for (uint8_t axis = X_AXIS; axis < n_axis; axis++) {
diff --git a/Grbl_Esp32/Custom/parallel_delta.cpp b/Grbl_Esp32/Custom/parallel_delta.cpp
index ea3f578d..23b28a81 100644
--- a/Grbl_Esp32/Custom/parallel_delta.cpp
+++ b/Grbl_Esp32/Custom/parallel_delta.cpp
@@ -91,7 +91,6 @@ static float last_cartesian[N_AXIS] = {
}; // A place to save the previous motor angles for distance/feed rate calcs // Z offset of the effector from the arm centers
// prototypes for helper functions
-int calc_forward_kinematics(float* angles, float* cartesian);
KinematicError delta_calcInverse(float* cartesian, float* angles);
KinematicError delta_calcAngleYZ(float x0, float y0, float z0, float& theta);
float three_axis_dist(float* point1, float* point2);
@@ -108,11 +107,9 @@ void machine_init() {
delta_crank_side_len = new FloatSetting(EXTENDED, WG, NULL, "Delta/CrankSideLength", LENGTH_FIXED_SIDE, 20.0, 500.0);
delta_effector_side_len = new FloatSetting(EXTENDED, WG, NULL, "Delta/EffectorSideLength", LENGTH_EFF_SIDE, 20.0, 500.0);
- read_settings();
-
// Calculate the Z offset at the arm zero angles ...
// Z offset is the z distance from the motor axes to the end effector axes at zero angle
- calc_forward_kinematics(angles, cartesian); // Sets the cartesian values
+ motors_to_cartesian(cartesian, angles, 3); // Sets the cartesian values
// print a startup message to show the kinematics are enabled. Print the offset for reference
grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Delta Kinematics Init: %s Z Offset:%4.3f", MACHINE_NAME, cartesian[Z_AXIS]);
@@ -126,17 +123,16 @@ void machine_init() {
// DXL_COUNT_MAX,
// DXL_COUNT_PER_RADIAN);
}
-bool user_defined_homing(uint8_t cycle_mask) { // true = do not continue with normal Grbl homing
-#ifdef USE_CUSTOM_HOMING
- return true;
-#else
- return false;
-#endif
-}
-// This function is used by Grbl
-bool inverse_kinematics(float* target, plan_line_data_t* pl_data, float* position) //The target and position are provided in MPos
-{
+// bool user_defined_homing(uint8_t cycle_mask) { // true = do not continue with normal Grbl homing
+// #ifdef USE_CUSTOM_HOMING
+// return true;
+// #else
+// return false;
+// #endif
+// }
+
+bool cartesian_to_motors(float* target, plan_line_data_t* pl_data, float* position) {
float dx, dy, dz; // distances in each cartesian axis
float motor_angles[3];
@@ -151,16 +147,17 @@ bool inverse_kinematics(float* target, plan_line_data_t* pl_data, float* positio
// grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Start %3.3f %3.3f %3.3f", position[0], position[1], position[2]);
// grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Target %3.3f %3.3f %3.3f", target[0], target[1], target[2]);
- status = delta_calcInverse(position, motor_angles);
+ status = delta_calcInverse(position, last_angle);
if (status == KinematicError::OUT_OF_RANGE) {
//grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Start position error %3.3f %3.3f %3.3f", position[0], position[1], position[2]);
- //start_position_error = true;
+ return false;
}
// Check the destination to see if it is in work area
status = delta_calcInverse(target, motor_angles);
if (status == KinematicError::OUT_OF_RANGE) {
grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Target unreachable error %3.3f %3.3f %3.3f", target[0], target[1], target[2]);
+ return false;
}
position[X_AXIS] += gc_state.coord_offset[X_AXIS];
@@ -187,21 +184,7 @@ bool inverse_kinematics(float* target, plan_line_data_t* pl_data, float* positio
// calculate the delta motor angles
KinematicError status = delta_calcInverse(seg_target, motor_angles);
- if (status == KinematicError ::NONE) {
- float delta_distance = three_axis_dist(motor_angles, last_angle);
-
- // save angles for next distance calc
- memcpy(last_angle, motor_angles, sizeof(motor_angles));
-
- if (pl_data->motion.rapidMotion) {
- pl_data->feed_rate = feed_rate;
- } else {
- pl_data->feed_rate = (feed_rate * delta_distance / segment_dist);
- }
-
- return mc_line(motor_angles, pl_data);
-
- } else {
+ if (status != KinematicError ::NONE) {
if (show_error) {
// grbl_msg_sendf(CLIENT_SERIAL,
// MsgLevel::Info,
@@ -211,10 +194,28 @@ bool inverse_kinematics(float* target, plan_line_data_t* pl_data, float* positio
// motor_angles[1],
// motor_angles[2]);
show_error = false;
- return false;
}
+ return false;
}
+ if (pl_data->motion.rapidMotion) {
+ pl_data->feed_rate = feed_rate;
+ } else {
+ float delta_distance = three_axis_dist(motor_angles, last_angle);
+ pl_data->feed_rate = (feed_rate * delta_distance / segment_dist);
+ }
+
+ // mc_line() returns false if a jog is cancelled.
+ // In that case we stop sending segments to the planner.
+ if (!mc_line(motor_angles, pl_data)) {
+ return false;
+ }
+
+ // save angles for next distance calc
+ // This is after mc_line() so that we do not update
+ // last_angle if the segment was discarded.
+ memcpy(last_angle, motor_angles, sizeof(motor_angles));
}
+ return true;
}
// this is used used by Grbl soft limits to see if the range of the machine is exceeded.
@@ -274,19 +275,21 @@ KinematicError delta_calcInverse(float* cartesian, float* angles) {
}
// inverse kinematics: angles -> cartesian
-int calc_forward_kinematics(float* angles, float* catesian) {
+void motors_to_cartesian(float* cartesian, float* motors, int n_axis) {
+ read_settings();
+
float t = (f - e) * tan30 / 2;
- float y1 = -(t + rf * cos(angles[0]));
- float z1 = -rf * sin(angles[0]);
+ float y1 = -(t + rf * cos(motors[0]));
+ float z1 = -rf * sin(motors[0]);
- float y2 = (t + rf * cos(angles[1])) * sin30;
+ float y2 = (t + rf * cos(motors[1])) * sin30;
float x2 = y2 * tan60;
- float z2 = -rf * sin(angles[1]);
+ float z2 = -rf * sin(motors[1]);
- float y3 = (t + rf * cos(angles[2])) * sin30;
+ float y3 = (t + rf * cos(motors[2])) * sin30;
float x3 = -y3 * tan60;
- float z3 = -rf * sin(angles[2]);
+ float z3 = -rf * sin(motors[2]);
float dnm = (y2 - y1) * x3 - (y3 - y1) * x2;
@@ -309,14 +312,16 @@ int calc_forward_kinematics(float* angles, float* catesian) {
// discriminant
float d = b * b - (float)4.0 * a * c;
- if (d < 0)
- return -1; // non-existing point
+ if (d < 0) {
+ grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "MSG:Fwd Kin Error");
+ return;
+ }
- catesian[Z_AXIS] = -(float)0.5 * (b + sqrt(d)) / a;
- catesian[X_AXIS] = (a1 * catesian[Z_AXIS] + b1) / dnm;
- catesian[Y_AXIS] = (a2 * catesian[Z_AXIS] + b2) / dnm;
- return 0;
+ cartesian[Z_AXIS] = -(float)0.5 * (b + sqrt(d)) / a;
+ cartesian[X_AXIS] = (a1 * cartesian[Z_AXIS] + b1) / dnm;
+ cartesian[Y_AXIS] = (a2 * cartesian[Z_AXIS] + b2) / dnm;
}
+
// helper functions, calculates angle theta1 (for YZ-pane)
KinematicError delta_calcAngleYZ(float x0, float y0, float z0, float& theta) {
float y1 = -0.5 * 0.57735 * f; // f/2 * tg 30
@@ -349,43 +354,15 @@ float three_axis_dist(float* point1, float* point2) {
return sqrt(((point1[0] - point2[0]) * (point1[0] - point2[0])) + ((point1[1] - point2[1]) * (point1[1] - point2[1])) +
((point1[2] - point2[2]) * (point1[2] - point2[2])));
}
-// called by reporting for WPos status
-void forward_kinematics(float* position) {
- float calc_fwd[N_AXIS];
- int status;
- read_settings();
-
- // convert the system position in steps to radians
- float position_radians[N_AXIS];
- int32_t position_steps[N_AXIS]; // Copy current state of the system position variable
- memcpy(position_steps, sys_position, sizeof(sys_position));
- system_convert_array_steps_to_mpos(position_radians, position_steps);
-
- // grbl_msg_sendf(
- // CLIENT_SERIAL, MsgLevel::Info, "Fwd Kin Angs %1.3f, %1.3f, %1.3f ", position_radians[0], position_radians[1], position_radians[2]);
-
- // detmine the position of the end effector joint center.
- status = calc_forward_kinematics(position_radians, calc_fwd);
-
- if (status == 0) {
- // apply offsets and set the returned values
- position[X_AXIS] = calc_fwd[X_AXIS] - gc_state.coord_system[X_AXIS] + gc_state.coord_offset[X_AXIS];
- position[Y_AXIS] = calc_fwd[Y_AXIS] - gc_state.coord_system[Y_AXIS] + gc_state.coord_offset[Y_AXIS];
- position[Z_AXIS] = calc_fwd[Z_AXIS] - gc_state.coord_system[Z_AXIS] + gc_state.coord_offset[Z_AXIS];
- } else {
- grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "MSG:Fwd Kin Error");
- }
-}
-
-bool kinematics_pre_homing(uint8_t cycle_mask) { // true = do not continue with normal Grbl homing
-#ifdef USE_CUSTOM_HOMING
- return true;
-#else
- //grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "kinematics_pre_homing");
- return false;
-#endif
-}
+// bool kinematics_pre_homing(uint8_t cycle_mask) { // true = do not continue with normal Grbl homing
+// #ifdef USE_CUSTOM_HOMING
+// return true;
+// #else
+// //grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "kinematics_pre_homing");
+// return false;
+// #endif
+// }
void kinematics_post_homing() {
#ifdef USE_CUSTOM_HOMING
@@ -395,9 +372,7 @@ void kinematics_post_homing() {
last_angle[Y_AXIS] = sys_position[Y_AXIS] / axis_settings[Y_AXIS]->steps_per_mm->get();
last_angle[Z_AXIS] = sys_position[Z_AXIS] / axis_settings[Z_AXIS]->steps_per_mm->get();
- read_settings();
-
- calc_forward_kinematics(last_angle, last_cartesian);
+ motors_to_cartesian(last_cartesian, last_angle, 3);
// grbl_msg_sendf(CLIENT_SERIAL,
// MsgLevel::Info,
diff --git a/Grbl_Esp32/Custom/polar_coaster.cpp b/Grbl_Esp32/Custom/polar_coaster.cpp
index 966f100e..7041fbb0 100644
--- a/Grbl_Esp32/Custom/polar_coaster.cpp
+++ b/Grbl_Esp32/Custom/polar_coaster.cpp
@@ -85,9 +85,8 @@ void kinematics_post_homing() {
*/
-bool inverse_kinematics(float* target, plan_line_data_t* pl_data, float* position) {
- //static float last_angle = 0;
- //static float last_radius = 0;
+
+bool cartesian_to_motors(float* target, plan_line_data_t* pl_data, float* position) {
float dx, dy, dz; // distances in each cartesian axis
float p_dx, p_dy, p_dz; // distances in each polar axis
float dist, polar_dist; // the distances in both systems...used to determine feed rate
@@ -111,7 +110,6 @@ bool inverse_kinematics(float* target, plan_line_data_t* pl_data, float* positio
segment_count = ceil(dist / SEGMENT_LENGTH); // determine the number of segments we need ... round up so there is at least 1
}
dist /= segment_count; // segment distance
- bool added = false;
for (uint32_t segment = 1; segment <= segment_count; segment++) {
// determine this segment's target
seg_target[X_AXIS] = position[X_AXIS] + (dx / float(segment_count) * segment) - x_offset;
@@ -140,12 +138,19 @@ bool inverse_kinematics(float* target, plan_line_data_t* pl_data, float* positio
// end determining new feed rate
polar[RADIUS_AXIS] += x_offset;
polar[Z_AXIS] += z_offset;
+
+ // mc_line() returns false if a jog is cancelled.
+ // In that case we stop sending segments to the planner.
+ if (!mc_line(polar, pl_data)) {
+ return false;
+ }
+
+ //
last_radius = polar[RADIUS_AXIS];
last_angle = polar[POLAR_AXIS];
- added = mc_line(polar, pl_data);
}
// TO DO don't need a feedrate for rapids
- return added;
+ return true;
}
/*
@@ -161,18 +166,10 @@ position = the current machine position
converted = position with forward kinematics applied.
*/
-void forward_kinematics(float* position) {
- float original_position[N_AXIS]; // temporary storage of original
- 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);
- original_position[X_AXIS] = print_position[X_AXIS] - gc_state.coord_system[X_AXIS] + gc_state.coord_offset[X_AXIS];
- original_position[Y_AXIS] = print_position[Y_AXIS] - gc_state.coord_system[Y_AXIS] + gc_state.coord_offset[Y_AXIS];
- original_position[Z_AXIS] = print_position[Z_AXIS] - gc_state.coord_system[Z_AXIS] + gc_state.coord_offset[Z_AXIS];
- position[X_AXIS] = cos(radians(original_position[Y_AXIS])) * original_position[X_AXIS] * -1;
- position[Y_AXIS] = sin(radians(original_position[Y_AXIS])) * original_position[X_AXIS];
- position[Z_AXIS] = original_position[Z_AXIS]; // unchanged
+void motors_to_cartesian(float* cartesian, float* motors, int n_axis) {
+ cartesian[X_AXIS] = cos(radians(motors[Y_AXIS])) * motors[X_AXIS] * -1;
+ cartesian[Y_AXIS] = sin(radians(motors[Y_AXIS])) * motors[X_AXIS];
+ cartesian[Z_AXIS] = motors[Z_AXIS]; // unchanged
}
// helper functions
diff --git a/Grbl_Esp32/src/Error.cpp b/Grbl_Esp32/src/Error.cpp
index c18ef200..70bb88d2 100644
--- a/Grbl_Esp32/src/Error.cpp
+++ b/Grbl_Esp32/src/Error.cpp
@@ -80,4 +80,5 @@ std::map ErrorNames = {
{ Error::NvsGetStatsFailed, "Failed to get setting status" },
{ Error::AuthenticationFailed, "Authentication failed!" },
{ Error::AnotherInterfaceBusy, "Another interface is busy" },
+ { Error::JogCancelled, "Jog Cancelled" },
};
diff --git a/Grbl_Esp32/src/Error.h b/Grbl_Esp32/src/Error.h
index 14b443eb..6b8fe3e0 100644
--- a/Grbl_Esp32/src/Error.h
+++ b/Grbl_Esp32/src/Error.h
@@ -84,6 +84,7 @@ enum class Error : uint8_t {
AuthenticationFailed = 110,
Eol = 111,
AnotherInterfaceBusy = 120,
+ JogCancelled = 130,
};
extern std::map ErrorNames;
diff --git a/Grbl_Esp32/src/GCode.cpp b/Grbl_Esp32/src/GCode.cpp
index 8254d80a..d55eb1ed 100644
--- a/Grbl_Esp32/src/GCode.cpp
+++ b/Grbl_Esp32/src/GCode.cpp
@@ -494,9 +494,7 @@ Error gc_execute_line(char* line, uint8_t client) {
break;
case 6: // tool change
gc_block.modal.tool_change = ToolChange::Enable;
-#ifdef USE_TOOL_CHANGE
//user_tool_change(gc_state.tool);
-#endif
mg_word_bit = ModalGroup::MM6;
break;
case 7:
@@ -1295,7 +1293,8 @@ Error gc_execute_line(char* line, uint8_t client) {
if (status == Error::Ok && !cancelledInflight) {
memcpy(gc_state.position, gc_block.values.xyz, sizeof(gc_block.values.xyz));
}
- return status;
+ // JogCancelled is not reported as a GCode error
+ return status == Error::JogCancelled ? Error::Ok : status;
}
// If in laser mode, setup laser power based on current and past parser conditions.
if (spindle->inLaserMode()) {
@@ -1362,9 +1361,7 @@ Error gc_execute_line(char* line, uint8_t client) {
// gc_state.tool = gc_block.values.t;
// [6. Change tool ]: NOT SUPPORTED
if (gc_block.modal.tool_change == ToolChange::Enable) {
-#ifdef USE_TOOL_CHANGE
user_tool_change(gc_state.tool);
-#endif
}
// [7. Spindle control ]:
if (gc_state.modal.spindle != gc_block.modal.spindle) {
@@ -1486,9 +1483,9 @@ Error gc_execute_line(char* line, uint8_t client) {
// and absolute and incremental modes.
pl_data->motion.rapidMotion = 1; // Set rapid motion flag.
if (axis_command != AxisCommand::None) {
- inverse_kinematics(gc_block.values.xyz, pl_data, gc_state.position);
+ cartesian_to_motors(gc_block.values.xyz, pl_data, gc_state.position);
}
- inverse_kinematics(coord_data, pl_data, gc_state.position);
+ cartesian_to_motors(coord_data, pl_data, gc_state.position);
memcpy(gc_state.position, coord_data, sizeof(gc_state.position));
break;
case NonModal::SetHome0:
@@ -1516,10 +1513,10 @@ Error gc_execute_line(char* line, uint8_t client) {
if (axis_command == AxisCommand::MotionMode) {
GCUpdatePos gc_update_pos = GCUpdatePos::Target;
if (gc_state.modal.motion == Motion::Linear) {
- inverse_kinematics(gc_block.values.xyz, pl_data, gc_state.position);
+ cartesian_to_motors(gc_block.values.xyz, pl_data, gc_state.position);
} else if (gc_state.modal.motion == Motion::Seek) {
pl_data->motion.rapidMotion = 1; // Set rapid motion flag.
- inverse_kinematics(gc_block.values.xyz, pl_data, gc_state.position);
+ cartesian_to_motors(gc_block.values.xyz, pl_data, gc_state.position);
} else if ((gc_state.modal.motion == Motion::CwArc) || (gc_state.modal.motion == Motion::CcwArc)) {
mc_arc(gc_block.values.xyz,
pl_data,
@@ -1603,9 +1600,7 @@ Error gc_execute_line(char* line, uint8_t client) {
coolant_off();
}
report_feedback_message(Message::ProgramEnd);
-#ifdef USE_M30
user_m30();
-#endif
break;
}
gc_state.modal.program_flow = ProgramFlow::Running; // Reset program flow.
diff --git a/Grbl_Esp32/src/Grbl.cpp b/Grbl_Esp32/src/Grbl.cpp
index c4a8926a..ea0bc2a3 100644
--- a/Grbl_Esp32/src/Grbl.cpp
+++ b/Grbl_Esp32/src/Grbl.cpp
@@ -122,6 +122,10 @@ void run_once() {
void __attribute__((weak)) machine_init() {}
void __attribute__((weak)) display_init() {}
+
+void __attribute__((weak)) user_m30() {}
+
+void __attribute__((weak)) user_tool_change(uint8_t new_tool) {}
/*
setup() and loop() in the Arduino .ino implements this control flow:
diff --git a/Grbl_Esp32/src/Grbl.h b/Grbl_Esp32/src/Grbl.h
index f23f1f0a..f0dbd60d 100644
--- a/Grbl_Esp32/src/Grbl.h
+++ b/Grbl_Esp32/src/Grbl.h
@@ -22,7 +22,7 @@
// Grbl versioning system
const char* const GRBL_VERSION = "1.3a";
-const char* const GRBL_VERSION_BUILD = "20210420";
+const char* const GRBL_VERSION_BUILD = "20210424";
//#include
#include
@@ -93,25 +93,21 @@ const char* const GRBL_VERSION_BUILD = "20210420";
void grbl_init();
void run_once();
-void machine_init(); // weak definition in Grbl.cpp
-void display_init(); // weak definition in Grbl.cpp
+void machine_init(); // weak definition in Grbl.cpp
+void display_init(); // weak definition in Grbl.cpp
+void user_m30(); // weak definition in Grbl.cpp/
+void user_tool_change(uint8_t new_tool); // weak definition in Grbl.cpp
bool user_defined_homing(uint8_t cycle_mask); // weak definition in Limits.cpp
// weak definitions in MotionControl.cpp
-bool inverse_kinematics(float* target, plan_line_data_t* pl_data, float* position);
+bool cartesian_to_motors(float* target, plan_line_data_t* pl_data, float* position);
bool kinematics_pre_homing(uint8_t cycle_mask);
void kinematics_post_homing();
bool limitsCheckTravel(float* target); // weak in Limits.cpp; true if out of range
-void forward_kinematics(float* position); // weak definition in Report.cpp
+void motors_to_cartesian(float* cartestian, float* motors, int n_axis); // weak definition
// 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);
-
-// Called if USE_M30 is defined
-void user_m30();
-
-// Called if USE_TOOL_CHANGE is defined
-void user_tool_change(uint8_t new_tool);
diff --git a/Grbl_Esp32/src/Jog.cpp b/Grbl_Esp32/src/Jog.cpp
index 568e9842..03474c7a 100644
--- a/Grbl_Esp32/src/Jog.cpp
+++ b/Grbl_Esp32/src/Jog.cpp
@@ -40,10 +40,8 @@ Error jog_execute(plan_line_data_t* pl_data, parser_block_t* gc_block, bool* can
}
}
// Valid jog command. Plan, set state, and execute.
- if (!inverse_kinematics(gc_block->values.xyz, pl_data, gc_state.position)) {
- if (cancelledInflight)
- *cancelledInflight = true;
- return Error::Ok;
+ if (!cartesian_to_motors(gc_block->values.xyz, pl_data, gc_state.position)) {
+ return Error::JogCancelled;
}
if (sys.state == State::Idle) {
diff --git a/Grbl_Esp32/src/Limits.cpp b/Grbl_Esp32/src/Limits.cpp
index ab167daa..fec18cfd 100644
--- a/Grbl_Esp32/src/Limits.cpp
+++ b/Grbl_Esp32/src/Limits.cpp
@@ -104,7 +104,6 @@ void limits_go_home(uint8_t cycle_mask) {
// Initialize variables used for homing computations.
uint8_t n_cycle = (2 * n_homing_locate_cycle + 1);
uint8_t step_pin[MAX_N_AXIS];
- float target[MAX_N_AXIS];
float max_travel = 0.0;
auto n_axis = number_axis->get();
@@ -122,7 +121,7 @@ void limits_go_home(uint8_t cycle_mask) {
uint8_t n_active_axis;
AxisMask limit_state, axislock;
do {
- system_convert_array_steps_to_mpos(target, sys_position);
+ float* target = system_get_mpos();
// Initialize and declare variables needed for homing routine.
axislock = 0;
n_active_axis = 0;
diff --git a/Grbl_Esp32/src/Machines/6_pack_TMC2130_XYZ_Test.h b/Grbl_Esp32/src/Machines/6_pack_TMC2130_XYZ_Test.h
new file mode 100644
index 00000000..fad84751
--- /dev/null
+++ b/Grbl_Esp32/src/Machines/6_pack_TMC2130_XYZ_Test.h
@@ -0,0 +1,110 @@
+#pragma once
+// clang-format off
+
+/*
+ 6_pack_TMC2130_XYZ_PWM.h
+
+ Part of Grbl_ESP32
+ Pin assignments for the ESP32 SPI 6-axis board
+
+ 2021-0302 B. Dring for Mike T.
+
+ Grbl_ESP32 is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+ Grbl is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+ You should have received a copy of the GNU General Public License
+ along with Grbl_ESP32. If not, see .
+*/
+#define MACHINE_NAME "6 Pack TMC2130 XYZ PWM"
+
+#define N_AXIS 3
+
+// I2S (steppers & other output-only pins)
+#define USE_I2S_OUT
+#define USE_I2S_STEPS
+//#define DEFAULT_STEPPER ST_I2S_STATIC
+
+#define I2S_OUT_BCK GPIO_NUM_22
+#define I2S_OUT_WS GPIO_NUM_17
+#define I2S_OUT_DATA GPIO_NUM_21
+
+#define TRINAMIC_RUN_MODE Motors::TrinamicMode::CoolStep
+#define TRINAMIC_HOMING_MODE Motors::TrinamicMode::CoolStep
+
+// Motor Socket #1
+#define X_TRINAMIC_DRIVER 2130
+#define X_DISABLE_PIN I2SO(0)
+#define X_DIRECTION_PIN I2SO(1)
+#define X_STEP_PIN I2SO(2)
+#define X_CS_PIN I2SO(3)
+#define X_RSENSE TMC2130_RSENSE_DEFAULT
+
+// Motor Socket #2
+#define Y_TRINAMIC_DRIVER X_TRINAMIC_DRIVER
+#define Y_DIRECTION_PIN I2SO(4)
+#define Y_STEP_PIN I2SO(5)
+#define Y_DISABLE_PIN I2SO(7)
+#define Y_CS_PIN I2SO(6)
+#define Y_RSENSE X_RSENSE
+
+// Motor Socket #3
+#define Z_TRINAMIC_DRIVER X_TRINAMIC_DRIVER
+#define Z_DISABLE_PIN I2SO(8)
+#define Z_DIRECTION_PIN I2SO(9)
+#define Z_STEP_PIN I2SO(10)
+#define Z_CS_PIN I2SO(11)
+#define Z_RSENSE X_RSENSE
+
+
+/*
+ Socket I/O reference
+ The list of modules is here...
+ https://github.com/bdring/6-Pack_CNC_Controller/wiki/CNC-I-O-Module-List
+ Click on each module to get example for using the modules in the sockets
+
+Socket #1
+#1 GPIO_NUM_33 (Sg1)
+#2 GPIO_NUM_32 (Sg2)
+#3 GPIO_NUM_35 (Sg3) (input only)
+#4 GPIO_NUM_34 (Sg4) (input only)
+
+Socket #2
+#1 GPIO_NUM_2
+#2 GPIO_NUM_25
+#3 GPIO_NUM_39 (Sg5) (input only)
+#4 GPIO_NUM_36 (Sg6) (input only)
+
+Socket #3
+#1 GPIO_NUM_26
+#2 GPIO_NUM_4
+#3 GPIO_NUM_16
+#4 GPIO_NUM_27
+
+Socket #4
+#1 GPIO_NUM_14
+#2 GPIO_NUM_13
+#3 GPIO_NUM_15
+#4 GPIO_NUM_12
+
+Socket #5
+#1 I2SO(24) (output only)
+#2 I2SO(25) (output only)
+#3 I2SO26) (output only)
+
+*/
+
+// Socket #1 (Empty)
+// Install StallGuard Jumpers
+#define X_LIMIT_PIN GPIO_NUM_33 // Sg1
+#define Y_LIMIT_PIN GPIO_NUM_32 // Sg2
+#define Z_LIMIT_PIN GPIO_NUM_35 // Sg3
+#define CONTROL_SAFETY_DOOR_PIN GPIO_NUM_34 // Sg4
+
+
+// === Default settings
+#define DEFAULT_STEP_PULSE_MICROSECONDS I2S_OUT_USEC_PER_PULSE
diff --git a/Grbl_Esp32/src/Machines/atari_1020.h b/Grbl_Esp32/src/Machines/atari_1020.h
index eecc2146..50186bb8 100644
--- a/Grbl_Esp32/src/Machines/atari_1020.h
+++ b/Grbl_Esp32/src/Machines/atari_1020.h
@@ -140,11 +140,7 @@
#define ATARI_HOMING_ATTEMPTS 13
// tells grbl we have some special functions to call
-#define USE_MACHINE_INIT
-#define USE_CUSTOM_HOMING
-#define USE_TOOL_CHANGE
#define ATARI_TOOL_CHANGE_Z 5.0
-#define USE_M30 // use the user defined end of program
#ifndef atari_h
#define atari_h
diff --git a/Grbl_Esp32/src/Machines/fysetc_ant.h b/Grbl_Esp32/src/Machines/fysetc_ant.h
new file mode 100644
index 00000000..79607088
--- /dev/null
+++ b/Grbl_Esp32/src/Machines/fysetc_ant.h
@@ -0,0 +1,85 @@
+#pragma once
+// clang-format off
+
+/*
+ fysetc_ant.h
+ https://github.com/FYSETC/FYSETC-E4
+
+ 2020-12-03 B. Dring
+
+ This is a machine definition file to use the FYSETC E4 3D Printer controller
+ This is a 4 motor controller. This is setup for XYZA, but XYYZ, could also be used.
+ There are 5 inputs
+ The controller has outputs for a Fan, Hotbed and Extruder. There are mapped to
+ spindle, mist and flood coolant to drive an external relay.
+
+ Grbl_ESP32 is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ Grbl is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with Grbl_ESP32. If not, see .
+*/
+
+#define MACHINE_NAME "FYSETC E4 3D Printer Controller"
+
+#define N_AXIS 4
+
+#define CUSTOM_CODE_FILENAME "../Custom/CoreXY.cpp"
+
+#define USE_KINEMATICS // there are kinematic equations for this machine
+
+#define TRINAMIC_RUN_MODE TrinamicMode :: StealthChop
+#define TRINAMIC_HOMING_MODE TrinamicMode :: StealthChop
+
+#define TMC_UART UART_NUM_1
+#define TMC_UART_RX GPIO_NUM_21
+#define TMC_UART_TX GPIO_NUM_22
+
+#define X_TRINAMIC_DRIVER 2209
+#define X_STEP_PIN GPIO_NUM_27
+#define X_DIRECTION_PIN GPIO_NUM_26
+#define X_RSENSE TMC2209_RSENSE_DEFAULT
+#define X_DRIVER_ADDRESS 1
+#define DEFAULT_X_MICROSTEPS 16
+
+#define Y_TRINAMIC_DRIVER 2209
+#define Y_STEP_PIN GPIO_NUM_33
+#define Y_DIRECTION_PIN GPIO_NUM_32
+#define Y_RSENSE TMC2209_RSENSE_DEFAULT
+#define Y_DRIVER_ADDRESS 3
+#define DEFAULT_Y_MICROSTEPS 16
+
+#define Z_TRINAMIC_DRIVER 2209
+#define Z_STEP_PIN GPIO_NUM_14
+#define Z_DIRECTION_PIN GPIO_NUM_12
+#define Z_RSENSE TMC2209_RSENSE_DEFAULT
+#define Z_DRIVER_ADDRESS 0
+#define DEFAULT_Z_MICROSTEPS 16
+
+#define A_TRINAMIC_DRIVER 2209
+#define A_STEP_PIN GPIO_NUM_16
+#define A_DIRECTION_PIN GPIO_NUM_17
+#define A_RSENSE TMC2209_RSENSE_DEFAULT
+#define A_DRIVER_ADDRESS 2
+#define DEFAULT_A_MICROSTEPS 16
+
+#define X_LIMIT_PIN GPIO_NUM_34
+#define Y_LIMIT_PIN GPIO_NUM_35
+#define Z_LIMIT_PIN GPIO_NUM_15
+#define A_LIMIT_PIN GPIO_NUM_36 // Labeled TB
+#define PROBE_PIN GPIO_NUM_39 // Labeled TE
+
+// OK to comment out to use pin for other features
+#define STEPPERS_DISABLE_PIN GPIO_NUM_25
+
+#define SPINDLE_TYPE SpindleType::RELAY
+#define SPINDLE_OUTPUT_PIN GPIO_NUM_13 // labeled Fan
+#define COOLANT_MIST_PIN GPIO_NUM_2 // Labeled Hotbed
+#define COOLANT_FLOOD_PIN GPIO_NUM_4 // Labeled Heater
diff --git a/Grbl_Esp32/src/Machines/fystec_e4.h b/Grbl_Esp32/src/Machines/fysetc_e4.h
similarity index 95%
rename from Grbl_Esp32/src/Machines/fystec_e4.h
rename to Grbl_Esp32/src/Machines/fysetc_e4.h
index 63b47bab..1bb8800d 100644
--- a/Grbl_Esp32/src/Machines/fystec_e4.h
+++ b/Grbl_Esp32/src/Machines/fysetc_e4.h
@@ -2,12 +2,12 @@
// clang-format off
/*
- fystec_e4.h
+ fysetc_e4.h
https://github.com/FYSETC/FYSETC-E4
2020-12-03 B. Dring
- This is a machine definition file to use the FYSTEC E4 3D Printer controller
+ This is a machine definition file to use the FYSETC E4 3D Printer controller
This is a 4 motor controller. This is setup for XYZA, but XYYZ, could also be used.
There are 5 inputs
The controller has outputs for a Fan, Hotbed and Extruder. There are mapped to
@@ -27,7 +27,7 @@
along with Grbl_ESP32. If not, see .
*/
-#define MACHINE_NAME "FYSTEC E4 3D Printer Controller"
+#define MACHINE_NAME "FYSETC E4 3D Printer Controller"
#define N_AXIS 4
diff --git a/Grbl_Esp32/src/Machines/midtbot.h b/Grbl_Esp32/src/Machines/midtbot.h
index 861295bb..a6bd8438 100644
--- a/Grbl_Esp32/src/Machines/midtbot.h
+++ b/Grbl_Esp32/src/Machines/midtbot.h
@@ -29,9 +29,7 @@
#define CUSTOM_CODE_FILENAME "../Custom/CoreXY.cpp"
-#define MIDTBOT // applies the geometry correction to the kinematics
-#define USE_MACHINE_INIT // There is some custom initialization for this machine
-#define USE_CUSTOM_HOMING
+#define MIDTBOT // applies the midTbot geometry correction to the CoreXY kinematics
#define SPINDLE_TYPE SpindleType::NONE
@@ -48,8 +46,6 @@
#define Z_SERVO_PIN GPIO_NUM_27
-// Set $Homing/Cycle0=Y and $Homing/Cycle1=X
-
#define SPINDLE_TYPE SpindleType::NONE
// defaults
diff --git a/Grbl_Esp32/src/Machines/midtbot_x2.h b/Grbl_Esp32/src/Machines/midtbot_x2.h
new file mode 100644
index 00000000..9e904133
--- /dev/null
+++ b/Grbl_Esp32/src/Machines/midtbot_x2.h
@@ -0,0 +1,119 @@
+#pragma once
+// clang-format off
+
+/*
+ midtbot.h
+ Part of Grbl_ESP32
+
+ Pin assignments for the Buildlog.net midtbot
+ https://github.com/bdring/midTbot_esp32
+
+ 2018 - Bart Dring
+ 2020 - Mitch Bradley
+
+ Grbl_ESP32 is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ Grbl is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with Grbl_ESP32. If not, see .
+*/
+
+#define MACHINE_NAME "midTbot"
+
+#define DISPLAY_CODE_FILENAME "Custom/oled_basic.cpp"
+
+#define CUSTOM_CODE_FILENAME "../Custom/CoreXY.cpp"
+#define MIDTBOT // applies the geometry correction to the kinematics
+#define USE_KINEMATICS // there are kinematic equations for this machine
+#define USE_FWD_KINEMATICS // report in cartesian
+#define SPINDLE_TYPE SpindleType::NONE
+
+#define TRINAMIC_UART_RUN_MODE TrinamicUartMode :: StealthChop
+#define TRINAMIC_UART_HOMING_MODE TrinamicUartMode :: StealthChop
+
+#define TMC_UART UART_NUM_1
+#define TMC_UART_RX GPIO_NUM_21
+#define TMC_UART_TX GPIO_NUM_22
+
+#define X_TRINAMIC_DRIVER 2209
+#define X_STEP_PIN GPIO_NUM_25 //GPIO_NUM_32
+#define X_DIRECTION_PIN GPIO_NUM_33 // GPIO_NUM_26
+#define X_RSENSE TMC2209_RSENSE_DEFAULT
+#define X_DRIVER_ADDRESS 1
+#define DEFAULT_X_MICROSTEPS 16
+
+#define Y_TRINAMIC_DRIVER 2209
+#define Y_STEP_PIN GPIO_NUM_32 //GPIO_NUM_25
+#define Y_DIRECTION_PIN GPIO_NUM_26 //GPIO_NUM_33
+#define Y_RSENSE TMC2209_RSENSE_DEFAULT
+#define Y_DRIVER_ADDRESS 0
+#define DEFAULT_Y_MICROSTEPS 16
+
+#define STEPPERS_DISABLE_PIN GPIO_NUM_27
+
+#define X_LIMIT_PIN GPIO_NUM_4
+#define Y_LIMIT_PIN GPIO_NUM_12
+
+#define Z_SERVO_PIN GPIO_NUM_15
+
+// Set $Homing/Cycle0=Y and $Homing/Cycle1=X
+
+#define SPINDLE_TYPE SpindleType::NONE
+
+// ================== defaults ================================
+#define DEFAULT_HOMING_CYCLE_0 bit(Z_AXIS)
+#define DEFAULT_HOMING_CYCLE_1 bit(Y_AXIS)
+#define DEFAULT_HOMING_CYCLE_2 bit(X_AXIS)
+
+#define DEFAULT_HOMING_DIR_MASK (bit(X_AXIS) | bit (Z_AXIS)) // these home negative
+
+#define DEFAULT_STEP_PULSE_MICROSECONDS 3
+#define DEFAULT_STEPPER_IDLE_LOCK_TIME 255 // stay on
+
+#define DEFAULT_STEPPING_INVERT_MASK 0 // uint8_t
+#define DEFAULT_DIRECTION_INVERT_MASK 2 // uint8_t
+#define DEFAULT_INVERT_ST_ENABLE 0 // boolean
+#define DEFAULT_INVERT_LIMIT_PINS 0 // boolean
+#define DEFAULT_INVERT_PROBE_PIN 0 // boolean
+
+#define DEFAULT_STATUS_REPORT_MASK 1
+
+#define DEFAULT_JUNCTION_DEVIATION 0.01 // mm
+#define DEFAULT_ARC_TOLERANCE 0.002 // mm
+#define DEFAULT_REPORT_INCHES 0 // false
+
+#define DEFAULT_SOFT_LIMIT_ENABLE 0 // false
+#define DEFAULT_HARD_LIMIT_ENABLE 0 // false
+
+#define DEFAULT_HOMING_ENABLE 1
+#define DEFAULT_HOMING_FEED_RATE 500.0 // mm/min
+#define DEFAULT_HOMING_SEEK_RATE 2000.0 // mm/min
+#define DEFAULT_HOMING_DEBOUNCE_DELAY 250 // msec (0-65k)
+#define DEFAULT_HOMING_PULLOFF 3.0 // mm
+
+#define DEFAULT_X_STEPS_PER_MM 100.0
+#define DEFAULT_Y_STEPS_PER_MM 100.0
+#define DEFAULT_Z_STEPS_PER_MM 100.0 // This is percent in servo mode
+
+#define DEFAULT_X_MAX_RATE 8000.0 // mm/min
+#define DEFAULT_Y_MAX_RATE 8000.0 // mm/min
+#define DEFAULT_Z_MAX_RATE 5000.0 // mm/min
+
+#define DEFAULT_X_ACCELERATION 200.0 // mm/sec^2. 200 mm/sec^2 = 720000 mm/min^2
+#define DEFAULT_Y_ACCELERATION 200.0 // mm/sec^2
+#define DEFAULT_Z_ACCELERATION 100.0 // mm/sec^2
+
+#define DEFAULT_X_MAX_TRAVEL 100.0 // mm NOTE: Must be a positive value.
+#define DEFAULT_Y_MAX_TRAVEL 100.0 // mm NOTE: Must be a positive value.
+#define DEFAULT_Z_MAX_TRAVEL 5.0 // This is percent in servo mode
+
+#define DEFAULT_X_HOMING_MPOS DEFAULT_Z_MAX_TRAVEL // stays up after homing
+
+
diff --git a/Grbl_Esp32/src/Machines/mpcnc_laser_module_v1p2.h b/Grbl_Esp32/src/Machines/mpcnc_laser_module_v1p2.h
index 43f2864c..a9fecf73 100644
--- a/Grbl_Esp32/src/Machines/mpcnc_laser_module_v1p2.h
+++ b/Grbl_Esp32/src/Machines/mpcnc_laser_module_v1p2.h
@@ -30,7 +30,6 @@
#define MACHINE_NAME "MPCNC_V1P2 with Laser Module"
// The laser module fires without a low signal. This keeps the enable on
-#define USE_MACHINE_INIT
#define LVL_SHIFT_ENABLE GPIO_NUM_32
#define CUSTOM_CODE_FILENAME "Custom/mpcnc_laser_module.cpp"
diff --git a/Grbl_Esp32/src/Machines/polar_coaster.h b/Grbl_Esp32/src/Machines/polar_coaster.h
index b0ff598b..03f9f1ea 100644
--- a/Grbl_Esp32/src/Machines/polar_coaster.h
+++ b/Grbl_Esp32/src/Machines/polar_coaster.h
@@ -37,7 +37,6 @@
#define POLAR_AXIS 1
#define SEGMENT_LENGTH 0.5 // segment length in mm
-#define USE_M30
#define X_STEP_PIN GPIO_NUM_15
#define Y_STEP_PIN GPIO_NUM_2
@@ -121,4 +120,6 @@
#define DEFAULT_X_MAX_TRAVEL 50.0 // mm NOTE: Must be a positive value.
#define DEFAULT_Y_MAX_TRAVEL 300.0 // mm NOTE: Must be a positive value.
-#define DEFAULT_Z_MAX_TRAVEL 100.0 // This is percent in servo mode
+#define DEFAULT_Z_MAX_TRAVEL 5.0 // This is percent in servo mode
+
+#define DEFAULT_Z_HOMING_MPOS DEFAULT_Z_MAX_TRAVEL // stays up after homing
diff --git a/Grbl_Esp32/src/Machines/tapster_3.h b/Grbl_Esp32/src/Machines/tapster_3.h
index e537ca06..d801aed5 100644
--- a/Grbl_Esp32/src/Machines/tapster_3.h
+++ b/Grbl_Esp32/src/Machines/tapster_3.h
@@ -23,10 +23,6 @@
#define N_AXIS 3
-// ================= Firmware Customization ===================
-
-#define USE_MACHINE_INIT // There is some custom initialization for this machine
-
// ================== Delta Geometry ===========================
#define RADIUS_FIXED 70.0 // radius of the fixed side (length of motor cranks)
diff --git a/Grbl_Esp32/src/Machines/tapster_pro_6P_trinamic.h b/Grbl_Esp32/src/Machines/tapster_pro_6P_trinamic.h
index 4f633511..6b4047ee 100644
--- a/Grbl_Esp32/src/Machines/tapster_pro_6P_trinamic.h
+++ b/Grbl_Esp32/src/Machines/tapster_pro_6P_trinamic.h
@@ -26,8 +26,6 @@
#define FWD_KINEMATICS_REPORTING // report in cartesian
#define USE_RMT_STEPS // Use the RMT periferal to generate step pulses
#define USE_TRINAMIC // some Trinamic motors are used on this machine
-#define USE_MACHINE_TRINAMIC_INIT // there is a machine specific setup for the drivers
-#define USE_MACHINE_INIT // There is some custom initialization for this machine
#define SEGMENT_LENGTH 0.5 // segment length in mm
#define KIN_ANGLE_CALC_OK 0
@@ -41,8 +39,6 @@
#define N_AXIS 3
-#define USE_MACHINE_INIT // There is some custom initialization for this machine
-
// ================== Delta Geometry ===========================
#define RADIUS_FIXED 100.0f // radius of the fixed side (length of motor cranks)
diff --git a/Grbl_Esp32/src/MotionControl.cpp b/Grbl_Esp32/src/MotionControl.cpp
index 3428206c..00762b97 100644
--- a/Grbl_Esp32/src/MotionControl.cpp
+++ b/Grbl_Esp32/src/MotionControl.cpp
@@ -95,7 +95,7 @@ bool mc_line(float* target, plan_line_data_t* pl_data) {
return submitted_result;
}
-bool __attribute__((weak)) inverse_kinematics(float* target, plan_line_data_t* pl_data, float* position) {
+bool __attribute__((weak)) cartesian_to_motors(float* target, plan_line_data_t* pl_data, float* position) {
return mc_line(target, pl_data);
}
@@ -105,6 +105,10 @@ bool __attribute__((weak)) kinematics_pre_homing(uint8_t cycle_mask) {
void __attribute__((weak)) kinematics_post_homing() {}
+void __attribute__((weak)) motors_to_cartesian(float* cartesian, float* motors, int n_axis) {
+ memcpy(cartesian, motors, n_axis * sizeof(motors[0]));
+}
+
void __attribute__((weak)) forward_kinematics(float* position) {}
// Execute an arc in offset mode format. position == current xyz, target == target xyz,
// offset == offset from current xyz, axis_X defines circle plane in tool space, axis_linear is
@@ -218,7 +222,7 @@ void mc_arc(float* target,
position[axis_1] = center_axis1 + r_axis1;
position[axis_linear] += linear_per_segment;
pl_data->feed_rate = original_feedrate; // This restores the feedrate kinematics may have altered
- inverse_kinematics(position, pl_data, previous_position);
+ cartesian_to_motors(position, pl_data, previous_position);
previous_position[axis_0] = position[axis_0];
previous_position[axis_1] = position[axis_1];
previous_position[axis_linear] = position[axis_linear];
@@ -229,7 +233,7 @@ void mc_arc(float* target,
}
}
// Ensure last segment arrives at target location.
- inverse_kinematics(target, pl_data, previous_position);
+ cartesian_to_motors(target, pl_data, previous_position);
}
// Execute dwell in seconds.
@@ -415,7 +419,7 @@ GCUpdatePos mc_probe_cycle(float* target, plan_line_data_t* pl_data, uint8_t par
}
// Setup and queue probing motion. Auto cycle-start should not start the cycle.
grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Found");
- inverse_kinematics(target, pl_data, gc_state.position);
+ cartesian_to_motors(target, pl_data, gc_state.position);
// Activate the probing state monitor in the stepper module.
sys_probe_state = Probe::Active;
// Perform probing cycle. Wait here until probe is triggered or motion completes.
diff --git a/Grbl_Esp32/src/MotionControl.h b/Grbl_Esp32/src/MotionControl.h
index 70d72cf0..c9f26c04 100644
--- a/Grbl_Esp32/src/MotionControl.h
+++ b/Grbl_Esp32/src/MotionControl.h
@@ -35,7 +35,7 @@ const int PARKING_MOTION_LINE_NUMBER = 0;
// Execute linear motion in absolute millimeter coordinates. Feed rate given in millimeters/second
// unless invert_feed_rate is true. Then the feed_rate means that the motion should be completed in
// (1 minute)/feed_rate time.
-bool inverse_kinematics(float* target, plan_line_data_t* pl_data, float* position);
+bool cartesian_to_motors(float* target, plan_line_data_t* pl_data, float* position);
bool mc_line(float* target, plan_line_data_t* pl_data); // returns true if line was submitted to planner
// Execute an arc in offset mode format. position == current xyz, target == target xyz,
diff --git a/Grbl_Esp32/src/Motors/Dynamixel2.cpp b/Grbl_Esp32/src/Motors/Dynamixel2.cpp
index 3c3faedc..8543dac2 100644
--- a/Grbl_Esp32/src/Motors/Dynamixel2.cpp
+++ b/Grbl_Esp32/src/Motors/Dynamixel2.cpp
@@ -36,7 +36,7 @@ namespace Motors {
grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Dynamixel Error. Missing pin definitions");
_has_errors = true;
} else {
- _has_errors = false; // The motor can be used
+ _has_errors = false; // The motor can be used
}
}
@@ -103,8 +103,8 @@ namespace Motors {
}
} else {
- grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "%s Dynamixel Servo ID %d Ping failed", reportAxisNameMsg(_axis_index, _dual_axis_index),
-_id);
+ grbl_msg_sendf(
+ CLIENT_SERIAL, MsgLevel::Info, "%s Dynamixel Servo ID %d Ping failed", reportAxisNameMsg(_axis_index, _dual_axis_index), _id);
return false;
}
@@ -197,7 +197,7 @@ _id);
set_disable(false);
set_location(); // force the PWM to update now
- return false; // Cannot do conventional homing
+ return false; // Cannot do conventional homing
}
void Dynamixel2::dxl_goal_position(int32_t position) {
@@ -345,16 +345,14 @@ _id);
tx_message[++msg_index] = 4; // low order data length
tx_message[++msg_index] = 0; // high order data length
- auto n_axis = number_axis->get();
+ auto n_axis = number_axis->get();
+ float* mpos = system_get_mpos();
for (uint8_t axis = X_AXIS; axis < n_axis; axis++) {
for (uint8_t gang_index = 0; gang_index < 2; gang_index++) {
current_id = ids[axis][gang_index];
if (current_id != 0) {
count++; // keep track of the count for the message length
- //determine the location of the axis
- float target = system_convert_axis_steps_to_mpos(sys_position, axis); // get the axis machine position in mm
-
dxl_count_min = DXL_COUNT_MIN;
dxl_count_max = DXL_COUNT_MAX;
@@ -362,7 +360,8 @@ _id);
swap(dxl_count_min, dxl_count_max);
// map the mm range to the servo range
- dxl_position = (uint32_t)mapConstrain(target, limitsMinPosition(axis), limitsMaxPosition(axis), dxl_count_min, dxl_count_max);
+ dxl_position =
+ (uint32_t)mapConstrain(mpos[axis], limitsMinPosition(axis), limitsMaxPosition(axis), dxl_count_min, dxl_count_max);
tx_message[++msg_index] = current_id; // ID of the servo
tx_message[++msg_index] = dxl_position & 0xFF; // data
diff --git a/Grbl_Esp32/src/Motors/Motor.cpp b/Grbl_Esp32/src/Motors/Motor.cpp
index 195601f8..6de1c869 100644
--- a/Grbl_Esp32/src/Motors/Motor.cpp
+++ b/Grbl_Esp32/src/Motors/Motor.cpp
@@ -16,7 +16,6 @@
Make sure public/private/protected is cleaned up.
Only a few Unipolar axes have been setup in init()
Get rid of Z_SERVO, just reply on Z_SERVO_PIN
- Deal with custom machine ... machine_trinamic_setup();
Class is ready to deal with non SPI pins, but they have not been needed yet.
It would be nice in the config message though
Testing
@@ -34,8 +33,7 @@
#include "Motor.h"
namespace Motors {
- Motor::Motor(uint8_t axis_index) :
- _axis_index(axis_index % MAX_AXES), _dual_axis_index(axis_index / MAX_AXES) {}
+ Motor::Motor(uint8_t axis_index) : _axis_index(axis_index % MAX_AXES), _dual_axis_index(axis_index / MAX_AXES) {}
void Motor::debug_message() {}
diff --git a/Grbl_Esp32/src/Motors/Motors.cpp b/Grbl_Esp32/src/Motors/Motors.cpp
index 4cb5289a..21f955bd 100644
--- a/Grbl_Esp32/src/Motors/Motors.cpp
+++ b/Grbl_Esp32/src/Motors/Motors.cpp
@@ -16,7 +16,6 @@
Make sure public/private/protected is cleaned up.
Only a few Unipolar axes have been setup in init()
Get rid of Z_SERVO, just reply on Z_SERVO_PIN
- Deal with custom machine ... machine_trinamic_setup();
Class is ready to deal with non SPI pins, but they have not been needed yet.
It would be nice in the config message though
Testing
diff --git a/Grbl_Esp32/src/Protocol.cpp b/Grbl_Esp32/src/Protocol.cpp
index 3c669128..0119a153 100644
--- a/Grbl_Esp32/src/Protocol.cpp
+++ b/Grbl_Esp32/src/Protocol.cpp
@@ -532,7 +532,6 @@ static void protocol_exec_rt_suspend() {
#ifdef PARKING_ENABLE
// Declare and initialize parking local variables
float restore_target[MAX_N_AXIS];
- float parking_target[MAX_N_AXIS];
float retract_waypoint = PARKING_PULLOUT_INCREMENT;
plan_line_data_t plan_data;
plan_line_data_t* pl_data = &plan_data;
@@ -579,6 +578,9 @@ static void protocol_exec_rt_suspend() {
// the safety door and sleep states.
if (sys.state == State::SafetyDoor || sys.state == State::Sleep) {
// Handles retraction motions and de-energizing.
+#ifdef PARKING_ENABLE
+ float* parking_target = system_get_mpos();
+#endif
if (!sys.suspend.bit.retractComplete) {
// Ensure any prior spindle stop override is disabled at start of safety door routine.
sys.spindle_stop_ovr.value = 0; // Disable override
@@ -587,9 +589,8 @@ static void protocol_exec_rt_suspend() {
coolant_off();
#else
// Get current position and store restore location and spindle retract waypoint.
- system_convert_array_steps_to_mpos(parking_target, sys_position);
if (!sys.suspend.bit.restartRetract) {
- memcpy(restore_target, parking_target, sizeof(parking_target));
+ memcpy(restore_target, parking_target, sizeof(restore_target[0]) * number_axis->get());
retract_waypoint += restore_target[PARKING_AXIS];
retract_waypoint = MIN(retract_waypoint, PARKING_TARGET);
}
diff --git a/Grbl_Esp32/src/Report.cpp b/Grbl_Esp32/src/Report.cpp
index 3339a23c..9afe1c45 100644
--- a/Grbl_Esp32/src/Report.cpp
+++ b/Grbl_Esp32/src/Report.cpp
@@ -299,11 +299,11 @@ void report_grbl_help(uint8_t client) {
// These values are retained until Grbl is power-cycled, whereby they will be re-zeroed.
void report_probe_parameters(uint8_t client) {
// Report in terms of machine position.
- float print_position[MAX_N_AXIS];
- char probe_rpt[(axesStringLen + 13 + 6 + 1)]; // the probe report we are building here
- char temp[axesStringLen];
+ char probe_rpt[(axesStringLen + 13 + 6 + 1)]; // the probe report we are building here
+ char temp[axesStringLen];
strcpy(probe_rpt, "[PRB:"); // initialize the string with the first characters
// get the machine position and put them into a string and append to the probe report
+ float print_position[MAX_N_AXIS];
system_convert_array_steps_to_mpos(print_position, sys_probe_position);
report_util_axis_values(print_position, temp);
strcat(probe_rpt, temp);
@@ -574,28 +574,6 @@ void report_echo_line_received(char* line, uint8_t client) {
// float print_position = returned position
// float wco = returns the work coordinate offset
// bool wpos = true for work position compensation
-void report_calc_status_position(float* print_position, float* wco, bool wpos) {
- int32_t current_position[MAX_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);
-
- //float wco[MAX_N_AXIS];
- if (wpos || (sys.report_wco_counter == 0)) {
- auto n_axis = number_axis->get();
- for (uint8_t idx = 0; idx < n_axis; idx++) {
- // Apply work coordinate offsets and tool length offset to current position.
- wco[idx] = gc_state.coord_system[idx] + gc_state.coord_offset[idx];
- if (idx == TOOL_LENGTH_OFFSET_AXIS) {
- wco[idx] += gc_state.tool_length_offset;
- }
- if (wpos) {
- print_position[idx] -= wco[idx];
- }
- }
- }
-
- forward_kinematics(print_position); // a weak definition does nothing. Users can provide strong version
-}
// Prints real-time data. This function grabs a real-time snapshot of the stepper subprogram
// and the actual location of the CNC machine. Users may change the following function to their
@@ -603,20 +581,19 @@ void report_calc_status_position(float* print_position, float* wco, bool wpos) {
// requires as it minimizes the computational overhead and allows grbl to keep running smoothly,
// especially during g-code programs with fast, short line segments and high frequency reports (5-20Hz).
void report_realtime_status(uint8_t client) {
- float print_position[MAX_N_AXIS];
- char status[200];
- char temp[MAX_N_AXIS * 20];
+ char status[200];
+ char temp[MAX_N_AXIS * 20];
strcpy(status, "<");
strcat(status, report_state_text());
// Report position
+ float* print_position = system_get_mpos();
if (bit_istrue(status_mask->get(), RtStatus::Position)) {
- calc_mpos(print_position);
strcat(status, "|MPos:");
} else {
- calc_wpos(print_position);
strcat(status, "|WPos:");
+ mpos_to_wpos(print_position);
}
report_util_axis_values(print_position, temp);
strcat(status, temp);
@@ -956,26 +933,14 @@ void reportTaskStackSize(UBaseType_t& saved) {
#endif
}
-void calc_mpos(float* print_position) {
- int32_t current_position[MAX_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);
- forward_kinematics(print_position); // a weak definition does nothing. Users can provide strong version
-}
-
-void calc_wpos(float* print_position) {
- int32_t current_position[MAX_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);
-
+void mpos_to_wpos(float* position) {
float* wco = get_wco();
auto n_axis = number_axis->get();
for (int idx = 0; idx < n_axis; idx++) {
- print_position[idx] -= wco[idx];
+ position[idx] -= wco[idx];
}
-
- forward_kinematics(print_position); // a weak definition does nothing. Users can provide strong version
}
+
float* get_wco() {
static float wco[MAX_N_AXIS];
auto n_axis = number_axis->get();
@@ -988,5 +953,3 @@ float* get_wco() {
}
return wco;
}
-
-void __attribute__((weak)) forward_kinematics(float* position) {} // This version does nothing. Make your own to do something with it
diff --git a/Grbl_Esp32/src/Report.h b/Grbl_Esp32/src/Report.h
index 366e8d9d..eecd905a 100644
--- a/Grbl_Esp32/src/Report.h
+++ b/Grbl_Esp32/src/Report.h
@@ -92,9 +92,6 @@ void report_grbl_settings(uint8_t client, uint8_t show_extended);
// Prints an echo of the pre-parsed line received right before execution.
void report_echo_line_received(char* line, uint8_t client);
-// calculate the postion for status reports
-void report_calc_status_position(float* print_position, float* wco, bool wpos);
-
// Prints realtime status report
void report_realtime_status(uint8_t client);
@@ -134,5 +131,4 @@ void reportTaskStackSize(UBaseType_t& saved);
char* report_state_text();
float* get_wco();
-void calc_mpos(float* print_position);
-void calc_wpos(float* print_position);
+void mpos_to_wpos(float* position);
diff --git a/Grbl_Esp32/src/System.cpp b/Grbl_Esp32/src/System.cpp
index 35e8837e..74b593fd 100644
--- a/Grbl_Esp32/src/System.cpp
+++ b/Grbl_Esp32/src/System.cpp
@@ -30,7 +30,7 @@ volatile ExecState sys_rt_exec_state; // Global realtime executor bitflag v
volatile ExecAlarm sys_rt_exec_alarm; // Global realtime executor bitflag variable for setting various alarms.
volatile ExecAccessory sys_rt_exec_accessory_override; // Global realtime executor bitflag variable for spindle/coolant overrides.
volatile bool cycle_stop; // For state transitions, instead of bitflag
-volatile void* sys_pl_data_inflight; // holds a plan_line_data_t while inverse_kinematics has taken ownership of a line motion
+volatile void* sys_pl_data_inflight; // holds a plan_line_data_t while cartesian_to_motors has taken ownership of a line motion
#ifdef DEBUG
volatile bool sys_rt_exec_debug;
#endif
@@ -167,9 +167,6 @@ void system_flag_wco_change() {
sys.report_wco_counter = 0;
}
-// Returns machine position of axis 'idx'. Must be sent a 'step' array.
-// NOTE: If motor steps and machine position are not in the same coordinate frame, this function
-// serves as a central place to compute the transformation.
float system_convert_axis_steps_to_mpos(int32_t* steps, uint8_t idx) {
float pos;
float steps_per_mm = axis_settings[idx]->steps_per_mm->get();
@@ -177,14 +174,22 @@ float system_convert_axis_steps_to_mpos(int32_t* steps, uint8_t idx) {
return pos;
}
+// Returns machine position of axis 'idx'. Must be sent a 'step' array.
+// NOTE: If motor steps and machine position are not in the same coordinate frame, this function
+// serves as a central place to compute the transformation.
void system_convert_array_steps_to_mpos(float* position, int32_t* steps) {
- uint8_t idx;
- auto n_axis = number_axis->get();
- for (idx = 0; idx < n_axis; idx++) {
- position[idx] = system_convert_axis_steps_to_mpos(steps, idx);
+ auto n_axis = number_axis->get();
+ float motors[n_axis];
+ for (int idx = 0; idx < n_axis; idx++) {
+ motors[idx] = (float)steps[idx] / axis_settings[idx]->steps_per_mm->get();
}
- return;
+ motors_to_cartesian(position, motors, n_axis);
}
+float* system_get_mpos() {
+ static float position[MAX_N_AXIS];
+ system_convert_array_steps_to_mpos(position, sys_position);
+ return position;
+};
// Returns control pin state as a uint8 bitfield. Each bit indicates the input pin state, where
// triggered is 1 and not triggered is 0. Invert mask is applied. Bitfield organization is
diff --git a/Grbl_Esp32/src/System.h b/Grbl_Esp32/src/System.h
index 3fb78809..c9f222ff 100644
--- a/Grbl_Esp32/src/System.h
+++ b/Grbl_Esp32/src/System.h
@@ -138,7 +138,7 @@ extern volatile Percent sys_rt_f_override; // Feed override
extern volatile Percent sys_rt_r_override; // Rapid feed override value in percent
extern volatile Percent sys_rt_s_override; // Spindle override value in percent
extern volatile bool cycle_stop;
-extern volatile void* sys_pl_data_inflight; // holds a plan_line_data_t while inverse_kinematics has taken ownership of a line motion
+extern volatile void* sys_pl_data_inflight; // holds a plan_line_data_t while cartesian_to_motors has taken ownership of a line motion
#ifdef DEBUG
extern volatile bool sys_rt_exec_debug;
#endif
@@ -165,7 +165,8 @@ void system_flag_wco_change();
float system_convert_axis_steps_to_mpos(int32_t* steps, uint8_t idx);
// Updates a machine 'position' array based on the 'step' array sent.
-void system_convert_array_steps_to_mpos(float* position, int32_t* steps);
+void system_convert_array_steps_to_mpos(float* position, int32_t* steps);
+float* system_get_mpos();
// A task that runs after a control switch interrupt for debouncing.
void controlCheckTask(void* pvParameters);