diff --git a/changelog.md b/changelog.md index 2d0cad2..8bf817c 100644 --- a/changelog.md +++ b/changelog.md @@ -1,6 +1,20 @@ ## grblHAL changelog -20230818 +Build 20230820 + +Core: + +* Delta kinematics improvements. Added setting for base > floor distance, `$DELTA` command for work envelope info. Still WIP. + +* Changed signature of `grbl.on_homing_completed` event. + +Drivers: + +* RP2040: Fix for [issue #72](https://github.com/grblHAL/RP2040/discussions/72) \(typo\), improved SPI chip select handling. + +--- + +Build 20230818 Core: diff --git a/config.h b/config.h index 6dca75f..022d62b 100644 --- a/config.h +++ b/config.h @@ -137,6 +137,9 @@ Experimental - testing required and homing needs to be worked out. */ #if !defined DELTA_ROBOT || defined __DOXYGEN__ #define DELTA_ROBOT Off +#if !defined MINIMUM_FEED_RATE +#define MINIMUM_FEED_RATE 0.1f // (radians/min) +#endif #endif /*! \def POLAR_ROBOT diff --git a/core_handlers.h b/core_handlers.h index c2e2e63..6198edc 100644 --- a/core_handlers.h +++ b/core_handlers.h @@ -97,7 +97,7 @@ typedef void (*on_unknown_feedback_message_ptr)(stream_write_ptr stream_write); typedef void (*on_stream_changed_ptr)(stream_type_t type); typedef bool (*on_laser_ppi_enable_ptr)(uint_fast16_t ppi, uint_fast16_t pulse_length); typedef void (*on_homing_rate_set_ptr)(axes_signals_t axes, float rate, homing_mode_t mode); -typedef void (*on_homing_completed_ptr)(void); +typedef void (*on_homing_completed_ptr)(bool succes); typedef bool (*on_probe_fixture_ptr)(tool_data_t *tool, bool at_g59_3, bool on); typedef bool (*on_probe_start_ptr)(axes_signals_t axes, float *target, plan_line_data_t *pl_data); typedef void (*on_probe_completed_ptr)(void); diff --git a/grbl.h b/grbl.h index 69033ac..df89b43 100644 --- a/grbl.h +++ b/grbl.h @@ -42,7 +42,7 @@ #else #define GRBL_VERSION "1.1f" #endif -#define GRBL_BUILD 20230818 +#define GRBL_BUILD 20230820 #define GRBL_URL "https://github.com/grblHAL" diff --git a/kinematics/delta.c b/kinematics/delta.c index 9a4e167..6ca8a31 100644 --- a/kinematics/delta.c +++ b/kinematics/delta.c @@ -6,6 +6,8 @@ Copyright (c) 2023 Terje Io Transforms derived from mzavatsky at Trossen Robotics https://hypertriangle.com/~alex/delta-robot-tutorial/ + get_cuboid_envelope() derived from javascript code in + https://www.marginallyclever.com/other/samples/fk-ik-test.html Grbl is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by @@ -35,33 +37,29 @@ #include "../kinematics.h" typedef struct { - float rf; - float re; - float f; - float e; - float sl; + float rf; // bicep length + float re; // forearm length + float f; // base radius + float e; // end effector radius + float b; // base to floor + float sl; // max segment length TODO: replace with calculated value? } delta_settings_t; -// trigonometric constants -const float sqrt3 = 1.732050807f; -const float sin120 = sqrt3 / 2.0f; -const float cos120 = -0.5f; -const float tan60 = sqrt3; -const float sin30 = 0.5f; -const float tan30 = 1.0f / sqrt3; - static bool jog_cancel = false; +static coord_data_t last_pos = {0}; static delta_settings_t machine = {0}, delta_settings; static on_report_options_ptr on_report_options; +static on_homing_completed_ptr on_homing_completed; +static on_report_command_help_ptr on_report_command_help; static nvs_address_t nvs_address; -static float z0; +static float z0, resolution; // inverse kinematics // helper functions, calculates angle position[X_AXIS] (for YZ-pane) int delta_calcAngleYZ (float x0, float y0, float z0, float *theta) { - float y1 = -0.5f * 0.57735f * machine.f; // f/2 * tg 30 - y0 -= 0.5f * 0.57735f * machine.e; // shift center to edge + float y1 = -0.5f * TAN30 * machine.f; // f/2 * tg 30 + y0 -= 0.5f * TAN30 * machine.e; // shift center to edge // z = a + b*y float a = (x0 * x0 + y0 * y0 + z0 * z0 + machine.rf * machine.rf - machine.re * machine.re - y1 * y1) / (2.0f * z0); float b = (y1 - y0) / z0; @@ -69,6 +67,7 @@ int delta_calcAngleYZ (float x0, float y0, float z0, float *theta) float d = -(a + b * y1) * (a + b * y1) + machine.rf * (b * b * machine.rf + machine.rf); if (d < 0.0f) return -1; // non-existing point + float yj = (y1 - a * b - sqrt(d)) / (b * b + 1); // choosing outer point float zj = a + b * yj; // *theta = 180.0f * atanf(-zj / (y1 - yj)) / M_PI + ((yj > y1) ? 180.0f : 0.0f); @@ -79,24 +78,23 @@ int delta_calcAngleYZ (float x0, float y0, float z0, float *theta) // inverse kinematics: (x0, y0, z0) -> (position[X_AXIS], position[Y_AXIS], position[Z_AXIS]) // returned status: 0=OK, -1=non-existing position -int delta_calcInverse (coord_data_t *cpos, float *position) +int delta_calcInverse (coord_data_t *mpos, float *position) { int status; position[X_AXIS] = position[Y_AXIS] = position[Z_AXIS] = 0.0f; - if((status = delta_calcAngleYZ(cpos->x, cpos->y, cpos->z, &position[X_AXIS])) == 0 && - (status = delta_calcAngleYZ(cpos->x * cos120 + cpos->y * sin120, cpos->y * cos120 - cpos->x * sin120, cpos->z, &position[Y_AXIS])) == 0) // rotate coords to +120 deg - status = delta_calcAngleYZ(cpos->x * cos120 - cpos->y * sin120, cpos->y * cos120 + cpos->x * sin120, cpos->z, &position[Z_AXIS]); // rotate coords to -120 deg + if((status = delta_calcAngleYZ(mpos->x, mpos->y, mpos->z, &position[X_AXIS])) == 0 && + (status = delta_calcAngleYZ(mpos->x * COS120 + mpos->y * SIN120, mpos->y * COS120 - mpos->x * SIN120, mpos->z, &position[Y_AXIS])) == 0) // rotate coords to +120 deg + status = delta_calcAngleYZ(mpos->x * COS120 - mpos->y * SIN120, mpos->y * COS120 + mpos->x * SIN120, mpos->z, &position[Z_AXIS]); // rotate coords to -120 deg return status; } -// Returns machine position in mm converted from system position steps. -// TODO: perhaps change to double precision here - float calculation results in errors of a couple of micrometers. +// Returns machine position in mm converted from system position. static float *transform_to_cartesian (float *target, float *position) { - float t = (machine.f - machine.e) * tan30 / 2.0f; + float t = (machine.f - machine.e) * TAN30_2; /* float dtr = M_PI / 180.0f; position[X_AXIS] *= dtr; @@ -106,12 +104,12 @@ static float *transform_to_cartesian (float *target, float *position) float y1 = -(t + machine.rf * cosf(position[X_AXIS])); float z1 = -machine.rf * sinf(position[X_AXIS]); - float y2 = (t + machine.rf * cosf(position[Y_AXIS])) * sin30; - float x2 = y2 * tan60; + float y2 = (t + machine.rf * cosf(position[Y_AXIS])) * SIN30; + float x2 = y2 * TAN60; float z2 = -machine.rf * sinf(position[Y_AXIS]); - float y3 = (t + machine.rf * cosf(position[Z_AXIS])) * sin30; - float x3 = -y3 * tan60; + float y3 = (t + machine.rf * cosf(position[Z_AXIS])) * SIN30; + float x3 = -y3 * TAN60; float z3 = -machine.rf * sinf(position[Z_AXIS]); float dnm = (y2 - y1) * x3 -(y3 - y1) * x2; @@ -147,21 +145,20 @@ static float *transform_to_cartesian (float *target, float *position) } // Returns machine position in mm converted from system position steps. -// TODO: perhaps change to double precision here - float calculation results in errors of a couple of micrometers. -static float *wp_convert_array_steps_to_mpos (float *position, int32_t *steps) +static float *delta_convert_array_steps_to_mpos (float *position, int32_t *steps) { - float cpos[N_AXIS]; + float mpos[N_AXIS]; uint_fast8_t idx = N_AXIS; do { idx--; - cpos[idx] = steps[idx] / settings.axis[idx].steps_per_mm; + mpos[idx] = steps[idx] / settings.axis[idx].steps_per_mm; } while(idx); - return transform_to_cartesian(position, cpos); + return transform_to_cartesian(position, mpos); } -// Transform absolute position from cartesian coordinate system to wall plotter coordinate system +// Transform absolute position from cartesian coordinate system to delta robot coordinate system static float *transform_from_cartesian (float *target, float *position) { delta_calcInverse((coord_data_t *)position, target); @@ -171,7 +168,7 @@ static float *transform_from_cartesian (float *target, float *position) static inline float get_distance (float *p0, float *p1) { - uint_fast8_t idx = Z_AXIS; + uint_fast8_t idx = Z_AXIS + 1; float distance = 0.0f; do { @@ -182,13 +179,13 @@ static inline float get_distance (float *p0, float *p1) return sqrtf(distance); } -// Wall plotter is circular in motion, so long lines must be divided up +// Delta robots needs long lines divided up static float *delta_segment_line (float *target, float *position, plan_line_data_t *pl_data, bool init) { static uint_fast16_t iterations; static bool segmented; - static coord_data_t delta, segment_target, final_target, cpos; -// static plan_line_data_t plan; + static float distance; + static coord_data_t delta, segment_target, final_target, mpos; uint_fast8_t idx = N_AXIS; @@ -197,7 +194,7 @@ static float *delta_segment_line (float *target, float *position, plan_line_data jog_cancel = false; memcpy(final_target.values, target, sizeof(final_target)); - if(delta_calcInverse((coord_data_t *)target, cpos.values) == 0) { + if(delta_calcInverse((coord_data_t *)target, mpos.values) == 0) { transform_to_cartesian(segment_target.values, position); @@ -205,7 +202,7 @@ static float *delta_segment_line (float *target, float *position, plan_line_data delta.y = target[Y_AXIS] - segment_target.y; delta.z = target[Z_AXIS] - segment_target.z; - float distance = sqrtf(delta.x * delta.x + delta.y * delta.y); + distance = sqrtf(delta.x * delta.x + delta.y * delta.y + delta.z * delta.z); if((segmented = distance > machine.sl && !(delta.x == 0.0f && delta.y == 0.0f && delta.z == 0.0f))) { @@ -221,7 +218,10 @@ static float *delta_segment_line (float *target, float *position, plan_line_data iterations = 1; memcpy(&segment_target, &final_target, sizeof(coord_data_t)); } - } else { // out of bounds, report? + + distance /= (float)iterations; + + } else { // out of bounds, report? or should never happen? iterations = 1; memcpy(&segment_target, target, sizeof(coord_data_t)); } @@ -237,115 +237,118 @@ static float *delta_segment_line (float *target, float *position, plan_line_data idx--; segment_target.values[idx] += delta.values[idx]; } while(idx); - } else memcpy(&segment_target, &final_target, sizeof(coord_data_t)); - delta_calcInverse(&segment_target, cpos.values); + delta_calcInverse(&segment_target, mpos.values); + + if(!pl_data->condition.rapid_motion && distance != 0.0f) { + float rate_multiplier = get_distance(mpos.values, last_pos.values) / distance; + pl_data->feed_rate *= rate_multiplier; + pl_data->rate_multiplier = 1.0 / rate_multiplier; + } + + memcpy(&last_pos, &mpos, sizeof(coord_data_t)); } - return iterations == 0 || jog_cancel ? NULL : cpos.values; + return iterations == 0 || jog_cancel ? NULL : mpos.values; } -// original javascript source: https://www.marginallyclever.com/other/samples/fk-ik-test.html static void get_cuboid_envelope (void) { float maxx = -machine.e - machine.f - machine.re - machine.rf; float maxz = maxx; float minz = -maxx; - float sr = (2.0f * M_PI) / settings.axis[X_AXIS].steps_per_mm; // Steps/rev -> steps/rad, XYZ motors should have the same setting! + float sr = (2.0f * M_PI) / settings.axis[X_AXIS].steps_per_mm; // Steps/rev -> rad/step, XYZ motors should have the same setting! float pos[N_AXIS]; - coord_data_t cpos; - struct xxx { + uint32_t z; + coord_data_t mpos; + struct { int32_t res; float pos[N_AXIS]; } r[8]; -#if (defined(SET_ORIGIN_AT_HOME_POS) || defined(DELTA_HOME_LIMITS_MAXZ)) - float homez = axis[AXIS_Z]; -#endif - // find extents - for (int32_t z = 0; z < settings.axis[X_AXIS].steps_per_mm; ++z) { + for(z = 0; z < settings.axis[X_AXIS].steps_per_mm; ++z) { pos[0] = pos[1] = pos[2] = sr * (float)z; - transform_to_cartesian(cpos.values, pos); - if(!isnanf(cpos.x)) { - if(minz > cpos.z) - minz = cpos.z; - if(maxz < cpos.z) - maxz = cpos.z; + transform_to_cartesian(mpos.values, pos); + if(!isnanf(mpos.x)) { + if(minz > mpos.z) + minz = mpos.z; + if(maxz < mpos.z) + maxz = mpos.z; } } - float btf = settings.axis[Z_AXIS].max_travel; + float btf = -machine.b; if(minz < btf) minz = btf; if(maxz < btf) maxz = btf; float middlez = (maxz + minz) * 0.5f; - // $('#output').append("

("+maxz+","+minz+","+middlez+")

"); float original_dist = (maxz - middlez); float dist = original_dist * 0.5f; float sum = 0.0f; +/* float mint1 = 2.0 * M_PI; float maxt1 = -2.0 * M_PI; float mint2 = 2.0 * M_PI; float maxt2 = -2.0 * M_PI; float mint3 = 2.0 * M_PI; float maxt3 = -2.0 * M_PI; - +*/ do { sum += dist; - cpos.x = sum; - cpos.y = sum; - cpos.z = middlez + sum; - r[0].res = delta_calcInverse(&cpos, r[0].pos); + mpos.x = sum; + mpos.y = sum; + mpos.z = middlez + sum; + r[0].res = delta_calcInverse(&mpos, r[0].pos); - cpos.y = -sum; - r[1].res = delta_calcInverse(&cpos, r[1].pos); + mpos.y = -sum; + r[1].res = delta_calcInverse(&mpos, r[1].pos); - cpos.x = -sum; - r[2].res = delta_calcInverse(&cpos, r[2].pos); + mpos.x = -sum; + r[2].res = delta_calcInverse(&mpos, r[2].pos); - cpos.y = sum; - r[3].res = delta_calcInverse(&cpos, r[3].pos); + mpos.y = sum; + r[3].res = delta_calcInverse(&mpos, r[3].pos); - cpos.x = sum; - cpos.z = middlez - sum; - r[4].res = delta_calcInverse(&cpos, r[4].pos); + mpos.x = sum; + mpos.z = middlez - sum; + r[4].res = delta_calcInverse(&mpos, r[4].pos); - cpos.y = -sum; - r[5].res = delta_calcInverse(&cpos, r[5].pos); + mpos.y = -sum; + r[5].res = delta_calcInverse(&mpos, r[5].pos); - cpos.x = -sum; - r[6].res = delta_calcInverse(&cpos, r[6].pos); + mpos.x = -sum; + r[6].res = delta_calcInverse(&mpos, r[6].pos); - cpos.y = sum; - r[7].res = delta_calcInverse(&cpos, r[7].pos); + mpos.y = sum; + r[7].res = delta_calcInverse(&mpos, r[7].pos); if(r[0].res || r[1].res || r[2].res || r[3].res || r[4].res || r[5].res || r[6].res || r[7].res) { sum -= dist; dist *= 0.5f; - } else { - minz = middlez - sum; - for (uint8_t i = 0; i < 8; ++i) + } /* else { + uint32_t i; + for(i = 0; i < 8; ++i) { - if (mint1 > r[i].pos[0]) + if(mint1 > r[i].pos[0]) mint1 = r[i].pos[0]; - if (maxt1 < r[i].pos[0]) + if(maxt1 < r[i].pos[0]) maxt1 = r[i].pos[0]; - if (mint2 > r[i].pos[1]) + if(mint2 > r[i].pos[1]) mint2 = r[i].pos[1]; - if (maxt2 < r[i].pos[1]) + if(maxt2 < r[i].pos[1]) maxt2 = r[i].pos[1]; - if (mint3 > r[i].pos[2]) + if(mint3 > r[i].pos[2]) mint3 = r[i].pos[2]; - if (maxt3 < r[i].pos[2]) + if(maxt3 < r[i].pos[2]) maxt3 = r[i].pos[2]; } - } + } */ } while(original_dist > sum && dist > 0.1f); sys.work_envelope.min[X_AXIS] = -sum; @@ -354,33 +357,84 @@ static void get_cuboid_envelope (void) sys.work_envelope.max[X_AXIS] = sum; sys.work_envelope.max[Y_AXIS] = sum; sys.work_envelope.max[Z_AXIS] = middlez + sum; + + // resolution + pos[0] = pos[1] = pos[2] = 0.0f; + transform_to_cartesian(r[0].pos, pos); + pos[0] = sr; + transform_to_cartesian(r[1].pos, pos); + + float x = r[0].pos[0] - r[1].pos[0]; + float y = r[0].pos[1] - r[1].pos[1]; + resolution = sqrtf(x * x + y * y); // use as segment length (/ 2)? } -static uint_fast8_t wp_limits_get_axis_mask (uint_fast8_t idx) +static float *get_homing_target (float *target, float *position) { - return 0; + uint_fast8_t idx = Z_AXIS + 1; + + do { + idx--; + if((settings.homing.pulloff * HOMING_AXIS_LOCATE_SCALAR - fabsf(position[X_AXIS])) > - 0.1f) + target[idx] = position[X_AXIS] * (2.0f * M_PI) / settings.axis[X_AXIS].steps_per_mm; + else + target[idx] = bit_istrue(settings.homing.dir_mask.value, bit(idx)) ? -.5f : .5f; + } while(idx); + + return target; } -static void wp_limits_set_target_pos (uint_fast8_t idx) // fn name? +static uint_fast8_t delta_limits_get_axis_mask (uint_fast8_t idx) { -// int32_t axis_position; - - switch(idx) { - case X_AXIS: - break; - case Y_AXIS: - break; - default: - sys.position[idx] = 0; - break; - } + return bit(idx); } +static void delta_limits_set_target_pos (uint_fast8_t idx) +{ + sys.position[idx] = 0; +} + +static float homing_cycle_get_feedrate (float feedrate, axes_signals_t cycle) +{ + kinematics.transform_from_cartesian = get_homing_target; + + return feedrate; +} // Set machine positions for homed limit switches. Don't update non-homed axes. // NOTE: settings.max_travel[] is stored as a negative value. -static void wp_limits_set_machine_positions (axes_signals_t cycle) +static void delta_limits_set_machine_positions (axes_signals_t cycle) { + uint_fast8_t idx = N_AXIS; +// float pulloff = add_pulloff ? settings.homing.pulloff : 0.0f; + float pulloff = settings.homing.pulloff; + + if(settings.homing.flags.force_set_origin || true) { + do { + if(cycle.mask & bit(--idx)) { + sys.position[idx] = 0; + sys.home_position[idx] = 0.0f; + } + } while(idx); + } else do { + if(cycle.mask & bit(--idx)) { + sys.home_position[idx] = bit_istrue(settings.homing.dir_mask.value, bit(idx)) + ? settings.axis[idx].max_travel + pulloff + : - pulloff; + sys.position[idx] = lroundf(sys.home_position[idx] * settings.axis[idx].steps_per_mm); + } + } while(idx); +} + +static void delta_homing_complete (bool success) +{ + kinematics.transform_from_cartesian = transform_from_cartesian; + + if(success) + get_cuboid_envelope(); + + if(on_homing_completed) + on_homing_completed(success); } static void cancel_jog (sys_state_t state) @@ -394,10 +448,11 @@ static const setting_group_detail_t kinematics_groups [] = { static const setting_detail_t kinematics_settings[] = { { Setting_Kinematics0, Group_Kinematics, "Segment length", "mm", Format_Decimal, "0.00", NULL, NULL, Setting_NonCore, &delta_settings.sl, NULL, NULL }, - { Setting_Kinematics1, Group_Kinematics, "Forearm length", "mm", Format_Decimal, "##0.000", NULL, NULL, Setting_NonCore, &delta_settings.re, NULL, NULL }, + { Setting_Kinematics1, Group_Kinematics, "Forearm length", "mm", Format_Decimal, "###0.000", NULL, NULL, Setting_NonCore, &delta_settings.re, NULL, NULL }, { Setting_Kinematics2, Group_Kinematics, "Bicep length", "mm", Format_Decimal, "##0.000", NULL, NULL, Setting_NonCore, &delta_settings.rf, NULL, NULL }, { Setting_Kinematics3, Group_Kinematics, "Base radius", "mm", Format_Decimal, "##0.000", NULL, NULL, Setting_NonCore, &delta_settings.f, NULL, NULL }, - { Setting_Kinematics4, Group_Kinematics, "End effector radius", "mm", Format_Decimal, "##0.000", NULL, NULL, Setting_NonCore, &delta_settings.e, NULL, NULL } + { Setting_Kinematics4, Group_Kinematics, "End effector radius", "mm", Format_Decimal, "##0.000", NULL, NULL, Setting_NonCore, &delta_settings.e, NULL, NULL }, + { Setting_Kinematics5, Group_Kinematics, "Base to floor", "mm", Format_Decimal, "###0.000", NULL, NULL, Setting_NonCore, &delta_settings.b, NULL, NULL } }; #ifndef NO_SETTINGS_DESCRIPTIONS @@ -412,6 +467,17 @@ static const setting_descr_t kinematics_settings_descr[] = { #endif +static void delta_settings_changed (settings_t *settings, settings_changed_flags_t changed) +{ + float position[N_AXIS] = {0}, cartesian[N_AXIS]; + + memcpy(&machine, &delta_settings, sizeof(delta_settings_t)); + + z0 = transform_to_cartesian(cartesian, position)[Z_AXIS]; + + get_cuboid_envelope(); +} + static void delta_settings_save (void) { hal.nvs.memcpy_to_nvs(nvs_address, (uint8_t *)&delta_settings, sizeof(delta_settings_t), true); @@ -419,8 +485,9 @@ static void delta_settings_save (void) static void delta_settings_restore (void) { - delta_settings.e = 24.0f; // end effector - delta_settings.f = 75.0f; // base + delta_settings.b = 400.0f; + delta_settings.e = 24.0f; + delta_settings.f = 75.0f; delta_settings.re = 300.0f; delta_settings.rf = 100.0f; delta_settings.sl = .2f; @@ -430,16 +497,8 @@ static void delta_settings_restore (void) static void delta_settings_load (void) { - float position[N_AXIS] = {0}, cartesian[N_AXIS]; - if(hal.nvs.memcpy_from_nvs((uint8_t *)&delta_settings, nvs_address, sizeof(delta_settings_t), true) != NVS_TransferResult_OK) delta_settings_restore(); - - memcpy(&machine, &delta_settings, sizeof(delta_settings_t)); - - z0 = transform_to_cartesian(cartesian, position)[Z_AXIS]; - - get_cuboid_envelope(); } static setting_details_t setting_details = { @@ -448,12 +507,13 @@ static setting_details_t setting_details = { .settings = kinematics_settings, .n_settings = sizeof(kinematics_settings) / sizeof(setting_detail_t), #ifndef NO_SETTINGS_DESCRIPTIONS - .descriptions = kinematics_settings_descr, - .n_descriptions = sizeof(kinematics_settings_descr) / sizeof(setting_descr_t), +// .descriptions = kinematics_settings_descr, +// .n_descriptions = sizeof(kinematics_settings_descr) / sizeof(setting_descr_t), #endif .load = delta_settings_load, .restore = delta_settings_restore, - .save = delta_settings_save + .save = delta_settings_save, + .on_changed = delta_settings_changed }; static void report_options (bool newopt) @@ -461,7 +521,55 @@ static void report_options (bool newopt) on_report_options(newopt); if(!newopt) - hal.stream.write("[KINEMATICS:Delta v0.00]" ASCII_EOL); + hal.stream.write("[KINEMATICS:Delta v0.01]" ASCII_EOL); +} + +static status_code_t delta_info (sys_state_t state, char *args) +{ + hal.stream.write("Delta robot:" ASCII_EOL); + hal.stream.write(" Rectangular cuboid envelope:" ASCII_EOL); + hal.stream.write(" X: "); + hal.stream.write(ftoa(sys.work_envelope.min[X_AXIS], 3)); + hal.stream.write(" to "); + hal.stream.write(ftoa(sys.work_envelope.max[X_AXIS], 3)); + hal.stream.write(" mm" ASCII_EOL); + hal.stream.write(" Y: "); + hal.stream.write(ftoa(sys.work_envelope.min[Y_AXIS], 3)); + hal.stream.write(" to "); + hal.stream.write(ftoa(sys.work_envelope.max[Y_AXIS], 3)); + hal.stream.write(" mm" ASCII_EOL); + hal.stream.write(" Z: "); + hal.stream.write(ftoa(sys.work_envelope.min[Z_AXIS], 3)); + hal.stream.write(" to "); + hal.stream.write(ftoa(sys.work_envelope.max[Z_AXIS], 3)); + hal.stream.write(" mm" ASCII_EOL); + hal.stream.write(" Resolution:" ASCII_EOL " "); + hal.stream.write(ftoa(resolution, 3)); + hal.stream.write(" mm" ASCII_EOL); + + return Status_OK; +} + +static void delta_command_help (void) +{ + hal.stream.write("$DELTA - show info about delta robot parameters" ASCII_EOL); + + if(on_report_command_help) + on_report_command_help(); +} + +const sys_command_t delta_command_list[] = { + {"DELTA", delta_info, { .noargs = On } }, +}; + +static sys_commands_t delta_commands = { + .n_commands = sizeof(delta_command_list) / sizeof(sys_command_t), + .commands = delta_command_list +}; + +sys_commands_t *delta_get_commands() +{ + return &delta_commands; } // Initialize API pointers for Delta robot kinematics @@ -469,18 +577,28 @@ void delta_robot_init (void) { if((nvs_address = nvs_alloc(sizeof(delta_settings_t)))) { - kinematics.limits_set_target_pos = wp_limits_set_target_pos; - kinematics.limits_get_axis_mask = wp_limits_get_axis_mask; - kinematics.limits_set_machine_positions = wp_limits_set_machine_positions; + kinematics.limits_set_target_pos = delta_limits_set_target_pos; + kinematics.limits_get_axis_mask = delta_limits_get_axis_mask; + kinematics.limits_set_machine_positions = delta_limits_set_machine_positions; kinematics.transform_from_cartesian = transform_from_cartesian; - kinematics.transform_steps_to_cartesian = wp_convert_array_steps_to_mpos; + kinematics.transform_steps_to_cartesian = delta_convert_array_steps_to_mpos; kinematics.segment_line = delta_segment_line; + kinematics.homing_cycle_get_feedrate = homing_cycle_get_feedrate; grbl.on_jog_cancel = cancel_jog; on_report_options = grbl.on_report_options; grbl.on_report_options = report_options; + delta_commands.on_get_commands = grbl.on_get_commands; + grbl.on_get_commands = delta_get_commands; + + on_report_command_help = grbl.on_report_command_help; + grbl.on_report_command_help = delta_command_help; + + on_homing_completed = grbl.on_homing_completed; + grbl.on_homing_completed = delta_homing_complete; + settings_register(&setting_details); } } diff --git a/motion_control.c b/motion_control.c index 73159ca..33b39da 100644 --- a/motion_control.c +++ b/motion_control.c @@ -80,6 +80,7 @@ bool mc_line (float *target, plan_line_data_t *pl_data) #ifdef KINEMATICS_API float feed_rate = pl_data->feed_rate; + pl_data->rate_multiplier = 1.0; target = kinematics.segment_line(target, plan_get_position(), pl_data, true); #endif @@ -894,14 +895,22 @@ status_code_t mc_homing_cycle (axes_signals_t cycle) if(cycle.mask) { - if(!protocol_execute_realtime()) // Check for reset and set system abort. - return Status_Unhandled; // Did not complete. Alarm state set by mc_alarm. + if(!protocol_execute_realtime()) { // Check for reset and set system abort. + + if(grbl.on_homing_completed) + grbl.on_homing_completed(false); + + return Status_Unhandled; // Did not complete. Alarm state set by mc_alarm. + } if(homed_status != Status_OK) { if(state_get() == STATE_HOMING) state_set(STATE_IDLE); + if(grbl.on_homing_completed) + grbl.on_homing_completed(false); + return homed_status; } @@ -929,13 +938,11 @@ status_code_t mc_homing_cycle (axes_signals_t cycle) ? Status_LimitsEngaged : Status_OK; - if(homed_status == Status_OK) { - + if(homed_status == Status_OK) limits_set_work_envelope(); - if(grbl.on_homing_completed) - grbl.on_homing_completed(); - } + if(grbl.on_homing_completed) + grbl.on_homing_completed(homed_status == Status_OK); return homed_status; } diff --git a/nuts_bolts.h b/nuts_bolts.h index d208d37..fbb54bb 100644 --- a/nuts_bolts.h +++ b/nuts_bolts.h @@ -42,9 +42,15 @@ #define TOLERANCE_EQUAL 0.0001f -#define TAN_30 0.57735f // Used for threading calculations (60 degree inserts) -#define RADDEG 0.0174532925f // Radians per degree -#define DEGRAD 57.29577951f // Degrees per radians +#define RADDEG 0.01745329251994329577f // Radians per degree +#define DEGRAD 57.29577951308232087680f // Degrees per radians +#define SQRT3 1.73205080756887729353f +#define SIN120 0.86602540378443864676f +#define COS120 -0.5f +#define TAN60 1.73205080756887729353f +#define SIN30 0.5f +#define TAN30 0.57735026918962576451f +#define TAN30_2 0.28867513459481288225f #define ABORTED (sys.abort || sys.cancel) diff --git a/planner.c b/planner.c index 129ba24..ebc9792 100644 --- a/planner.c +++ b/planner.c @@ -524,6 +524,9 @@ bool plan_buffer_line (float *target, plan_line_data_t *pl_data) block->programmed_rate = block->rapid_rate; else { block->programmed_rate = pl_data->feed_rate; +#ifdef KINEMATICS_API + block->rate_multiplier = pl_data->rate_multiplier; +#endif if (block->condition.inverse_time) block->programmed_rate *= block->millimeters; } diff --git a/planner.h b/planner.h index 0891f19..522a92f 100644 --- a/planner.h +++ b/planner.h @@ -67,7 +67,9 @@ typedef struct plan_block { float max_junction_speed_sqr; // Junction entry speed limit based on direction vectors in (mm/min)^2 float rapid_rate; // Axis-limit adjusted maximum rate for this block direction in (mm/min) float programmed_rate; // Programmed rate of this block (mm/min). - +#ifdef KINEMATICS_API + float rate_multiplier; // Rate multiplier of this block. +#endif // Stored spindle speed data used by spindle overrides and resuming methods. spindle_t spindle; // Block spindle parameters. Copied from pl_line_data. @@ -80,6 +82,9 @@ typedef struct plan_block { // Planner data prototype. Must be used when passing new motions to the planner. typedef struct { float feed_rate; // Desired feed rate for line motion. Value is ignored, if rapid motion. +#ifdef KINEMATICS_API + float rate_multiplier; // Feed rate multiplier. +#endif // float blending_tolerance; // Motion blending tolerance spindle_t spindle; // Desired spindle parameters, such as RPM, through line motion. planner_cond_t condition; // Bitfield variable to indicate planner conditions. See defines above. diff --git a/settings.c b/settings.c index c9cd79f..e28f855 100644 --- a/settings.c +++ b/settings.c @@ -424,8 +424,11 @@ static char spindle_types[100] = ""; static char axis_dist[4] = "mm"; static char axis_rate[8] = "mm/min"; static char axis_accel[10] = "mm/sec^2"; +#if DELTA_ROBOT +static char axis_steps[9] = "step/rev"; +#else static char axis_steps[9] = "step/mm"; - +#endif #define AXIS_OPTS { .subgroups = On, .increment = 1 } PROGMEM static const setting_detail_t setting_detail[] = { @@ -721,7 +724,11 @@ PROGMEM static const setting_descr_t setting_descr[] = { { Setting_PositionIGain, "" }, { Setting_PositionDGain, "" }, { Setting_PositionIMaxError, "Spindle sync PID max integrator error." }, +#if DELTA_ROBOT + { Setting_AxisStepsPerMM, "Travel resolution in steps per revolution." }, +#else { Setting_AxisStepsPerMM, "Travel resolution in steps per millimeter." }, +#endif { (setting_id_t)(Setting_AxisStepsPerMM + 1), "Travel resolution in steps per degree." }, // "Hack" to get correct description for rotary axes { Setting_AxisMaxRate, "Maximum rate. Used as G0 rapid rate." }, { Setting_AxisAcceleration, "Acceleration. Used for motion planning to not exceed motor torque and lose steps." }, diff --git a/stepper.c b/stepper.c index 6f19ab8..2a553f9 100644 --- a/stepper.c +++ b/stepper.c @@ -125,6 +125,9 @@ typedef struct { float current_speed; // Current speed at the end of the segment buffer (mm/min) float maximum_speed; // Maximum speed of executing block. Not always nominal speed. (mm/min) float exit_speed; // Exit speed of executing block (mm/min) +#ifdef KINEMATICS_API + float rate_multiplier; // Rate multiplier of executing block. +#endif float accelerate_until; // Acceleration ramp end measured from end of block (mm) float decelerate_after; // Deceleration ramp start measured from end of block (mm) float target_position; // @@ -726,6 +729,7 @@ void st_prep_buffer (void) st_prep_block->direction_bits = pl_block->direction_bits; st_prep_block->programmed_rate = pl_block->programmed_rate; +// st_prep_block->r = pl_block->programmed_rate; st_prep_block->millimeters = pl_block->millimeters; st_prep_block->steps_per_mm = (float)pl_block->step_event_count / pl_block->millimeters; st_prep_block->output_commands = pl_block->output_commands; @@ -739,7 +743,9 @@ void st_prep_buffer (void) prep.steps_remaining = pl_block->step_event_count; prep.req_mm_increment = REQ_MM_INCREMENT_SCALAR / prep.steps_per_mm; prep.dt_remainder = prep.target_position = 0.0f; // Reset for new segment block - +#ifdef KINEMATICS_API + prep.rate_multiplier = pl_block->rate_multiplier; +#endif if (sys.step_control.execute_hold || prep.recalculate.decel_override) { // New block loaded mid-hold. Override planner block entry speed to enforce deceleration. prep.current_speed = prep.exit_speed; @@ -1116,5 +1122,11 @@ void st_prep_buffer (void) // divided by the ACCELERATION TICKS PER SECOND in seconds. float st_get_realtime_rate (void) { - return state_get() & (STATE_CYCLE|STATE_HOMING|STATE_HOLD|STATE_JOG|STATE_SAFETY_DOOR) ? prep.current_speed : 0.0f; + return state_get() & (STATE_CYCLE|STATE_HOMING|STATE_HOLD|STATE_JOG|STATE_SAFETY_DOOR) +#ifdef KINEMATICS_API + ? prep.current_speed * prep.rate_multiplier +#else + ? prep.current_speed +#endif + : 0.0f; }