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

Update parallel_delta.cpp

Browse Source
This commit is contained in:
bdring
2020-09-26 10:18:39 -05:00
parent 193775001d
commit 0c60b95b8c
1 changed files with 28 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files

29
Grbl_Esp32/Custom/parallel_delta.cpp
View File

@@ -72,6 +72,7 @@ const float f = LENGTH_FIXED_SIDE; // sized of fixed side triangel
const float e = LENGTH_EFF_SIDE; // size of end effector side triangle const float e = LENGTH_EFF_SIDE; // size of end effector side triangle
static float last_angle[N_AXIS] = { 0.0, 0.0, 0.0 }; // A place to save the previous motor angles for distance/feed rate calcs static float last_angle[N_AXIS] = { 0.0, 0.0, 0.0 }; // A place to save the previous motor angles for distance/feed rate calcs
static float last_cartesian[N_AXIS] = { 0.0, 0.0, 0.0 }; // A place to save the previous motor angles for distance/feed rate calcs
float delta_z_offset; // Z offset of the effector from the arm centers float delta_z_offset; // Z offset of the effector from the arm centers
// prototypes for helper functions // prototypes for helper functions
@@ -114,6 +115,18 @@ void inverse_kinematics(float* target, plan_line_data_t* pl_data, float* positio
float pos_cart[N_AXIS]; float pos_cart[N_AXIS];
KinematicError status; KinematicError status;
grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, Target: %3.3f %3.3f %3.3f", target[0], target[1], target[2]);
grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Position: %3.3f %3.3f %3.3f", position[0], position[1], position[2]);
// Look for unchanged axes. If there is no position change on an axis in gcode, it will send the angle
// G0Z10 will send the Z10 and the previous values for other axes. Unfortunately those are in angles
// we need to restore them for our previous postision copy.
for (int axis = X_AXIS; axis <= Z_AXIS; axis++) {
if (target[axis] == last_angle[axis]) {
target[axis] == last_cartesian[axis];
}
}
// convert the current position to cartesian // convert the current position to cartesian
calc_forward_kinematics(position, pos_cart); calc_forward_kinematics(position, pos_cart);
@@ -122,13 +135,14 @@ void inverse_kinematics(float* target, plan_line_data_t* pl_data, float* positio
grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "From Cart: %3.3f %3.3f %3.3f", pos_cart[0], pos_cart[1], pos_cart[2]); grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "From Cart: %3.3f %3.3f %3.3f", pos_cart[0], pos_cart[1], pos_cart[2]);
grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Raw Target: %3.3f %3.3f %3.3f", target[0], target[1], target[2]); grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Raw Target: %3.3f %3.3f %3.3f", target[0], target[1], target[2]);
/*
grbl_msg_sendf(CLIENT_SERIAL, grbl_msg_sendf(CLIENT_SERIAL,
MsgLevel::Info, MsgLevel::Info,
"Offsets: %3.3f %3.3f %3.3f", "Z Offsets: %3.3f %3.3f %3.3f",
delta_z_offset, delta_z_offset,
gc_state.coord_offset[Z_AXIS], gc_state.coord_offset[Z_AXIS],
gc_state.coord_system[Z_AXIS]); gc_state.coord_system[Z_AXIS]);
*/
//target[Z_AXIS] += delta_z_offset + (gc_state.coord_system[Z_AXIS] + gc_state.coord_offset[Z_AXIS]); //target[Z_AXIS] += delta_z_offset + (gc_state.coord_system[Z_AXIS] + gc_state.coord_offset[Z_AXIS]);
grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "To: %3.3f %3.3f %3.3f", target[0], target[1], target[2]); grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "To: %3.3f %3.3f %3.3f", target[0], target[1], target[2]);
@@ -362,13 +376,22 @@ void kinematics_post_homing() {
last_angle[Y_AXIS] = sys_position[Y_AXIS] / axis_settings[Y_AXIS]->steps_per_mm->get(); 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(); last_angle[Z_AXIS] = sys_position[Z_AXIS] / axis_settings[Z_AXIS]->steps_per_mm->get();
calc_forward_kinematics(last_angle, last_cartesian);
grbl_msg_sendf(CLIENT_SERIAL, grbl_msg_sendf(CLIENT_SERIAL,
MsgLevel::Info, MsgLevel::Info,
"kinematics_post_homing: %3.3f, %3.3f, %3.3f", "kinematics_post_homing Angles: %3.3f, %3.3f, %3.3f",
last_angle[X_AXIS], last_angle[X_AXIS],
last_angle[Y_AXIS], last_angle[Y_AXIS],
last_angle[Z_AXIS]); last_angle[Z_AXIS]);
grbl_msg_sendf(CLIENT_SERIAL,
MsgLevel::Info,
"kinematics_post_homing Cartesian: %3.3f, %3.3f, %3.3f",
last_cartesian[X_AXIS],
last_cartesian[Y_AXIS],
last_cartesian[Z_AXIS]);
#endif #endif
} }