From 2d58927a6cb3cc83803c1c610598195f8d708796 Mon Sep 17 00:00:00 2001 From: bdring Date: Mon, 28 Sep 2020 20:36:45 -0500 Subject: [PATCH] Still working on kinematics - Added an interface into the jogging section --- Grbl_Esp32/Custom/parallel_delta.cpp | 108 ++++++++------------------ Grbl_Esp32/src/Config.h | 4 +- Grbl_Esp32/src/Jog.cpp | 7 +- Grbl_Esp32/src/Machines/tapster_pro.h | 14 ++-- 4 files changed, 47 insertions(+), 86 deletions(-) diff --git a/Grbl_Esp32/Custom/parallel_delta.cpp b/Grbl_Esp32/Custom/parallel_delta.cpp index b3e4815c..3f031529 100644 --- a/Grbl_Esp32/Custom/parallel_delta.cpp +++ b/Grbl_Esp32/Custom/parallel_delta.cpp @@ -71,9 +71,9 @@ const float re = RADIUS_EFF; // radius of end effector side (length of l const float f = LENGTH_FIXED_SIDE; // sized of fixed side triangel 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 int calc_forward_kinematics(float* angles, float* cartesian); @@ -106,76 +106,33 @@ bool user_defined_homing() { // true = do not continue with normal Grbl homing 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[N_AXIS]; + float motor_angles[3]; - float seg_target[N_AXIS]; // 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[N_AXIS]; - float inital_position[N_AXIS]; + 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)); + //memcpy(inital_position, position, sizeof(position)); - inital_position[X_AXIS] += gc_state.coord_system[X_AXIS]; - inital_position[Y_AXIS] += gc_state.coord_system[Y_AXIS]; - inital_position[Z_AXIS] += gc_state.coord_system[Z_AXIS]; + // 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, "Sys Po: %3.3f %3.3f %3.3f", sys_position[0], sys_position[1], sys_position[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 - - calc_forward_kinematics(inital_position, pos_cart); - - //grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "From Angs: %3.3f %3.3f %3.3f", position[0], position[1], position[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, - "Z Offsets: %3.3f %3.3f %3.3f", - delta_z_offset, - gc_state.coord_offset[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]); - - //grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "To: %3.3f %3.3f %3.3f", target[0], target[1], target[2]); - status = delta_calcInverse(target, motor_angles); - //grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Angles: %3.3f %3.3f %3.3f", motor_angles[0], motor_angles[1], motor_angles[2]); - - // see if start is in work area...if not skip segments and try to go to target - -/* - float start_pos[N_AXIS]; - start_pos[X_AXIS] = position[X_AXIS] + gc_state.coord_system[X_AXIS]; - start_pos[Y_AXIS] = position[Y_AXIS] + gc_state.coord_system[Y_AXIS]; - start_pos[Z_AXIS] = position[Z_AXIS] + gc_state.coord_system[Z_AXIS]; -*/ 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; + //start_position_erorr = true; } // Check the destination to see if it is in work area @@ -186,35 +143,34 @@ void inverse_kinematics(float* target, plan_line_data_t* pl_data, float* positio //return; } + 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]; + //target[Z_AXIS] -= delta_z_offset; // restore it - memcpy(position, target, sizeof(target)); - - memcpy(last_angle, motor_angles, sizeof(motor_angles)); - + //memcpy(position, target, sizeof(target)); + //memcpy(last_angle, motor_angles, sizeof(motor_angles)); mc_line(motor_angles, pl_data); - 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]; + 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 / 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 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); + 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); // calculate the delta motor angles KinematicError status = delta_calcInverse(seg_target, motor_angles); @@ -391,8 +347,6 @@ void kinematics_post_homing() { #ifdef USE_CUSTOM_HOMING #else - //sys_position[Z_AXIS] = delta_z_offset * axis_settings[Z_AXIS]->steps_per_mm->get(); - //plan_sync_position(); last_angle[X_AXIS] = sys_position[X_AXIS] / axis_settings[X_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(); diff --git a/Grbl_Esp32/src/Config.h b/Grbl_Esp32/src/Config.h index 39d8d681..658cade3 100644 --- a/Grbl_Esp32/src/Config.h +++ b/Grbl_Esp32/src/Config.h @@ -96,11 +96,11 @@ const int MAX_N_AXIS = 6; //#define CONNECT_TO_SSID "your SSID" //#define SSID_PASSWORD "your SSID password" //CONFIGURE_EYECATCH_BEGIN (DO NOT MODIFY THIS LINE) -//#define ENABLE_BLUETOOTH // enable bluetooth +#define ENABLE_BLUETOOTH // enable bluetooth #define ENABLE_SD_CARD // enable use of SD Card to run jobs -//#define ENABLE_WIFI //enable wifi +#define ENABLE_WIFI //enable wifi #if defined(ENABLE_WIFI) || defined(ENABLE_BLUETOOTH) # define WIFI_OR_BLUETOOTH diff --git a/Grbl_Esp32/src/Jog.cpp b/Grbl_Esp32/src/Jog.cpp index 5d7b5773..1c0c3d00 100644 --- a/Grbl_Esp32/src/Jog.cpp +++ b/Grbl_Esp32/src/Jog.cpp @@ -37,8 +37,13 @@ Error jog_execute(plan_line_data_t* pl_data, parser_block_t* gc_block) { return Error::TravelExceeded; } } - // Valid jog command. Plan, set state, and execute. +// Valid jog command. Plan, set state, and execute. +#ifndef USE_KINEMATICS mc_line(gc_block->values.xyz, pl_data); +#else // else use kinematics + inverse_kinematics(gc_block->values.xyz, pl_data, gc_state.position); +#endif + if (sys.state == State::Idle) { if (plan_get_current_block() != NULL) { // Check if there is a block to execute. sys.state = State::Jog; diff --git a/Grbl_Esp32/src/Machines/tapster_pro.h b/Grbl_Esp32/src/Machines/tapster_pro.h index 991ecfa0..0e3b5ae9 100644 --- a/Grbl_Esp32/src/Machines/tapster_pro.h +++ b/Grbl_Esp32/src/Machines/tapster_pro.h @@ -48,12 +48,14 @@ // ================== Delta Geometry =========================== -#define RADIUS_FIXED 100.0f // radius of the fixed side (length of motor cranks) -#define RADIUS_EFF 220.0f // radius of end effector side (length of linkages) -#define LENGTH_FIXED_SIDE 294.449f // sized of fixed side triangel -#define LENGTH_EFF_SIDE 86.6025f // size of end effector side triangle -#define SEGMENT_LENGTH 0.5f // segment length in mm -#define MAX_NEGATIVE_ANGLE -0.75f // +#define RADIUS_FIXED 100.0f // radius of the fixed side (length of motor cranks) +#define RADIUS_EFF 220.0f // radius of end effector side (length of linkages) +#define LENGTH_FIXED_SIDE 294.449f // sized of fixed side triangel +#define LENGTH_EFF_SIDE 86.6025f // size of end effector side triangle +#define KINEMATIC_SEGMENT_LENGTH 0.5f // segment length in mm +#define MAX_NEGATIVE_ANGLE -0.75f // +#define MAX_POSITIVE_ANGLE (M_PI / 2.0) // + // ================== Config ======================