mirror of
https://github.com/grblHAL/core.git
synced 2026-09-27 19:13:58 +08:00
Moved kinematics implementations to separate folder and added initial implementation of delta and polar kinematics.
NOTE: Delta and polar kinematics is WIP (work in progress) and incomplete. Feedback is required as I do not have machines at hand for testing. Ref. issue #341 and #346.
This commit is contained in:
Terje Io
@@ -0,0 +1,229 @@
|
||||
/*
|
||||
corexy.c - corexy kinematics implementation
|
||||
|
||||
Part of grblHAL
|
||||
|
||||
Copyright (c) 2019-2023 Terje Io
|
||||
Copyright (c) 2011-2016 Sungeun K. Jeon for Gnea Research LLC
|
||||
|
||||
Grbl is free software: you can redistribute it and/or modify
|
||||
it under the terms of the GNU General Public License as published by
|
||||
the Free Software Foundation, either version 3 of the License, or
|
||||
(at your option) any later version.
|
||||
|
||||
Grbl is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
GNU General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU General Public License
|
||||
along with Grbl. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include "../grbl.h"
|
||||
|
||||
#if COREXY
|
||||
|
||||
#include <math.h>
|
||||
|
||||
#include "../hal.h"
|
||||
#include "../settings.h"
|
||||
#include "../planner.h"
|
||||
#include "../kinematics.h"
|
||||
|
||||
// CoreXY motor assignments. DO NOT ALTER.
|
||||
// NOTE: If the A and B motor axis bindings are changed, this effects the CoreXY equations.
|
||||
#define A_MOTOR X_AXIS // Must be X_AXIS
|
||||
#define B_MOTOR Y_AXIS // Must be Y_AXIS
|
||||
|
||||
static on_report_options_ptr on_report_options;
|
||||
|
||||
// Returns x or y-axis "steps" based on CoreXY motor steps.
|
||||
inline static int32_t corexy_convert_to_a_motor_steps (int32_t *steps)
|
||||
{
|
||||
return (steps[A_MOTOR] + steps[B_MOTOR]) >> 1;
|
||||
}
|
||||
|
||||
inline static int32_t corexy_convert_to_b_motor_steps (int32_t *steps)
|
||||
{
|
||||
return (steps[A_MOTOR] - steps[B_MOTOR]) >> 1;
|
||||
}
|
||||
|
||||
// Returns machine position of axis 'idx'. Must be sent a 'step' array.
|
||||
static float *corexy_convert_array_steps_to_mpos (float *position, int32_t *steps)
|
||||
{
|
||||
uint_fast8_t idx;
|
||||
|
||||
position[X_AXIS] = corexy_convert_to_a_motor_steps(steps) / settings.axis[X_AXIS].steps_per_mm;
|
||||
position[Y_AXIS] = corexy_convert_to_b_motor_steps(steps) / settings.axis[Y_AXIS].steps_per_mm;
|
||||
|
||||
for(idx = Z_AXIS; idx < N_AXIS; idx++)
|
||||
position[idx] = steps[idx] / settings.axis[idx].steps_per_mm;
|
||||
|
||||
return position;
|
||||
}
|
||||
|
||||
// Transform position from cartesian coordinate system to corexy coordinate system
|
||||
static inline float *transform_from_cartesian (float *target, float *position)
|
||||
{
|
||||
uint_fast8_t idx;
|
||||
|
||||
target[X_AXIS] = position[X_AXIS] + position[Y_AXIS];
|
||||
target[Y_AXIS] = position[X_AXIS] - position[Y_AXIS];
|
||||
|
||||
for(idx = Z_AXIS; idx < N_AXIS; idx++)
|
||||
target[idx] = position[idx];
|
||||
|
||||
return target;
|
||||
}
|
||||
|
||||
static uint_fast8_t corexy_limits_get_axis_mask (uint_fast8_t idx)
|
||||
{
|
||||
return ((idx == A_MOTOR) || (idx == B_MOTOR)) ? (bit(X_AXIS) | bit(Y_AXIS)) : bit(idx);
|
||||
}
|
||||
|
||||
|
||||
static void corexy_limits_set_target_pos (uint_fast8_t idx) // fn name?
|
||||
{
|
||||
int32_t axis_position;
|
||||
|
||||
switch(idx) {
|
||||
case X_AXIS:
|
||||
axis_position = corexy_convert_to_b_motor_steps(sys.position);
|
||||
sys.position[A_MOTOR] = axis_position;
|
||||
sys.position[B_MOTOR] = -axis_position;
|
||||
break;
|
||||
case Y_AXIS:
|
||||
sys.position[A_MOTOR] = sys.position[B_MOTOR] = corexy_convert_to_a_motor_steps(sys.position);
|
||||
break;
|
||||
default:
|
||||
sys.position[idx] = 0;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// 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 corexy_limits_set_machine_positions (axes_signals_t cycle)
|
||||
{
|
||||
uint_fast8_t idx = N_AXIS;
|
||||
|
||||
if(settings.homing.flags.force_set_origin) {
|
||||
if (cycle.mask & bit(--idx)) do {
|
||||
switch(--idx) {
|
||||
case X_AXIS:
|
||||
sys.position[A_MOTOR] = corexy_convert_to_b_motor_steps(sys.position);
|
||||
sys.position[B_MOTOR] = - sys.position[A_MOTOR];
|
||||
break;
|
||||
case Y_AXIS:
|
||||
sys.position[A_MOTOR] = corexy_convert_to_a_motor_steps(sys.position);
|
||||
sys.position[B_MOTOR] = sys.position[A_MOTOR];
|
||||
break;
|
||||
default:
|
||||
sys.position[idx] = 0;
|
||||
break;
|
||||
}
|
||||
} while (idx);
|
||||
} else do {
|
||||
if (cycle.mask & bit(--idx)) {
|
||||
int32_t off_axis_position;
|
||||
int32_t set_axis_position = bit_istrue(settings.homing.dir_mask.value, bit(idx))
|
||||
? lroundf((settings.axis[idx].max_travel + settings.homing.pulloff) * settings.axis[idx].steps_per_mm)
|
||||
: lroundf(-settings.homing.pulloff * settings.axis[idx].steps_per_mm);
|
||||
switch(idx) {
|
||||
case X_AXIS:
|
||||
off_axis_position = corexy_convert_to_b_motor_steps(sys.position);
|
||||
sys.position[A_MOTOR] = set_axis_position + off_axis_position;
|
||||
sys.position[B_MOTOR] = set_axis_position - off_axis_position;
|
||||
break;
|
||||
case Y_AXIS:
|
||||
off_axis_position = corexy_convert_to_a_motor_steps(sys.position);
|
||||
sys.position[A_MOTOR] = off_axis_position + set_axis_position;
|
||||
sys.position[B_MOTOR] = off_axis_position - set_axis_position;
|
||||
break;
|
||||
default:
|
||||
sys.position[idx] = set_axis_position;
|
||||
break;
|
||||
}
|
||||
}
|
||||
} while(idx);
|
||||
}
|
||||
|
||||
static inline float get_distance (float *p0, float *p1)
|
||||
{
|
||||
uint_fast8_t idx = N_AXIS;
|
||||
float distance = 0.0f;
|
||||
|
||||
do {
|
||||
idx--;
|
||||
distance += (p0[idx] - p1[idx]) * (p0[idx] - p1[idx]);
|
||||
} while(idx);
|
||||
|
||||
return sqrtf(distance);
|
||||
}
|
||||
|
||||
// called from mc_line() to segment lines if not overridden, default implementation for pass-through
|
||||
static float *kinematics_segment_line (float *target, float *position, plan_line_data_t *pl_data, bool init)
|
||||
{
|
||||
static uint_fast8_t iterations;
|
||||
static float trsf[N_AXIS];
|
||||
|
||||
if(init) {
|
||||
|
||||
iterations = 2;
|
||||
|
||||
transform_from_cartesian(trsf, target);
|
||||
|
||||
if(!pl_data->condition.rapid_motion) {
|
||||
|
||||
uint_fast8_t idx;
|
||||
float cpos[N_AXIS];
|
||||
|
||||
cpos[X_AXIS] = (position[X_AXIS] + position[Y_AXIS]) * .5f;
|
||||
cpos[Y_AXIS] = (position[X_AXIS] - position[Y_AXIS]) * .5f;
|
||||
for(idx = Z_AXIS; idx < N_AXIS; idx++)
|
||||
cpos[idx] = position[idx];
|
||||
|
||||
pl_data->feed_rate *= get_distance(trsf, position) / get_distance(target, cpos);
|
||||
}
|
||||
}
|
||||
|
||||
return iterations-- == 0 ? NULL : trsf;
|
||||
}
|
||||
|
||||
static bool homing_cycle_validate (axes_signals_t cycle)
|
||||
{
|
||||
return (cycle.mask & (X_AXIS_BIT|Y_AXIS_BIT)) == 0 || cycle.mask < 3;
|
||||
}
|
||||
|
||||
static float homing_cycle_get_feedrate (float feedrate, axes_signals_t cycle)
|
||||
{
|
||||
return feedrate * sqrtf(2.0f);
|
||||
}
|
||||
|
||||
static void report_options (bool newopt)
|
||||
{
|
||||
on_report_options(newopt);
|
||||
|
||||
if(!newopt)
|
||||
hal.stream.write("[KINEMATICS:CoreXY v2.00]" ASCII_EOL);
|
||||
}
|
||||
|
||||
// Initialize API pointers for CoreXY kinematics
|
||||
void corexy_init (void)
|
||||
{
|
||||
kinematics.limits_set_target_pos = corexy_limits_set_target_pos;
|
||||
kinematics.limits_get_axis_mask = corexy_limits_get_axis_mask;
|
||||
kinematics.limits_set_machine_positions = corexy_limits_set_machine_positions;
|
||||
kinematics.transform_from_cartesian = transform_from_cartesian;
|
||||
kinematics.transform_steps_to_cartesian = corexy_convert_array_steps_to_mpos;
|
||||
kinematics.segment_line = kinematics_segment_line;
|
||||
kinematics.homing_cycle_validate = homing_cycle_validate;
|
||||
kinematics.homing_cycle_get_feedrate = homing_cycle_get_feedrate;
|
||||
|
||||
on_report_options = grbl.on_report_options;
|
||||
grbl.on_report_options = report_options;
|
||||
}
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,29 @@
|
||||
/*
|
||||
corexy.c - corexy kinematics implementation
|
||||
|
||||
Part of grblHAL
|
||||
|
||||
Copyright (c) 2019 Terje Io
|
||||
Copyright (c) 2011-2016 Sungeun K. Jeon for Gnea Research LLC
|
||||
|
||||
Grbl is free software: you can redistribute it and/or modify
|
||||
it under the terms of the GNU General Public License as published by
|
||||
the Free Software Foundation, either version 3 of the License, or
|
||||
(at your option) any later version.
|
||||
|
||||
Grbl is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
GNU General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU General Public License
|
||||
along with Grbl. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#ifndef _COREXY_H_
|
||||
#define _COREXY_H_
|
||||
|
||||
// Initialize HAL pointers for CoreXY kinematics
|
||||
void corexy_init (void);
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,488 @@
|
||||
/*
|
||||
delta.c - delta kinematics implementation
|
||||
|
||||
Part of grblHAL
|
||||
|
||||
Copyright (c) 2023 Terje Io
|
||||
Transforms derived from mzavatsky at Trossen Robotics
|
||||
https://hypertriangle.com/~alex/delta-robot-tutorial/
|
||||
|
||||
Grbl is free software: you can redistribute it and/or modify
|
||||
it under the terms of the GNU General Public License as published by
|
||||
the Free Software Foundation, either version 3 of the License, or
|
||||
(at your option) any later version.
|
||||
|
||||
Grbl is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
GNU General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU General Public License
|
||||
along with Grbl. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include "../grbl.h"
|
||||
|
||||
#if DELTA_ROBOT
|
||||
|
||||
#include <math.h>
|
||||
#include <string.h>
|
||||
|
||||
#include "../hal.h"
|
||||
#include "../settings.h"
|
||||
#include "../nvs_buffer.h"
|
||||
#include "../planner.h"
|
||||
#include "../kinematics.h"
|
||||
|
||||
typedef struct {
|
||||
float rf;
|
||||
float re;
|
||||
float f;
|
||||
float e;
|
||||
float sl;
|
||||
} 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 delta_settings_t machine = {0}, delta_settings;
|
||||
static on_report_options_ptr on_report_options;
|
||||
static nvs_address_t nvs_address;
|
||||
static float z0;
|
||||
|
||||
// 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
|
||||
// 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;
|
||||
// discriminant
|
||||
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);
|
||||
*theta = atanf(-zj / (y1 - yj)) + ((yj > y1) ? M_PI : 0.0f);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
// 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 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
|
||||
|
||||
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.
|
||||
static float *transform_to_cartesian (float *target, float *position)
|
||||
{
|
||||
float t = (machine.f - machine.e) * tan30 / 2.0f;
|
||||
/* float dtr = M_PI / 180.0f;
|
||||
|
||||
position[X_AXIS] *= dtr;
|
||||
position[Y_AXIS] *= dtr;
|
||||
position[Z_AXIS] *= dtr;
|
||||
*/
|
||||
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 z2 = -machine.rf * sinf(position[Y_AXIS]);
|
||||
|
||||
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;
|
||||
|
||||
float w1 = y1 * y1 + z1 * z1;
|
||||
float w2 = x2 * x2 + y2 * y2 + z2 * z2;
|
||||
float w3 = x3 * x3 + y3 * y3 + z3 * z3;
|
||||
|
||||
// x = (a1*z + b1)/dnm
|
||||
float a1 = (z2 - z1) * (y3 - y1) - (z3 - z1) * (y2 - y1);
|
||||
float b1 = -((w2 - w1) * (y3 - y1) - (w3 - w1) *(y2 - y1)) / 2.0f;
|
||||
|
||||
// y = (a2*z + b2)/dnm;
|
||||
float a2 = -(z2 - z1) * x3 + (z3 - z1) * x2;
|
||||
float b2 = ((w2 - w1) *x3 - (w3 - w1) * x2) / 2.0;
|
||||
|
||||
// a*z^2 + b*z + c = 0
|
||||
float a = a1 * a1 + a2 * a2 + dnm * dnm;
|
||||
float b = 2.0f * (a1 * b1 + a2 * (b2 - y1 * dnm) - z1 * dnm * dnm);
|
||||
float c = (b2 - y1 * dnm) * (b2 - y1 * dnm) + b1 * b1 + dnm * dnm * (z1 * z1 - machine.re * machine.re);
|
||||
|
||||
// discriminant
|
||||
float d = b * b - 4.0f * a * c;
|
||||
if (d < 0.0f)
|
||||
target[X_AXIS] = target[Y_AXIS] = target[Z_AXIS] = NAN; // non-existing point
|
||||
else {
|
||||
target[Z_AXIS] = -0.5f * (b + sqrtf(d)) / a;
|
||||
target[X_AXIS] = (a1 * target[Z_AXIS] + b1) / dnm;
|
||||
target[Y_AXIS] = (a2 * target[Z_AXIS] + b2) / dnm;
|
||||
}
|
||||
|
||||
return target;
|
||||
}
|
||||
|
||||
// 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)
|
||||
{
|
||||
float cpos[N_AXIS];
|
||||
|
||||
uint_fast8_t idx = N_AXIS;
|
||||
do {
|
||||
idx--;
|
||||
cpos[idx] = steps[idx] / settings.axis[idx].steps_per_mm;
|
||||
} while(idx);
|
||||
|
||||
return transform_to_cartesian(position, cpos);
|
||||
}
|
||||
|
||||
// Transform absolute position from cartesian coordinate system to wall plotter coordinate system
|
||||
static float *transform_from_cartesian (float *target, float *position)
|
||||
{
|
||||
delta_calcInverse((coord_data_t *)position, target);
|
||||
|
||||
return target;
|
||||
}
|
||||
|
||||
static inline float get_distance (float *p0, float *p1)
|
||||
{
|
||||
uint_fast8_t idx = Z_AXIS;
|
||||
float distance = 0.0f;
|
||||
|
||||
do {
|
||||
idx--;
|
||||
distance += (p0[idx] - p1[idx]) * (p0[idx] - p1[idx]);
|
||||
} while(idx);
|
||||
|
||||
return sqrtf(distance);
|
||||
}
|
||||
|
||||
// Wall plotter is circular in motion, so long lines must be 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;
|
||||
|
||||
uint_fast8_t idx = N_AXIS;
|
||||
|
||||
if(init) {
|
||||
|
||||
jog_cancel = false;
|
||||
memcpy(final_target.values, target, sizeof(final_target));
|
||||
|
||||
if(delta_calcInverse((coord_data_t *)target, cpos.values) == 0) {
|
||||
|
||||
transform_to_cartesian(segment_target.values, position);
|
||||
|
||||
delta.x = target[X_AXIS] - segment_target.x;
|
||||
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);
|
||||
|
||||
if((segmented = distance > machine.sl && !(delta.x == 0.0f && delta.y == 0.0f && delta.z == 0.0f))) {
|
||||
|
||||
idx = N_AXIS;
|
||||
iterations = (uint_fast16_t)ceilf(distance / machine.sl);
|
||||
|
||||
do {
|
||||
--idx;
|
||||
delta.values[idx] = delta.values[idx] / (float)iterations;
|
||||
} while(idx);
|
||||
|
||||
} else {
|
||||
iterations = 1;
|
||||
memcpy(&segment_target, &final_target, sizeof(coord_data_t));
|
||||
}
|
||||
} else { // out of bounds, report?
|
||||
iterations = 1;
|
||||
memcpy(&segment_target, target, sizeof(coord_data_t));
|
||||
}
|
||||
|
||||
iterations++; // return at least one iteration
|
||||
|
||||
} else {
|
||||
|
||||
iterations--;
|
||||
|
||||
if(segmented && iterations > 1) {
|
||||
do {
|
||||
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);
|
||||
}
|
||||
|
||||
return iterations == 0 || jog_cancel ? NULL : cpos.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 pos[N_AXIS];
|
||||
coord_data_t cpos;
|
||||
struct xxx {
|
||||
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) {
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
float btf = settings.axis[Z_AXIS].max_travel;
|
||||
if(minz < btf)
|
||||
minz = btf;
|
||||
if(maxz < btf)
|
||||
maxz = btf;
|
||||
|
||||
float middlez = (maxz + minz) * 0.5f;
|
||||
// $('#output').append("<p>("+maxz+","+minz+","+middlez+")</p>");
|
||||
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);
|
||||
|
||||
cpos.y = -sum;
|
||||
r[1].res = delta_calcInverse(&cpos, r[1].pos);
|
||||
|
||||
cpos.x = -sum;
|
||||
r[2].res = delta_calcInverse(&cpos, r[2].pos);
|
||||
|
||||
cpos.y = sum;
|
||||
r[3].res = delta_calcInverse(&cpos, r[3].pos);
|
||||
|
||||
cpos.x = sum;
|
||||
cpos.z = middlez - sum;
|
||||
r[4].res = delta_calcInverse(&cpos, r[4].pos);
|
||||
|
||||
cpos.y = -sum;
|
||||
r[5].res = delta_calcInverse(&cpos, r[5].pos);
|
||||
|
||||
cpos.x = -sum;
|
||||
r[6].res = delta_calcInverse(&cpos, r[6].pos);
|
||||
|
||||
cpos.y = sum;
|
||||
r[7].res = delta_calcInverse(&cpos, 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)
|
||||
{
|
||||
if (mint1 > r[i].pos[0])
|
||||
mint1 = r[i].pos[0];
|
||||
if (maxt1 < r[i].pos[0])
|
||||
maxt1 = r[i].pos[0];
|
||||
if (mint2 > r[i].pos[1])
|
||||
mint2 = r[i].pos[1];
|
||||
if (maxt2 < r[i].pos[1])
|
||||
maxt2 = r[i].pos[1];
|
||||
if (mint3 > r[i].pos[2])
|
||||
mint3 = 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;
|
||||
sys.work_envelope.min[Y_AXIS] = -sum;
|
||||
sys.work_envelope.min[Z_AXIS] = middlez - sum;
|
||||
sys.work_envelope.max[X_AXIS] = sum;
|
||||
sys.work_envelope.max[Y_AXIS] = sum;
|
||||
sys.work_envelope.max[Z_AXIS] = middlez + sum;
|
||||
}
|
||||
|
||||
static uint_fast8_t wp_limits_get_axis_mask (uint_fast8_t idx)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
|
||||
static void wp_limits_set_target_pos (uint_fast8_t idx) // fn name?
|
||||
{
|
||||
// int32_t axis_position;
|
||||
|
||||
switch(idx) {
|
||||
case X_AXIS:
|
||||
break;
|
||||
case Y_AXIS:
|
||||
break;
|
||||
default:
|
||||
sys.position[idx] = 0;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// 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 cancel_jog (sys_state_t state)
|
||||
{
|
||||
jog_cancel = true;
|
||||
}
|
||||
|
||||
static const setting_group_detail_t kinematics_groups [] = {
|
||||
{ Group_Root, Group_Kinematics, "Delta robot"}
|
||||
};
|
||||
|
||||
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_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 }
|
||||
};
|
||||
|
||||
#ifndef NO_SETTINGS_DESCRIPTIONS
|
||||
|
||||
static const setting_descr_t kinematics_settings_descr[] = {
|
||||
{ Setting_Kinematics0, "Step jogging speed in millimeters per minute." },
|
||||
{ Setting_Kinematics1, "Slow jogging speed in millimeters per minute." },
|
||||
{ Setting_Kinematics2, "Fast jogging speed in millimeters per minute." },
|
||||
{ Setting_Kinematics3, "Jog distance for single step jogging." },
|
||||
{ Setting_Kinematics4, "Jog distance before automatic stop." }
|
||||
};
|
||||
|
||||
#endif
|
||||
|
||||
static void delta_settings_save (void)
|
||||
{
|
||||
hal.nvs.memcpy_to_nvs(nvs_address, (uint8_t *)&delta_settings, sizeof(delta_settings_t), true);
|
||||
}
|
||||
|
||||
static void delta_settings_restore (void)
|
||||
{
|
||||
delta_settings.e = 24.0f; // end effector
|
||||
delta_settings.f = 75.0f; // base
|
||||
delta_settings.re = 300.0f;
|
||||
delta_settings.rf = 100.0f;
|
||||
delta_settings.sl = .2f;
|
||||
|
||||
hal.nvs.memcpy_to_nvs(nvs_address, (uint8_t *)&delta_settings, sizeof(delta_settings_t), true);
|
||||
}
|
||||
|
||||
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 = {
|
||||
.groups = kinematics_groups,
|
||||
.n_groups = sizeof(kinematics_groups) / sizeof(setting_group_detail_t),
|
||||
.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),
|
||||
#endif
|
||||
.load = delta_settings_load,
|
||||
.restore = delta_settings_restore,
|
||||
.save = delta_settings_save
|
||||
};
|
||||
|
||||
static void report_options (bool newopt)
|
||||
{
|
||||
on_report_options(newopt);
|
||||
|
||||
if(!newopt)
|
||||
hal.stream.write("[KINEMATICS:Delta v0.00]" ASCII_EOL);
|
||||
}
|
||||
|
||||
// Initialize API pointers for Delta robot kinematics
|
||||
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.transform_from_cartesian = transform_from_cartesian;
|
||||
kinematics.transform_steps_to_cartesian = wp_convert_array_steps_to_mpos;
|
||||
kinematics.segment_line = delta_segment_line;
|
||||
|
||||
grbl.on_jog_cancel = cancel_jog;
|
||||
|
||||
on_report_options = grbl.on_report_options;
|
||||
grbl.on_report_options = report_options;
|
||||
|
||||
settings_register(&setting_details);
|
||||
}
|
||||
}
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,28 @@
|
||||
/*
|
||||
delta.c - delta kinematics implementation
|
||||
|
||||
Part of grblHAL
|
||||
|
||||
Copyright (c) 2023 Terje Io
|
||||
|
||||
Grbl is free software: you can redistribute it and/or modify
|
||||
it under the terms of the GNU General Public License as published by
|
||||
the Free Software Foundation, either version 3 of the License, or
|
||||
(at your option) any later version.
|
||||
|
||||
Grbl is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
GNU General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU General Public License
|
||||
along with Grbl. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#ifndef _delta_H_
|
||||
#define _delta_H_
|
||||
|
||||
// Initialize HAL pointers for delta kinematics
|
||||
void delta_robot_init (void);
|
||||
|
||||
#endif
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,161 @@
|
||||
/*
|
||||
maslow.h - Maslow router kinematics implementation
|
||||
|
||||
Part of grblHAL
|
||||
|
||||
Grbl is free software: you can redistribute it and/or modify
|
||||
it under the terms of the GNU General Public License as published by
|
||||
the Free Software Foundation, either version 3 of the License, or
|
||||
(at your option) any later version.
|
||||
|
||||
Grbl is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
GNU General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU General Public License
|
||||
along with Grbl. If not, see <http://www.gnu.org/licenses/>.
|
||||
|
||||
The basis for this code has been pulled from MaslowDue created by Larry D O'Cull.
|
||||
<https://github.com/ldocull/MaslowDue>
|
||||
|
||||
Some portions of that package directly or indirectly has been pulled from from the Maslow CNC
|
||||
firmware for Aduino Mega. Those parts are Copyright 2014-2017 Bar Smith.
|
||||
<https://www.maslowcnc.com/>
|
||||
|
||||
It has been adapted for grbl by Terje Io.
|
||||
|
||||
*** TO BE COMPLETED ***
|
||||
|
||||
*/
|
||||
|
||||
#include "../grbl.h"
|
||||
|
||||
#ifndef _MASLOW_H_
|
||||
#define _MASLOW_H_
|
||||
|
||||
#define FP_SCALING 1024.0f
|
||||
#define SPROCKET_RADIUS_MM (10.1f)
|
||||
#define MAX_SEG_LENGTH_MM 2.0f /* long lines must be segmented due to circular motion */
|
||||
|
||||
// PID position loop factors X: Kp = 25000 Ki = 15000 Kd = 22000 Imax = 5000
|
||||
// 14.000 fixed point arithmetic S13.10
|
||||
#ifdef DRIVER_TLE5206
|
||||
#define MASLOW_A_KP 10.0f
|
||||
#define MASLOW_A_KI 21.0f
|
||||
#define MASLOW_A_KD 18.0f
|
||||
#define MASLOW_A_IMAX 5000
|
||||
|
||||
#define MASLOW_B_KP 10.0f
|
||||
#define MASLOW_B_KI 21.0f
|
||||
#define MASLOW_B_KD 18.0f
|
||||
#define MASLOW_B_IMAX 5000
|
||||
|
||||
#define MASLOW_Z_KP 10.0f
|
||||
#define MASLOW_Z_KI 21.0f
|
||||
#define MASLOW_Z_KD 17.0f
|
||||
#define MASLOW_Z_IMAX 5000
|
||||
#else
|
||||
#define MASLOW_A_KP 22.0f
|
||||
#define MASLOW_A_KI 17.0f
|
||||
#define MASLOW_A_KD 20.0f
|
||||
#define MASLOW_A_IMAX 5000
|
||||
|
||||
#define MASLOW_B_KP 22.0f
|
||||
#define MASLOW_B_KI 17.0f
|
||||
#define MASLOW_B_KD 20.0f
|
||||
#define MASLOW_B_IMAX 5000
|
||||
|
||||
#define MASLOW_Z_KP 20.0f
|
||||
#define MASLOW_Z_KI 17.0f
|
||||
#define MASLOW_Z_KD 18.0f
|
||||
#define MASLOW_Z_IMAX 5000
|
||||
#endif
|
||||
|
||||
#define MASLOW_MACHINEWIDTH 2400.0f
|
||||
#define MASLOW_MACHINEHEIGHT 1200.0f
|
||||
#define MASLOW_DISTBETWEENMOTORS 3000.0f
|
||||
#define MASLOW_MOTOROFFSETY 600.0f
|
||||
#define MASLOW_CHAINLENGTH 3000.0f
|
||||
#define MASLOW_CHAINOVERSPROCKET 0
|
||||
#define MASLOW_CHAINSAGCORRECTION 59.504839f
|
||||
#define MASLOW_LEFTCHAINTOLERANCE 0.0f
|
||||
#define MASLOW_RIGHTCHAINTOLERANCE 0.0f
|
||||
#define MASLOW_ROTATIONDISKRADIUS 104.3f
|
||||
#define MASLOW_SLEDHEIGHT 139.0f
|
||||
#define MASLOW_SLEDWIDTH 310.0f
|
||||
#define MASLOW_ACORRSCALING 1.003922f
|
||||
#define MASLOW_BCORRSCALING 1.002611f
|
||||
|
||||
typedef enum {
|
||||
Maslow_ChainOverSprocket = 260,
|
||||
Maslow_MachineWidth,
|
||||
Maslow_MachineHeight,
|
||||
Maslow_DistBetweenMotors,
|
||||
Maslow_MotorOffsetY,
|
||||
Maslow_AcorrScaling,
|
||||
Maslow_BcorrScaling,
|
||||
Maslow_SettingMax,
|
||||
} maslow_setting_t;
|
||||
|
||||
typedef enum {
|
||||
AxisSetting_MaslowKP = 10,
|
||||
AxisSetting_MaslowKI,
|
||||
AxisSetting_MaslowKD,
|
||||
AxisSetting_MaslowIMax,
|
||||
AxisSetting_MaslowMaxSetting
|
||||
} maslow_axis_setting_t;
|
||||
|
||||
typedef struct {
|
||||
float Kp;
|
||||
float Ki;
|
||||
float Kd;
|
||||
float Imax;
|
||||
} maslow_pid_coefficients_t;
|
||||
|
||||
typedef struct {
|
||||
maslow_pid_coefficients_t pid[N_AXIS];
|
||||
|
||||
uint32_t chainOverSprocket;
|
||||
float machineWidth; /* Maslow specific settings */
|
||||
float machineHeight;
|
||||
float distBetweenMotors;
|
||||
float motorOffsetY;
|
||||
float chainSagCorrection;
|
||||
float leftChainTolerance;
|
||||
float rightChainTolerance;
|
||||
float rotationDiskRadius;
|
||||
float chainLength;
|
||||
float sledHeight;
|
||||
float sledWidth;
|
||||
|
||||
float XcorrScaling;
|
||||
float YcorrScaling;
|
||||
} maslow_settings_t;
|
||||
|
||||
typedef struct {
|
||||
float Error;
|
||||
float Integral;
|
||||
float iterm;
|
||||
float DiffTerm;
|
||||
float speed;
|
||||
} maslow_debug_t;
|
||||
|
||||
typedef struct {
|
||||
maslow_settings_t settings;
|
||||
void (*pid_settings_changed)(uint_fast8_t idx);
|
||||
void (*move)(uint_fast8_t idx, int_fast16_t distance);
|
||||
void (*reset_pid)(uint_fast8_t idx);
|
||||
void (*pos_enable)(bool enable);
|
||||
void (*tuning_enable)(bool enable);
|
||||
int32_t (*set_step_size)(uint_fast8_t idx, int32_t step_size);
|
||||
maslow_debug_t *(*get_debug_data)(uint_fast8_t idx);
|
||||
} maslow_hal_t;
|
||||
|
||||
extern maslow_hal_t maslow_hal;
|
||||
|
||||
// Initialize HAL pointers for Maslow Router kinematics
|
||||
bool maslow_init (void);
|
||||
static status_code_t maslow_tuning (uint_fast16_t state, char *line);
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,251 @@
|
||||
/*
|
||||
polar.c - polar robot kinematics implementation
|
||||
|
||||
Part of grblHAL
|
||||
|
||||
Code lifted from Grbl_Esp32 from request by user @ https://github.com/Melkiyby
|
||||
|
||||
Note: homing is not implemented!
|
||||
|
||||
Grbl is free software: you can redistribute it and/or modify
|
||||
it under the terms of the GNU General Public License as published by
|
||||
the Free Software Foundation, either version 3 of the License, or
|
||||
(at your option) any later version.
|
||||
|
||||
Grbl is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
GNU General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU General Public License
|
||||
along with Grbl. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include "../grbl.h"
|
||||
|
||||
#if POLAR_ROBOT
|
||||
|
||||
#include <math.h>
|
||||
#include <string.h>
|
||||
|
||||
#include "../hal.h"
|
||||
#include "../settings.h"
|
||||
#include "../planner.h"
|
||||
#include "../kinematics.h"
|
||||
|
||||
#define RADIUS_AXIS X_AXIS
|
||||
#define POLAR_AXIS Y_AXIS
|
||||
#define MAX_SEG_LENGTH_MM 0.5f
|
||||
|
||||
static bool jog_cancel = false;
|
||||
static coord_data_t last_pos = {0};
|
||||
static on_report_options_ptr on_report_options;
|
||||
|
||||
// Simple hypotenuse computation function.
|
||||
inline static float hypot_f (float x, float y)
|
||||
{
|
||||
return sqrtf(x * x + y * y);
|
||||
}
|
||||
|
||||
// Return a 0-360 angle ... fix above 360 and below zero
|
||||
inline static float abs_angle (float ang)
|
||||
{
|
||||
ang = fmodf(ang, 360.0f); // 0-360 or 0 to -360
|
||||
|
||||
return ang < 0.0f ? 360.0f + ang : ang;
|
||||
}
|
||||
|
||||
// Returns machine position in mm converted from system position steps.
|
||||
static float *transform_to_cartesian (float *target, float *position)
|
||||
{
|
||||
target[X_AXIS] = cosf(position[POLAR_AXIS] * RADDEG) * position[RADIUS_AXIS];
|
||||
target[Y_AXIS] = sinf(position[POLAR_AXIS] * RADDEG) * position[RADIUS_AXIS];
|
||||
target[Z_AXIS] = position[Z_AXIS]; // unchanged
|
||||
|
||||
return target;
|
||||
}
|
||||
|
||||
// Returns machine position in mm converted from system position steps.
|
||||
static float *polar_convert_array_steps_to_mpos (float *position, int32_t *steps)
|
||||
{
|
||||
float cpos[N_AXIS];
|
||||
|
||||
uint_fast8_t idx = N_AXIS;
|
||||
do {
|
||||
idx--;
|
||||
cpos[idx] = steps[idx] / settings.axis[idx].steps_per_mm;
|
||||
} while(idx);
|
||||
|
||||
return transform_to_cartesian(position, cpos);
|
||||
|
||||
return position;
|
||||
}
|
||||
|
||||
// Transform absolute position from cartesian coordinate system to polar coordinate system
|
||||
static float *transform_from_cartesian (float *target, float *position)
|
||||
{
|
||||
float delta_ang; // the difference from the last and next angle
|
||||
uint_fast8_t idx = N_AXIS - 1;
|
||||
|
||||
do {
|
||||
target[idx] = position[idx];
|
||||
} while(--idx > Y_AXIS);
|
||||
|
||||
target[RADIUS_AXIS] = hypot_f(position[X_AXIS], position[Y_AXIS]);
|
||||
if (target[RADIUS_AXIS] == 0.0f) {
|
||||
target[POLAR_AXIS] = last_pos.values[POLAR_AXIS]; // don't care about angle at center
|
||||
} else {
|
||||
target[POLAR_AXIS] = atan2f(position[Y_AXIS], position[X_AXIS]) * DEGRAD;
|
||||
// no negative angles...we want the absolute angle not -90, use 270
|
||||
target[POLAR_AXIS] = abs_angle(target[POLAR_AXIS]);
|
||||
}
|
||||
|
||||
delta_ang = target[POLAR_AXIS] - abs_angle(last_pos.values[POLAR_AXIS]);
|
||||
// if the delta is above 180 degrees it means we are crossing the 0 degree line
|
||||
if (fabs(delta_ang) <= 180.0f)
|
||||
target[POLAR_AXIS] = last_pos.values[POLAR_AXIS] + delta_ang;
|
||||
else
|
||||
target[POLAR_AXIS] = last_pos.values[POLAR_AXIS] + (delta_ang > 0.0f ? - (360.0f - delta_ang) : delta_ang + 360.0f);
|
||||
|
||||
return target;
|
||||
}
|
||||
|
||||
static inline float get_distance (float *p0, float *p1)
|
||||
{
|
||||
uint_fast8_t idx = N_AXIS;
|
||||
float distance = 0.0f;
|
||||
|
||||
do {
|
||||
idx--;
|
||||
distance += (p0[idx] - p1[idx]) * (p0[idx] - p1[idx]);
|
||||
} while(idx);
|
||||
|
||||
return sqrtf(distance);
|
||||
}
|
||||
|
||||
// Polar is circular in motion, so long lines must be divided up
|
||||
static float *polar_segment_line (float *target, float *position, plan_line_data_t *pl_data, bool init)
|
||||
{
|
||||
static uint_fast16_t iterations;
|
||||
static bool segmented;
|
||||
static float r_offset, distance;
|
||||
static coord_data_t delta, segment_target, final_target, cpos;
|
||||
// static plan_line_data_t plan;
|
||||
|
||||
uint_fast8_t idx = N_AXIS;
|
||||
|
||||
if(init) {
|
||||
|
||||
jog_cancel = false;
|
||||
r_offset = gc_get_offset(RADIUS_AXIS) * 2.0f; //??
|
||||
|
||||
memcpy(final_target.values, target, sizeof(final_target));
|
||||
|
||||
transform_to_cartesian(segment_target.values, position);
|
||||
|
||||
delta.x = target[X_AXIS] - segment_target.x;
|
||||
delta.y = target[Y_AXIS] - segment_target.y;
|
||||
delta.z = target[Z_AXIS] - segment_target.z;
|
||||
|
||||
distance = sqrtf(delta.x * delta.x + delta.y * delta.y + delta.z * delta.z);
|
||||
|
||||
if((segmented = !pl_data->condition.rapid_motion && distance > MAX_SEG_LENGTH_MM && !(delta.x == 0.0f && delta.y == 0.0f))) {
|
||||
|
||||
idx = N_AXIS;
|
||||
iterations = (uint_fast16_t)ceilf(distance / MAX_SEG_LENGTH_MM);
|
||||
|
||||
do {
|
||||
--idx;
|
||||
delta.values[idx] = delta.values[idx] / (float)iterations;
|
||||
} while(idx);
|
||||
|
||||
} else {
|
||||
iterations = 1;
|
||||
memcpy(&segment_target, &final_target, sizeof(coord_data_t));
|
||||
}
|
||||
|
||||
iterations++; // return at least one iteration
|
||||
|
||||
} else {
|
||||
|
||||
iterations--;
|
||||
|
||||
if(segmented && iterations > 1) {
|
||||
do {
|
||||
idx--;
|
||||
segment_target.values[idx] += delta.values[idx];
|
||||
} while(idx);
|
||||
|
||||
} else
|
||||
memcpy(&segment_target, &final_target, sizeof(coord_data_t));
|
||||
|
||||
segment_target.values[RADIUS_AXIS] -= r_offset;
|
||||
transform_from_cartesian(cpos.values, segment_target.values);
|
||||
segment_target.values[RADIUS_AXIS] += r_offset;
|
||||
|
||||
if(!pl_data->condition.rapid_motion) {
|
||||
float fr_mul = distance / get_distance(last_pos.values, cpos.values);
|
||||
pl_data->feed_rate *= fr_mul == 0.0f ? 1.0 : (fr_mul < 0.5f ? 0.5f : fr_mul);
|
||||
}
|
||||
|
||||
memcpy(&last_pos, &cpos, sizeof(coord_data_t));
|
||||
}
|
||||
|
||||
return iterations == 0 || jog_cancel ? NULL : cpos.values;
|
||||
}
|
||||
|
||||
static uint_fast8_t polar_limits_get_axis_mask (uint_fast8_t idx)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
|
||||
static void polar_limits_set_target_pos (uint_fast8_t idx) // fn name?
|
||||
{
|
||||
switch(idx) {
|
||||
case X_AXIS:
|
||||
break;
|
||||
case Y_AXIS:
|
||||
break;
|
||||
default:
|
||||
sys.position[idx] = 0;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// 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 polar_limits_set_machine_positions (axes_signals_t cycle)
|
||||
{
|
||||
}
|
||||
|
||||
static void cancel_jog (sys_state_t state)
|
||||
{
|
||||
jog_cancel = true;
|
||||
}
|
||||
|
||||
static void report_options (bool newopt)
|
||||
{
|
||||
on_report_options(newopt);
|
||||
|
||||
if(!newopt)
|
||||
hal.stream.write("[KINEMATICS:Polar v0.00]" ASCII_EOL);
|
||||
}
|
||||
|
||||
// Initialize API pointers for Wall Plotter kinematics
|
||||
void polar_init (void)
|
||||
{
|
||||
kinematics.limits_set_target_pos = polar_limits_set_target_pos;
|
||||
kinematics.limits_get_axis_mask = polar_limits_get_axis_mask;
|
||||
kinematics.limits_set_machine_positions = polar_limits_set_machine_positions;
|
||||
kinematics.transform_from_cartesian = transform_from_cartesian;
|
||||
kinematics.transform_steps_to_cartesian = polar_convert_array_steps_to_mpos;
|
||||
kinematics.segment_line = polar_segment_line;
|
||||
|
||||
grbl.on_jog_cancel = cancel_jog;
|
||||
|
||||
on_report_options = grbl.on_report_options;
|
||||
grbl.on_report_options = report_options;
|
||||
}
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,28 @@
|
||||
/*
|
||||
corexy.c - polar kinematics implementation
|
||||
|
||||
Part of grblHAL
|
||||
|
||||
Copyright (c) 2023 Terje Io
|
||||
|
||||
Grbl is free software: you can redistribute it and/or modify
|
||||
it under the terms of the GNU General Public License as published by
|
||||
the Free Software Foundation, either version 3 of the License, or
|
||||
(at your option) any later version.
|
||||
|
||||
Grbl is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
GNU General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU General Public License
|
||||
along with Grbl. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#ifndef _POLAR_H_
|
||||
#define _POLAR_H_
|
||||
|
||||
// Initialize HAL pointers for Polar kinematics
|
||||
void polar_init (void);
|
||||
|
||||
#endif // _POLAR_H_
|
||||
@@ -0,0 +1,325 @@
|
||||
/*
|
||||
wall_plotter.c - wall plotter kinematics implementation
|
||||
|
||||
Part of grblHAL
|
||||
|
||||
Code lifted from Grbl_Esp32 pull request by user @ https://github.com/rognlien
|
||||
|
||||
Original code here: https://github.com/jasonwebb/grbl-mega-wall-plotter
|
||||
Note: homing is not implemented!
|
||||
|
||||
Bits also pulled from: https://github.com/ldocull/MaslowDue
|
||||
|
||||
Grbl is free software: you can redistribute it and/or modify
|
||||
it under the terms of the GNU General Public License as published by
|
||||
the Free Software Foundation, either version 3 of the License, or
|
||||
(at your option) any later version.
|
||||
|
||||
Grbl is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
GNU General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU General Public License
|
||||
along with Grbl. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include "../grbl.h"
|
||||
|
||||
#if WALL_PLOTTER
|
||||
|
||||
#include <math.h>
|
||||
#include <string.h>
|
||||
|
||||
#include "../hal.h"
|
||||
#include "../settings.h"
|
||||
#include "../planner.h"
|
||||
#include "../kinematics.h"
|
||||
|
||||
#define A_MOTOR X_AXIS // Must be X_AXIS
|
||||
#define B_MOTOR Y_AXIS // Must be Y_AXIS
|
||||
#define MAX_SEG_LENGTH_MM 2.0f
|
||||
|
||||
typedef struct {
|
||||
int32_t width;
|
||||
float width_mm;
|
||||
float width_pow;
|
||||
int32_t height;
|
||||
int32_t width_2;
|
||||
int32_t height_2;
|
||||
int32_t spindlezero[2];
|
||||
float spindlezero_mm[2];
|
||||
} machine_t;
|
||||
|
||||
typedef struct {
|
||||
float a;
|
||||
float b;
|
||||
} coord_t;
|
||||
|
||||
static bool jog_cancel = false;
|
||||
static machine_t machine = {0};
|
||||
static on_report_options_ptr on_report_options;
|
||||
|
||||
// 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)
|
||||
{
|
||||
coord_t len;
|
||||
|
||||
len.a = (float)steps[A_MOTOR] / settings.axis[A_MOTOR].steps_per_mm;
|
||||
len.b = (float)steps[B_MOTOR] / settings.axis[B_MOTOR].steps_per_mm;
|
||||
|
||||
position[X_AXIS] = (machine.width_pow + len.a * len.a - len.b * len.b) / (2.0f * machine.width_mm);
|
||||
len.a = machine.width_mm - position[X_AXIS];
|
||||
position[Y_AXIS] = sqrtf(len.b * len.b - len.a * len.a );
|
||||
position[Z_AXIS] = steps[Z_AXIS] / settings.axis[Z_AXIS].steps_per_mm;
|
||||
|
||||
return 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 *transform_to_cartesian (float *target, float *position)
|
||||
{
|
||||
coord_t len;
|
||||
|
||||
len.a = position[A_MOTOR];
|
||||
len.b = position[B_MOTOR];
|
||||
|
||||
target[X_AXIS] = (machine.width_pow + len.a * len.a - len.b * len.b) / (2.0f * machine.width_mm);
|
||||
len.a = machine.width_mm - target[X_AXIS];
|
||||
target[Y_AXIS] = sqrtf(len.b * len.b - len.a * len.a );
|
||||
target[Z_AXIS] = position[Z_AXIS];
|
||||
|
||||
return target;
|
||||
}
|
||||
|
||||
// Wall plotter calculation only. Returns x or y-axis "steps" based on wall plotter motor steps.
|
||||
// A length = sqrt( X^2 + Y^2 )
|
||||
// B length = sqrt( (MACHINE_WIDTH - X)^2 + Y^2 )
|
||||
inline static float wp_convert_to_a_motor_steps (float *target)
|
||||
{
|
||||
return sqrtf(target[A_MOTOR] * target[A_MOTOR] + target[B_MOTOR] * target[B_MOTOR]);
|
||||
}
|
||||
|
||||
inline static float wp_convert_to_b_motor_steps (float *target)
|
||||
{
|
||||
float xpos = machine.width_mm - target[A_MOTOR];
|
||||
|
||||
return sqrtf(xpos * xpos + target[B_MOTOR] * target[B_MOTOR]);
|
||||
}
|
||||
|
||||
// Transform absolute position from cartesian coordinate system to wall plotter coordinate system
|
||||
static float *transform_from_cartesian (float *target, float *position)
|
||||
{
|
||||
uint_fast8_t idx = N_AXIS - 1;
|
||||
|
||||
do {
|
||||
target[idx] = position[idx];
|
||||
} while(--idx > Y_AXIS);
|
||||
|
||||
target[A_MOTOR] = wp_convert_to_a_motor_steps(position);
|
||||
target[B_MOTOR] = wp_convert_to_b_motor_steps(position);
|
||||
|
||||
return target;
|
||||
}
|
||||
|
||||
|
||||
static inline float get_distance (float *p0, float *p1)
|
||||
{
|
||||
uint_fast8_t idx = Z_AXIS;
|
||||
float distance = 0.0f;
|
||||
|
||||
do {
|
||||
idx--;
|
||||
distance += (p0[idx] - p1[idx]) * (p0[idx] - p1[idx]);
|
||||
} while(idx);
|
||||
|
||||
return sqrtf(distance);
|
||||
}
|
||||
|
||||
// Wall plotter is circular in motion, so long lines must be divided up
|
||||
static float *wp_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;
|
||||
|
||||
uint_fast8_t idx = N_AXIS;
|
||||
|
||||
if(init) {
|
||||
|
||||
jog_cancel = false;
|
||||
|
||||
memcpy(final_target.values, target, sizeof(final_target));
|
||||
|
||||
transform_to_cartesian(segment_target.values, position);
|
||||
|
||||
delta.x = target[X_AXIS] - segment_target.x;
|
||||
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);
|
||||
|
||||
if((segmented = !pl_data->condition.rapid_motion && distance > MAX_SEG_LENGTH_MM && !(delta.x == 0.0f && delta.y == 0.0f))) {
|
||||
|
||||
idx = N_AXIS;
|
||||
iterations = (uint_fast16_t)ceilf(distance / MAX_SEG_LENGTH_MM);
|
||||
|
||||
do {
|
||||
--idx;
|
||||
delta.values[idx] = delta.values[idx] / (float)iterations;
|
||||
} while(idx);
|
||||
|
||||
} else {
|
||||
iterations = 1;
|
||||
memcpy(&segment_target, &final_target, sizeof(coord_data_t));
|
||||
}
|
||||
|
||||
iterations++; // return at least one iteration
|
||||
|
||||
} else {
|
||||
|
||||
iterations--;
|
||||
|
||||
if(segmented && iterations > 1) {
|
||||
do {
|
||||
idx--;
|
||||
segment_target.values[idx] += delta.values[idx];
|
||||
} while(idx);
|
||||
|
||||
} else
|
||||
memcpy(&segment_target, &final_target, sizeof(coord_data_t));
|
||||
|
||||
transform_from_cartesian(cpos.values, segment_target.values);
|
||||
}
|
||||
|
||||
return iterations == 0 || jog_cancel ? NULL : cpos.values;
|
||||
}
|
||||
|
||||
|
||||
static uint_fast8_t wp_limits_get_axis_mask (uint_fast8_t idx)
|
||||
{
|
||||
return ((idx == A_MOTOR) || (idx == B_MOTOR)) ? (bit(X_AXIS) | bit(Y_AXIS)) : bit(idx);
|
||||
}
|
||||
|
||||
|
||||
static void wp_limits_set_target_pos (uint_fast8_t idx) // fn name?
|
||||
{
|
||||
float xy[2];
|
||||
int32_t axis_position;
|
||||
|
||||
xy[X_AXIS] = sys.position[X_AXIS] / settings.axis[X_AXIS].steps_per_mm;
|
||||
xy[Y_AXIS] = sys.position[Y_AXIS] / settings.axis[Y_AXIS].steps_per_mm;
|
||||
|
||||
switch(idx) {
|
||||
case X_AXIS:
|
||||
axis_position = wp_convert_to_b_motor_steps(xy);
|
||||
sys.position[A_MOTOR] = axis_position;
|
||||
sys.position[B_MOTOR] = -axis_position;
|
||||
break;
|
||||
case Y_AXIS:
|
||||
sys.position[A_MOTOR] = sys.position[B_MOTOR] = wp_convert_to_a_motor_steps(xy);
|
||||
break;
|
||||
default:
|
||||
sys.position[idx] = 0;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// 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)
|
||||
{
|
||||
float xy[2];
|
||||
uint_fast8_t idx = N_AXIS;
|
||||
|
||||
xy[X_AXIS] = sys.position[X_AXIS] / settings.axis[X_AXIS].steps_per_mm;
|
||||
xy[Y_AXIS] = sys.position[Y_AXIS] / settings.axis[Y_AXIS].steps_per_mm;
|
||||
|
||||
if(settings.homing.flags.force_set_origin) {
|
||||
if (cycle.mask & bit(--idx)) do {
|
||||
switch(--idx) {
|
||||
case X_AXIS:
|
||||
sys.position[A_MOTOR] = wp_convert_to_b_motor_steps(xy);
|
||||
sys.position[B_MOTOR] = - sys.position[A_MOTOR];
|
||||
break;
|
||||
case Y_AXIS:
|
||||
sys.position[A_MOTOR] = wp_convert_to_a_motor_steps(xy);
|
||||
sys.position[B_MOTOR] = sys.position[A_MOTOR];
|
||||
break;
|
||||
default:
|
||||
sys.position[idx] = 0;
|
||||
break;
|
||||
}
|
||||
} while (idx);
|
||||
} else do {
|
||||
if (cycle.mask & bit(--idx)) {
|
||||
int32_t off_axis_position;
|
||||
int32_t set_axis_position = bit_istrue(settings.homing.dir_mask.value, bit(idx))
|
||||
? lroundf((settings.axis[idx].max_travel + settings.homing.pulloff) * settings.axis[idx].steps_per_mm)
|
||||
: lroundf(-settings.homing.pulloff * settings.axis[idx].steps_per_mm);
|
||||
switch(idx) {
|
||||
case X_AXIS:
|
||||
off_axis_position = wp_convert_to_b_motor_steps(xy);
|
||||
sys.position[A_MOTOR] = set_axis_position + off_axis_position;
|
||||
sys.position[B_MOTOR] = set_axis_position - off_axis_position;
|
||||
break;
|
||||
case Y_AXIS:
|
||||
off_axis_position = wp_convert_to_a_motor_steps(xy);
|
||||
sys.position[A_MOTOR] = off_axis_position + set_axis_position;
|
||||
sys.position[B_MOTOR] = off_axis_position - set_axis_position;
|
||||
break;
|
||||
default:
|
||||
sys.position[idx] = set_axis_position;
|
||||
break;
|
||||
}
|
||||
}
|
||||
} while(idx);
|
||||
}
|
||||
|
||||
static void cancel_jog (sys_state_t state)
|
||||
{
|
||||
jog_cancel = true;
|
||||
}
|
||||
|
||||
static void report_options (bool newopt)
|
||||
{
|
||||
on_report_options(newopt);
|
||||
|
||||
if(!newopt)
|
||||
hal.stream.write("[KINEMATICS:WallPlotter v2.00]" ASCII_EOL);
|
||||
}
|
||||
|
||||
// Initialize API pointers for Wall Plotter kinematics
|
||||
void wall_plotter_init (void)
|
||||
{
|
||||
machine.width_mm = -settings.axis[A_MOTOR].max_travel;
|
||||
machine.width = (int32_t)(machine.width_mm * settings.axis[A_MOTOR].steps_per_mm);
|
||||
machine.width_2 = machine.width >> 1;
|
||||
machine.width_pow = machine.width_mm * machine.width_mm;
|
||||
machine.height = (int32_t)((float)settings.axis[B_MOTOR].max_travel * settings.axis[B_MOTOR].steps_per_mm);
|
||||
machine.height_2 = machine.height >> 1;
|
||||
machine.spindlezero[A_MOTOR] = 0; // machine.width_2;
|
||||
machine.spindlezero[B_MOTOR] = 0; // machine.height_2;
|
||||
machine.spindlezero_mm[A_MOTOR] = (float)machine.spindlezero[A_MOTOR] / settings.axis[A_MOTOR].steps_per_mm;
|
||||
machine.spindlezero_mm[B_MOTOR] = (float)machine.spindlezero[B_MOTOR] / settings.axis[B_MOTOR].steps_per_mm;
|
||||
|
||||
sys.position[B_MOTOR] = machine.width;
|
||||
|
||||
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.transform_from_cartesian = transform_from_cartesian;
|
||||
kinematics.transform_steps_to_cartesian = wp_convert_array_steps_to_mpos;
|
||||
kinematics.segment_line = wp_segment_line;
|
||||
|
||||
grbl.on_jog_cancel = cancel_jog;
|
||||
|
||||
on_report_options = grbl.on_report_options;
|
||||
grbl.on_report_options = report_options;
|
||||
}
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,28 @@
|
||||
/*
|
||||
wall_plotter.h - wall plotter kinematics implementation
|
||||
|
||||
Part of grblHAL
|
||||
|
||||
Copyright (c) 2019 Terje Io
|
||||
|
||||
Grbl is free software: you can redistribute it and/or modify
|
||||
it under the terms of the GNU General Public License as published by
|
||||
the Free Software Foundation, either version 3 of the License, or
|
||||
(at your option) any later version.
|
||||
|
||||
Grbl is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
GNU General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU General Public License
|
||||
along with Grbl. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#ifndef _WALL_PLOTTER_H_
|
||||
#define _WALL_PLOTTER_H_
|
||||
|
||||
// Initialize HAL pointers for Wall Plotter kinematics
|
||||
void wall_plotter_init (void);
|
||||
|
||||
#endif
|
||||
Reference in New Issue
Block a user