From ed157d7f25ec6fc61e22ec252176007b24e7beb9 Mon Sep 17 00:00:00 2001 From: bdring Date: Wed, 30 Sep 2020 10:54:42 -0500 Subject: [PATCH] WIP --- Grbl_Esp32/Custom/parallel_delta.cpp | 157 ++++----------------------- Grbl_Esp32/src/GCode.cpp | 2 + Grbl_Esp32/src/Grbl.h | 2 + 3 files changed, 23 insertions(+), 138 deletions(-) diff --git a/Grbl_Esp32/Custom/parallel_delta.cpp b/Grbl_Esp32/Custom/parallel_delta.cpp index 4bf10932..aa8d8d9e 100644 --- a/Grbl_Esp32/Custom/parallel_delta.cpp +++ b/Grbl_Esp32/Custom/parallel_delta.cpp @@ -90,7 +90,7 @@ void machine_init() { calc_forward_kinematics(angles, cartesian); // Sets the cartesian values 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); } @@ -98,43 +98,52 @@ bool user_defined_homing() { // true = do not continue with normal Grbl homing #ifdef USE_CUSTOM_HOMING return true; #else - //grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "User defined homing"); return false; #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 { 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 seg_target[3]; // The target of the current segment + float feed_rate = pl_data->feed_rate; // save original feed rate + bool start_position_error = false; + bool show_error = true; // shows error once 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); 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; + start_position_error = 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; + return; } position[X_AXIS] += gc_state.coord_offset[X_AXIS]; position[Y_AXIS] += gc_state.coord_offset[Y_AXIS]; position[Z_AXIS] += gc_state.coord_offset[Z_AXIS]; + // calculate cartesian move distance for each axis dx = target[X_AXIS] - position[X_AXIS]; dy = target[Y_AXIS] - position[Y_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) 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 - 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); seg_target[Y_AXIS] = position[Y_AXIS] + (dy / 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 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); } - mc_line(motor_angles, pl_data); } 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 diff --git a/Grbl_Esp32/src/GCode.cpp b/Grbl_Esp32/src/GCode.cpp index 0134952e..1c9ff1e4 100644 --- a/Grbl_Esp32/src/GCode.cpp +++ b/Grbl_Esp32/src/GCode.cpp @@ -1477,6 +1477,8 @@ Error gc_execute_line(char* line, uint8_t client) { } mc_line(coord_data, pl_data); 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; case NonModal::SetHome0: coords[CoordIndex::G28]->set(gc_state.position); diff --git a/Grbl_Esp32/src/Grbl.h b/Grbl_Esp32/src/Grbl.h index b2e9c2d8..04ac70fc 100644 --- a/Grbl_Esp32/src/Grbl.h +++ b/Grbl_Esp32/src/Grbl.h @@ -99,11 +99,13 @@ void machine_init(); bool user_defined_homing(); // Called if USE_KINEMATICS is defined + void inverse_kinematics(float* target, plan_line_data_t* pl_data, float* position); bool kinematics_pre_homing(uint8_t cycle_mask); void kinematics_post_homing(); // Called if USE_FWD_KINEMATICS is defined +void inverse_kinematics(float* position); // used to return a converted value void forward_kinematics(float* position); // Called if MACRO_BUTTON_0_PIN or MACRO_BUTTON_1_PIN or MACRO_BUTTON_2_PIN is defined