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

Big kinematics cleanup (#875)

Browse Source 
* Applied 741 to new Devt

* Make kinematics routines weak

to eliminate ifdefs

* Fixed warning

* Big kinematics cleanup

* Cleanup

* no isCancelled arg for jog_execute; return it instead

* WIP

* Made OLED compliant with new kinematics

* Added system_get_mpos

* system_get_mpos() returns float*

* WIP

* Cleanup after testing

- Had MPos and WPos text on OLED backwards.
- Added my cartesian test def
- Will remove test defs before merging to devt.

* Cleanup of remaining user optional code.

* Fixed delta kinematics loop ending early.

* Account for jog cancel in saved motor positions

* Update Grbl.h

Co-authored-by: bdring <barton.dring@gmail.com>
This commit is contained in:
Mitch Bradley
2021-04-24 07:26:34 -10:00
committed by GitHub
parent 972599aaaa
commit e2740aeef8
34 changed files with 527 additions and 342 deletions
Download Patch File Download Diff File Expand all files Collapse all files

77
Grbl_Esp32/Custom/CoreXY.cpp
View File

@@ -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.

5
Grbl_Esp32/Custom/atari_1020.cpp
View File

@@ -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);
}
}

72
Grbl_Esp32/Custom/custom_code_template.cpp
View File

@@ -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

2
Grbl_Esp32/Custom/mpcnc_laser_module.cpp
View File

@@ -21,7 +21,6 @@
along with Grbl. If not, see <http://www.gnu.org/licenses/>.
*/
#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

9
Grbl_Esp32/Custom/oled_basic.cpp
View File

@@ -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++) {

143
Grbl_Esp32/Custom/parallel_delta.cpp
View File

@@ -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,

33
Grbl_Esp32/Custom/polar_coaster.cpp
View File

@@ -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

1
Grbl_Esp32/src/Error.cpp
View File

@@ -80,4 +80,5 @@ std::map<Error, const char*> ErrorNames = {
{ Error::NvsGetStatsFailed, "Failed to get setting status" },
{ Error::AuthenticationFailed, "Authentication failed!" },
{ Error::AnotherInterfaceBusy, "Another interface is busy" },
{ Error::JogCancelled, "Jog Cancelled" },
};

1
Grbl_Esp32/src/Error.h
View File

@@ -84,6 +84,7 @@ enum class Error : uint8_t {
AuthenticationFailed = 110,
Eol = 111,
AnotherInterfaceBusy = 120,
JogCancelled = 130,
};
extern std::map<Error, const char*> ErrorNames;

17
Grbl_Esp32/src/GCode.cpp
View File

@@ -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.

4
Grbl_Esp32/src/Grbl.cpp
View File

@@ -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:

18
Grbl_Esp32/src/Grbl.h
View File

@@ -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 <sdkconfig.h>
#include <Arduino.h>
@@ -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);

6
Grbl_Esp32/src/Jog.cpp
View File

@@ -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) {

3
Grbl_Esp32/src/Limits.cpp
View File

@@ -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;

110
Grbl_Esp32/src/Machines/6_pack_TMC2130_XYZ_Test.h Normal file
View File

@@ -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 <http://www.gnu.org/licenses/>.
*/
#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

4
Grbl_Esp32/src/Machines/atari_1020.h
View File

@@ -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

85
Grbl_Esp32/src/Machines/fysetc_ant.h Normal file
View File

@@ -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 <http://www.gnu.org/licenses/>.
*/
#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

6
Grbl_Esp32/src/Machines/fystec_e4.h → Grbl_Esp32/src/Machines/fysetc_e4.h
View File

@@ -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 <http://www.gnu.org/licenses/>.
*/
#define MACHINE_NAME "FYSTEC E4 3D Printer Controller"
#define MACHINE_NAME "FYSETC E4 3D Printer Controller"
#define N_AXIS 4

6
Grbl_Esp32/src/Machines/midtbot.h
View File

@@ -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

119
Grbl_Esp32/src/Machines/midtbot_x2.h Normal file
View File

@@ -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 <http://www.gnu.org/licenses/>.
*/
#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

1
Grbl_Esp32/src/Machines/mpcnc_laser_module_v1p2.h
View File

@@ -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"

5
Grbl_Esp32/src/Machines/polar_coaster.h
View File

@@ -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

4
Grbl_Esp32/src/Machines/tapster_3.h
View File

@@ -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)

4
Grbl_Esp32/src/Machines/tapster_pro_6P_trinamic.h
View File

@@ -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)

12
Grbl_Esp32/src/MotionControl.cpp
View File

@@ -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.

2
Grbl_Esp32/src/MotionControl.h
View File

@@ -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,

17
Grbl_Esp32/src/Motors/Dynamixel2.cpp
View File

@@ -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

4
Grbl_Esp32/src/Motors/Motor.cpp
View File

@@ -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() {}

1
Grbl_Esp32/src/Motors/Motors.cpp
View File

@@ -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

7
Grbl_Esp32/src/Protocol.cpp
View File

@@ -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);
}

57
Grbl_Esp32/src/Report.cpp
View File

@@ -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

6
Grbl_Esp32/src/Report.h
View File

@@ -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);

23
Grbl_Esp32/src/System.cpp
View File

@@ -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

5
Grbl_Esp32/src/System.h
View File

@@ -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);