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

WIP

Browse Source
This commit is contained in:
bdring
2020-09-30 10:54:42 -05:00
parent f866a57828
commit ed157d7f25
3 changed files with 23 additions and 138 deletions
Download Patch File Download Diff File Expand all files Collapse all files

157
Grbl_Esp32/Custom/parallel_delta.cpp
View File

@@ -90,7 +90,7 @@ void machine_init() {
calc_forward_kinematics(angles, cartesian); // Sets the cartesian values calc_forward_kinematics(angles, cartesian); // Sets the cartesian values
delta_z_offset = cartesian[Z_AXIS]; delta_z_offset = cartesian[Z_AXIS];
// print a startup message to show the kinematics are enables // 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, delta_z_offset); grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Delta Kinematics Init: %s Z Offset:%4.3f", MACHINE_NAME, delta_z_offset);
} }
@@ -98,43 +98,52 @@ bool user_defined_homing() { // true = do not continue with normal Grbl homing
#ifdef USE_CUSTOM_HOMING #ifdef USE_CUSTOM_HOMING
return true; return true;
#else #else
//grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "User defined homing");
return false; return false;
#endif #endif
} }
void inverse_kinematics(float* position) {
float motor_angles[3];
delta_calcInverse(position, motor_angles);
position[0] = motor_angles[0];
position[1] = motor_angles[1];
position[2] = motor_angles[2];
}
void inverse_kinematics(float* target, plan_line_data_t* pl_data, float* position) //The target and position are provided in MPos void 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 dx, dy, dz; // distances in each cartesian axis
float motor_angles[3]; float motor_angles[3];
float seg_target[3]; // The target of the current segment float seg_target[3]; // The target of the current segment
float feed_rate = pl_data->feed_rate; // save original feed rate float feed_rate = pl_data->feed_rate; // save original feed rate
bool start_position_erorr = false; bool start_position_error = false;
bool show_error = true; // shows error once bool show_error = true; // shows error once
KinematicError status; KinematicError status;
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]);
//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]);
status = delta_calcInverse(position, motor_angles); status = delta_calcInverse(position, motor_angles);
if (status == KinematicError::OUT_OF_RANGE) { 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]); grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Start position error %3.3f %3.3f %3.3f", position[0], position[1], position[2]);
//start_position_erorr = true; start_position_error = true;
} }
// Check the destination to see if it is in work area // Check the destination to see if it is in work area
status = delta_calcInverse(target, motor_angles); status = delta_calcInverse(target, motor_angles);
if (status == KinematicError::OUT_OF_RANGE) { 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]); grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Target unreachable error %3.3f %3.3f %3.3f", target[0], target[1], target[2]);
//return; return;
} }
position[X_AXIS] += gc_state.coord_offset[X_AXIS]; position[X_AXIS] += gc_state.coord_offset[X_AXIS];
position[Y_AXIS] += gc_state.coord_offset[Y_AXIS]; position[Y_AXIS] += gc_state.coord_offset[Y_AXIS];
position[Z_AXIS] += gc_state.coord_offset[Z_AXIS]; position[Z_AXIS] += gc_state.coord_offset[Z_AXIS];
// calculate cartesian move distance for each axis
dx = target[X_AXIS] - position[X_AXIS]; dx = target[X_AXIS] - position[X_AXIS];
dy = target[Y_AXIS] - position[Y_AXIS]; dy = target[Y_AXIS] - position[Y_AXIS];
dz = target[Z_AXIS] - position[Z_AXIS]; dz = target[Z_AXIS] - position[Z_AXIS];
@@ -143,19 +152,14 @@ void inverse_kinematics(float* target, plan_line_data_t* pl_data, float* positio
// determine the number of segments we need ... round up so there is at least 1 (except when dist is 0) // determine the number of segments we need ... round up so there is at least 1 (except when dist is 0)
uint32_t segment_count = ceil(dist / KINEMATIC_SEGMENT_LENGTH); uint32_t segment_count = ceil(dist / KINEMATIC_SEGMENT_LENGTH);
//grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Dist %3.3f Segemnts %d", dist, segment_count);
float segment_dist = dist / ((float)segment_count); // distance of each segment...will be used for feedrate conversion float segment_dist = dist / ((float)segment_count); // distance of each segment...will be used for feedrate conversion
for (uint32_t segment = 1; segment <= segment_count; segment++) { for (uint32_t segment = 1; segment <= segment_count; segment++) {
// determine this segment's target // determine this segment's target
seg_target[X_AXIS] = position[X_AXIS] + (dx / float(segment_count) * segment); seg_target[X_AXIS] = position[X_AXIS] + (dx / float(segment_count) * segment);
seg_target[Y_AXIS] = position[Y_AXIS] + (dy / float(segment_count) * segment); seg_target[Y_AXIS] = position[Y_AXIS] + (dy / float(segment_count) * segment);
seg_target[Z_AXIS] = position[Z_AXIS] + (dz / float(segment_count) * segment); seg_target[Z_AXIS] = position[Z_AXIS] + (dz / float(segment_count) * segment);
//grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Segment Target: %3.3f %3.3f %3.3f", seg_target[0], seg_target[1], seg_target[2]);
// calculate the delta motor angles // calculate the delta motor angles
KinematicError status = delta_calcInverse(seg_target, motor_angles); KinematicError status = delta_calcInverse(seg_target, motor_angles);
@@ -171,7 +175,6 @@ void inverse_kinematics(float* target, plan_line_data_t* pl_data, float* positio
pl_data->feed_rate = (feed_rate * delta_distance / segment_dist); pl_data->feed_rate = (feed_rate * delta_distance / segment_dist);
} }
mc_line(motor_angles, pl_data); mc_line(motor_angles, pl_data);
} else { } else {
@@ -187,128 +190,6 @@ void inverse_kinematics(float* target, plan_line_data_t* pl_data, float* positio
} }
} }
} }
}
void inverse_kinematic2(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 motor_angles[3];
float seg_target[3]; // The target of the current segment
float feed_rate = pl_data->feed_rate; // save original feed rate
//bool start_position_erorr = false;
bool show_error = true; // shows error once
float pos_cart[3];
float inital_position[3];
KinematicError status;
//memcpy(inital_position, position, sizeof(position));
// determine the initial position.
inital_position[X_AXIS] = position[X_AXIS] + gc_state.coord_system[X_AXIS];
inital_position[Y_AXIS] = position[Y_AXIS] + gc_state.coord_system[Y_AXIS];
inital_position[Z_AXIS] = position[Z_AXIS] + gc_state.coord_system[Z_AXIS];
//grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Init Pos: %3.3f %3.3f %3.3f", inital_position[0], inital_position[1], inital_position[2]);
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]);
status = delta_calcInverse(inital_position, motor_angles);
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_erorr = true;
}
// 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;
}
//target[Z_AXIS] -= delta_z_offset; // restore it
//memcpy(position, target, sizeof(target));
//memcpy(last_angle, motor_angles, sizeof(motor_angles));
// calculate cartesian move distance for each axis
//return;
dx = target[X_AXIS] - inital_position[X_AXIS];
dy = target[Y_AXIS] - inital_position[Y_AXIS];
dz = target[Z_AXIS] - inital_position[Z_AXIS];
float dist = sqrt((dx * dx) + (dy * dy) + (dz * dz));
// determine the number of segments we need ... round up so there is at least 1 (except when dist is 0)
uint32_t segment_count = ceil(dist / KINEMATIC_SEGMENT_LENGTH);
grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Dist %3.3f Segemnts %d", dist, segment_count);
float segment_dist = dist / ((float)segment_count); // distance of each segment...will be used for feedrate conversion
//position[X_AXIS] = target[X_AXIS] + gc_state.coord_system[X_AXIS];
//position[Y_AXIS] = target[Y_AXIS] + gc_state.coord_system[Y_AXIS];
//position[Z_AXIS] = target[Z_AXIS] + gc_state.coord_system[Z_AXIS];
// grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Position Return: %3.3f %3.3f %3.3f", position[0], position[1], position[2]);
// mc_line(motor_angles, pl_data);
// return;
for (uint32_t segment = 1; segment <= segment_count; segment++) {
// determine this segment's target
seg_target[X_AXIS] = inital_position[X_AXIS] + (dx / float(segment_count) * segment);
seg_target[Y_AXIS] = inital_position[Y_AXIS] + (dy / float(segment_count) * segment);
seg_target[Z_AXIS] = inital_position[Z_AXIS] + (dz / float(segment_count) * segment);
// seg_target[X_AXIS] = target[X_AXIS] + (dx / float(segment_count) * segment);
// seg_target[Y_AXIS] = target[Y_AXIS] + (dy / float(segment_count) * segment);
// seg_target[Z_AXIS] = target[Z_AXIS] + (dz / float(segment_count) * segment);
grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Segment Target: %3.3f %3.3f %3.3f", seg_target[0], seg_target[1], seg_target[2]);
// 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);
}
//position[X_AXIS] = seg_target[X_AXIS] + gc_state.coord_system[X_AXIS];
//position[Y_AXIS] = seg_target[Y_AXIS] + gc_state.coord_system[Y_AXIS];
//position[Z_AXIS] = seg_target[Z_AXIS] + gc_state.coord_system[Z_AXIS];
mc_line(motor_angles, pl_data);
} else {
if (show_error) {
grbl_msg_sendf(CLIENT_SERIAL,
MsgLevel::Info,
"Error:%d, Angs X:%4.3f Y:%4.3f Z:%4.3f]\r\n\r\n",
status,
motor_angles[0],
motor_angles[1],
motor_angles[2]);
show_error = false;
}
}
}
position[X_AXIS] = target[X_AXIS] - gc_state.coord_system[X_AXIS];
position[Y_AXIS] = target[Y_AXIS] - gc_state.coord_system[Y_AXIS];
position[Z_AXIS] = target[Z_AXIS] - gc_state.coord_system[Z_AXIS];
// position[X_AXIS] = target[X_AXIS];
// position[Y_AXIS] = target[Y_AXIS];
// position[Z_AXIS] = target[Z_AXIS];
grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Position Return: %3.3f %3.3f %3.3f", position[0], position[1], position[2]);
} }
// inverse kinematics: cartesian -> angles // inverse kinematics: cartesian -> angles

2
Grbl_Esp32/src/GCode.cpp
View File

@@ -1477,6 +1477,8 @@ Error gc_execute_line(char* line, uint8_t client) {
} }
mc_line(coord_data, pl_data); mc_line(coord_data, pl_data);
memcpy(gc_state.position, coord_data, sizeof(gc_state.position)); memcpy(gc_state.position, coord_data, sizeof(gc_state.position));
== == == = mc_line_kins(gc_block.values.ijk, pl_data, gc_state.position);
memcpy(gc_state.position, gc_block.values.ijk, MAX_N_AXIS * sizeof(float));
break; break;
case NonModal::SetHome0: case NonModal::SetHome0:
coords[CoordIndex::G28]->set(gc_state.position); coords[CoordIndex::G28]->set(gc_state.position);

2
Grbl_Esp32/src/Grbl.h
View File

@@ -99,11 +99,13 @@ void machine_init();
bool user_defined_homing(); bool user_defined_homing();
// Called if USE_KINEMATICS is defined // Called if USE_KINEMATICS is defined
void inverse_kinematics(float* target, plan_line_data_t* pl_data, float* position); void inverse_kinematics(float* target, plan_line_data_t* pl_data, float* position);
bool kinematics_pre_homing(uint8_t cycle_mask); bool kinematics_pre_homing(uint8_t cycle_mask);
void kinematics_post_homing(); void kinematics_post_homing();
// Called if USE_FWD_KINEMATICS is defined // Called if USE_FWD_KINEMATICS is defined
void inverse_kinematics(float* position); // used to return a converted value
void forward_kinematics(float* position); void forward_kinematics(float* position);
// Called if MACRO_BUTTON_0_PIN or MACRO_BUTTON_1_PIN or MACRO_BUTTON_2_PIN is defined // Called if MACRO_BUTTON_0_PIN or MACRO_BUTTON_1_PIN or MACRO_BUTTON_2_PIN is defined