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

Still working on kinematics

Browse Source 
- Added an interface into the jogging section
This commit is contained in:
bdring
2020-09-28 20:36:45 -05:00
parent 7818a4426f
commit 2d58927a6c
4 changed files with 47 additions and 86 deletions
Download Patch File Download Diff File Expand all files Collapse all files

108
Grbl_Esp32/Custom/parallel_delta.cpp
View File

@@ -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 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 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
int calc_forward_kinematics(float* angles, float* cartesian); 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 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[N_AXIS]; float motor_angles[3];
float seg_target[N_AXIS]; // 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_erorr = false;
bool show_error = true; // shows error once bool show_error = true; // shows error once
float pos_cart[N_AXIS]; float pos_cart[3];
float inital_position[N_AXIS]; float inital_position[3];
KinematicError status; KinematicError status;
memcpy(inital_position, position, sizeof(position)); //memcpy(inital_position, position, sizeof(position));
inital_position[X_AXIS] += gc_state.coord_system[X_AXIS]; // determine the initial position.
inital_position[Y_AXIS] += gc_state.coord_system[Y_AXIS]; inital_position[X_AXIS] = position[X_AXIS] + gc_state.coord_system[X_AXIS];
inital_position[Z_AXIS] += gc_state.coord_system[Z_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, "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, "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]); 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); status = delta_calcInverse(inital_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_erorr = true;
} }
// Check the destination to see if it is in work area // 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; //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 //target[Z_AXIS] -= delta_z_offset; // restore it
memcpy(position, target, sizeof(target)); //memcpy(position, target, sizeof(target));
//memcpy(last_angle, motor_angles, sizeof(motor_angles));
memcpy(last_angle, motor_angles, sizeof(motor_angles));
mc_line(motor_angles, pl_data); 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 // calculate cartesian move distance for each axis
return;
dx = target[X_AXIS] - position[X_AXIS]; dx = target[X_AXIS] - inital_position[X_AXIS];
dy = target[Y_AXIS] - position[Y_AXIS]; dy = target[Y_AXIS] - inital_position[Y_AXIS];
dz = target[Z_AXIS] - position[Z_AXIS]; dz = target[Z_AXIS] - inital_position[Z_AXIS];
float dist = sqrt((dx * dx) + (dy * dy) + (dz * dz)); 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) // 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 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] = inital_position[X_AXIS] + (dx / float(segment_count) * segment);
seg_target[Y_AXIS] = position[Y_AXIS] + (dy / float(segment_count) * segment); seg_target[Y_AXIS] = inital_position[Y_AXIS] + (dy / float(segment_count) * segment);
seg_target[Z_AXIS] = position[Z_AXIS] + (dz / float(segment_count) * segment); seg_target[Z_AXIS] = inital_position[Z_AXIS] + (dz / float(segment_count) * segment);
// 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);
@@ -391,8 +347,6 @@ void kinematics_post_homing() {
#ifdef USE_CUSTOM_HOMING #ifdef USE_CUSTOM_HOMING
#else #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[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[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();

4
Grbl_Esp32/src/Config.h
View File

@@ -96,11 +96,11 @@ const int MAX_N_AXIS = 6;
//#define CONNECT_TO_SSID "your SSID" //#define CONNECT_TO_SSID "your SSID"
//#define SSID_PASSWORD "your SSID password" //#define SSID_PASSWORD "your SSID password"
//CONFIGURE_EYECATCH_BEGIN (DO NOT MODIFY THIS LINE) //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_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) #if defined(ENABLE_WIFI) || defined(ENABLE_BLUETOOTH)
# define WIFI_OR_BLUETOOTH # define WIFI_OR_BLUETOOTH

7
Grbl_Esp32/src/Jog.cpp
View File

@@ -37,8 +37,13 @@ Error jog_execute(plan_line_data_t* pl_data, parser_block_t* gc_block) {
return Error::TravelExceeded; 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); 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 (sys.state == State::Idle) {
if (plan_get_current_block() != NULL) { // Check if there is a block to execute. if (plan_get_current_block() != NULL) { // Check if there is a block to execute.
sys.state = State::Jog; sys.state = State::Jog;

14
Grbl_Esp32/src/Machines/tapster_pro.h
View File

@@ -48,12 +48,14 @@
// ================== Delta Geometry =========================== // ================== Delta Geometry ===========================
#define RADIUS_FIXED 100.0f // radius of the fixed side (length of motor cranks) #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 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_FIXED_SIDE 294.449f // sized of fixed side triangel
#define LENGTH_EFF_SIDE 86.6025f // size of end effector side triangle #define LENGTH_EFF_SIDE 86.6025f // size of end effector side triangle
#define SEGMENT_LENGTH 0.5f // segment length in mm #define KINEMATIC_SEGMENT_LENGTH 0.5f // segment length in mm
#define MAX_NEGATIVE_ANGLE -0.75f // #define MAX_NEGATIVE_ANGLE -0.75f //
#define MAX_POSITIVE_ANGLE (M_PI / 2.0) //
// ================== Config ====================== // ================== Config ======================