Mirrors_Framework/grblHAL
1
0
Fork 0
mirror of https://github.com/grblHAL/core.git synced 2026-08-17 00:22:09 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

Fix for M3 enabled laser turned off on feed hold when $63 option Disable laser during hold is off. Ref. issue #991.

Browse Source 
Improved handling of some VFS file system actions (unlink, mkdirv and rmdir), added support for truncate.
Changed some kinematics API function signatures for readability/debugability.
The ioports implementation has been changed to allow for consecutive pin numbers for external (expander based) I/O. See changelog for details.
This commit is contained in:
Terje Io
2026-07-26 08:21:33 +02:00
parent 8913dd41e7
commit 35431d1bd3
20 changed files with 407 additions and 277 deletions
Download Patch File Download Diff File Expand all files Collapse all files
README.md
+1 -1
View File
@@ -1,6 +1,6 @@
## grblHAL ##
Latest build date is 20260718, see the [changelog](changelog.md) for details.
Latest build date is 20260726, see the [changelog](changelog.md) for details.
> [!NOTE]
> A settings reset will be performed on an update of builds prior to 20241208. Backup and restore of settings is recommended.
changelog.md
+33 -2
View File
@@ -1,15 +1,46 @@
## grblHAL changelog
<a name="20260726">Build 20260726
Core:
* Fix for M3 enabled laser turned off on feed hold when $63 option _Disable laser during hold_ is off. Ref. issue [#991](https://github.com/grblHAL/core/issues/991).
* Improved handling of some VFS file system actions \(`unlink`, `mkdirv` and `rmdir`\), added support for `truncate`.
* Changed some kinematics API function signatures for readability/debugability.
* For developers: the ioports implementation has been changed to allow for consecutive pin numbers for external \(expander based\) I/O
so that external pins can be assigned to a given expander for basic I/O functions.
Plugins should thus declare themself as external before registering and add a pin base, provided by the core, to their pin information output.
In addition they should allow multiple calls to their init function, with only the first acted upon,
to allow board code to sequence registration by implementing the weak function `board_ports_init()`.
Drivers:
* iMRXT1062, RP2040 and STM32F4xx: I/O expander code updated for core change.
Plugins:
* SDCard, YModem: no longer stores the file name to avoid potential buffer overflow. Files will be truncated to zero length on aborted transfers.
* SDCard, filing systems: added support for creating and deleting subdirectories via `$FMD` and `$FRD`.
Based on PR [#15](https://github.com/grblHAL/Plugin_SD_card/pull/15) which was not accepted due to the core VFS had to be changed to properly support the new commands.
* Misc: updated for core changes.
---
<a name="20260719">Build 20260719
Core:
* Updated VFS to correctly handle long working directory paths that would otherwise lead to buffer overflows. Ref. PR#986 which is a partial fix.
* Updated VFS to correctly handle long working directory paths that would otherwise lead to buffer overflows. Ref. PR [#986](https://github.com/grblHAL/core/pull/986) which is a partial fix.
> [!NOTE]
> The underlying file systems may impose their own limits to path lengths.
* Updated named O calls to allow name lengths only limited by available heap. Ref. PR#989 which likely would return an error on overly long names and would potentially execute incorrect code if not.
* Updated named O calls to allow name lengths only limited by available heap. Ref. PR [#989](https://github.com/grblHAL/core/pull/989) which likely would return an error on overly long names and would potentially execute incorrect code if not.
Plugins:
errors.h
+2 -1
View File
@@ -121,7 +121,8 @@ typedef enum {
Status_AuxiliaryPortUnusable = 86,
Status_ToolInSPindle = 87,
Status_NoToolInSPindle = 88,
Status_StatusMax = Status_NoToolInSPindle,
Status_FileDeleteFailed = 89,
Status_StatusMax = Status_FileDeleteFailed,
Status_UserException = 253,
Status_Handled, // For internal use only
Status_Unhandled // For internal use only
grbllib.c
+1 -1
View File
@@ -42,7 +42,7 @@
#include "motion_control.h"
#endif
#ifdef KINEMATICS_API
#include "kinematics.h"
#include "kinematics/interface.h"
#endif
static void task_execute (sys_state_t state);
ioports.c
+25 -5
View File
@@ -857,6 +857,19 @@ FLASHMEM static io_ports_list_t *insert_ports (void)
return io_ports;
}
FLASHMEM static uint8_t get_pin_base (io_port_type_t type, io_port_direction_t dir)
{
uint8_t pin_base = 0;
io_ports_list_t *io_ports;
if((io_ports = ports)) do {
if(io_ports->type == type && io_ports->ports_id && io_ports->ports_id->external)
pin_base += io_ports->ports_id->cfg[dir].n_ports;
} while((io_ports = io_ports->next));
return pin_base;
}
FLASHMEM static bool claim_hal (void)
{
io_port_t empty = {};
@@ -1062,13 +1075,17 @@ FLASHMEM bool ioports_add_analog (io_analog_t *analog)
if((ports = insert_ports())) {
ports->type = Port_Analog;
ports->ports_id = analog->ports;
ports->hal.set_pin_description = analog->set_pin_description;
ports->hal.get_pin_info = analog->get_pin_info;
if(analog->ports->out.n_ports)
if(analog->ports->out.n_ports) {
ports->hal.analog_out = analog->analog_out;
if(analog->ports->in.n_ports)
analog->ports->out.pin_base = get_pin_base(Port_Analog, Port_Output);
}
if(analog->ports->in.n_ports) {
ports->hal.wait_on_input = analog->wait_on_input;
analog->ports->out.pin_base = get_pin_base(Port_Analog, Port_Input);
}
ports->ports_id = analog->ports;
}
}
@@ -1091,15 +1108,18 @@ FLASHMEM bool ioports_add_digital (io_digital_t *digital)
if((io_ports = insert_ports())) {
io_ports->type = Port_Digital;
io_ports->ports_id = digital->ports;
io_ports->hal.set_pin_description = digital->set_pin_description;
io_ports->hal.get_pin_info = digital->get_pin_info;
if(digital->ports->out.n_ports)
if(digital->ports->out.n_ports) {
io_ports->hal.digital_out = digital->digital_out;
digital->ports->out.pin_base = get_pin_base(Port_Digital, Port_Output);
}
if(digital->ports->in.n_ports) {
io_ports->hal.wait_on_input = digital->wait_on_input;
io_ports->hal.register_interrupt_handler = digital->register_interrupt_handler;
digital->ports->in.pin_base = get_pin_base(Port_Digital, Port_Input);
}
io_ports->ports_id = digital->ports;
}
ioports_add_settings(NULL, NULL);
ioports.h
+2
View File
@@ -242,12 +242,14 @@ typedef struct {
uint8_t n_ports;
uint8_t n_start;
uint8_t idx_last;
uint8_t pin_base;
#ifdef IOPORTS_KEEP_DEPRECATED
uint8_t *map; //!< Deprecated - do not reference in new code!
#endif
} io_ports_detail_t;
typedef struct io_ports_data {
bool external;
union {
io_ports_detail_t cfg[2];
struct {
kinematics.h
-38
View File
@@ -1,38 +0,0 @@
/*
kinematics.h - kinematics interface (API)
Part of grblHAL
Copyright (c) 2019-2023 Terje Io
grblHAL 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.
grblHAL 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 grblHAL. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef _KINEMATICS_H_
#define _KINEMATICS_H_
typedef struct {
float *(*transform_steps_to_cartesian)(float *position, int32_t *steps);
float *(*transform_from_cartesian) (float *target, float *position);
float *(*segment_line) (float *target, float *position, plan_line_data_t *pl_data, bool init); // target is cartesian, position transformed
uint_fast8_t (*limits_get_axis_mask)(uint_fast8_t idx);
void (*limits_set_target_pos)(uint_fast8_t idx);
void (*limits_set_machine_positions)(axes_signals_t cycle);
bool (*homing_cycle_validate)(axes_signals_t cycle);
float (*homing_cycle_get_feedrate)(axes_signals_t axes, float rate, homing_mode_t mode);
} kinematics_t;
extern kinematics_t kinematics;
#endif
kinematics/asymmetric_ganging.c
+37 -11
View File
@@ -32,7 +32,7 @@
#include "../hal.h"
#include "../settings.h"
#include "../planner.h"
#include "../kinematics.h"
#include "interface.h"
#ifdef ASYMMETRIC_AUTO_SQUARE
#define PRIMARY_AXIS ASYMMETRIC_AUTO_SQUARE
@@ -155,23 +155,23 @@ static axes_signals_t onGetGangedAxes (bool auto_squared)
#endif // ASYMMETRIC_AUTO_SQUARE
static float *convert_array_steps_to_mpos (float *position, int32_t *steps)
static coord_data_t *convert_array_steps_to_mpos (coord_data_t *position, mpos_t *steps)
{
uint_fast8_t idx = N_AXIS;
do {
idx--;
position[idx] = steps[idx] / settings.axis[idx].steps_per_mm;
position->values[idx] = steps->values[idx] / settings.axis[idx].steps_per_mm;
} while(idx);
return position;
}
// Transform position from cartesian coordinate system to corexy coordinate system
static inline float *transform_from_cartesian (float *target, float *position)
static inline coord_data_t *transform_from_cartesian (coord_data_t *target, coord_data_t *position)
{
memcpy(target, position, sizeof(coord_data_t));
target[GANGED_AXIS] = position[PRIMARY_AXIS];
target->values[GANGED_AXIS] = position->values[PRIMARY_AXIS];
return target;
}
@@ -197,17 +197,17 @@ static void set_machine_positions (axes_signals_t cycle)
}
// 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 coord_data_t *kinematics_segment_line (coord_data_t *target, coord_data_t *position, plan_line_data_t *pl_data, bool init)
{
static uint_fast8_t iterations;
static coord_data_t trsf;
if(init) {
iterations = 2;
transform_from_cartesian(trsf.values, target);
transform_from_cartesian(&trsf, target);
}
return iterations-- == 0 ? NULL : trsf.values;
return iterations-- == 0 ? NULL : &trsf;
}
static float homing_cycle_get_feedrate (axes_signals_t cycle, float feedrate, homing_mode_t mode)
@@ -215,6 +215,12 @@ static float homing_cycle_get_feedrate (axes_signals_t cycle, float feedrate, ho
return feedrate;
}
static bool validate_steps_mm (float value)
{
// max diff 1%
return fabs(settings.axis[PRIMARY_AXIS].steps_per_mm - value) <= settings.axis[PRIMARY_AXIS].steps_per_mm / 100.0f;
}
static void onSettingsChanged (settings_t *settings, settings_changed_flags_t changed)
{
uint_fast8_t idx = sizeof(settings->homing.cycle) / sizeof(axes_signals_t);
@@ -225,7 +231,12 @@ static void onSettingsChanged (settings_t *settings, settings_changed_flags_t ch
memcpy(&settings->axis[GANGED_AXIS], &settings->axis[PRIMARY_AXIS], sizeof(axis_settings_t));
settings->axis[GANGED_AXIS].steps_per_mm = steps_per_mm;
if(!changed.restore_defaults) {
if(validate_steps_mm(steps_per_mm))
settings->axis[GANGED_AXIS].steps_per_mm = steps_per_mm;
else
report_message("Ganged axis step/mm is out of range, reset to default.", Message_Warning);
}
do {
if(settings->homing.cycle[--idx].bits & PRIMARY_AXIS_BIT)
@@ -286,8 +297,23 @@ PROGMEM static const char label[] = {
#endif
};
FLASHMEM static status_code_t set_steps_mm (setting_id_t setting, float value)
{
if(!validate_steps_mm(value))
return Status_SettingValueOutOfRange;
settings.axis[GANGED_AXIS].steps_per_mm = value;
return Status_OK;
}
FLASHMEM static float get_steps_mm (setting_id_t setting)
{
return settings.axis[GANGED_AXIS].steps_per_mm;
}
PROGMEM static const setting_detail_t axis_settings[] = {
{ Setting_AxisStepsPerMM + GANGED_AXIS, Group_Axis0 + PRIMARY_AXIS, label, "step/mm", Format_Decimal, "#####0.000##", NULL, NULL, Setting_IsLegacy, &settings.axis[GANGED_AXIS].steps_per_mm, NULL, NULL },
{ Setting_AxisStepsPerMM + GANGED_AXIS, Group_Axis0 + PRIMARY_AXIS, label, "step/mm", Format_Decimal, "#####0.000##", NULL, NULL, Setting_IsLegacyFn, set_steps_mm, get_steps_mm, NULL },
};
static void report_options (bool newopt)
@@ -295,7 +321,7 @@ static void report_options (bool newopt)
on_report_options(newopt);
if(!newopt)
hal.stream.write("[KINEMATICS:Asymmetric ganging v0.01]" ASCII_EOL);
hal.stream.write("[KINEMATICS:Asymmetric ganging v0.02]" ASCII_EOL);
}
// Initialize API pointers for xxx kinematics
kinematics/corexy.c
+38 -38
View File
@@ -3,7 +3,7 @@
Part of grblHAL
Copyright (c) 2019-2024 Terje Io
Copyright (c) 2019-2026 Terje Io
Copyright (c) 2011-2016 Sungeun K. Jeon for Gnea Research LLC
grblHAL is free software: you can redistribute it and/or modify
@@ -29,7 +29,7 @@
#include "../hal.h"
#include "../settings.h"
#include "../planner.h"
#include "../kinematics.h"
#include "interface.h"
// CoreXY motor assignments. DO NOT ALTER.
// NOTE: If the A and B motor axis bindings are changed, this effects the CoreXY equations.
@@ -40,54 +40,54 @@ static on_report_options_ptr on_report_options;
static travel_limits_ptr check_travel_limits;
// Returns x or y-axis "steps" based on CoreXY motor steps.
inline static int32_t corexy_convert_to_a_motor_steps (int32_t *steps)
inline static int32_t corexy_convert_to_a_motor_steps (mpos_t *steps)
{
return (steps[A_MOTOR] + steps[B_MOTOR]) >> 1;
return (steps->values[A_MOTOR] + steps->values[B_MOTOR]) >> 1;
}
inline static int32_t corexy_convert_to_b_motor_steps (int32_t *steps)
inline static int32_t corexy_convert_to_b_motor_steps (mpos_t *steps)
{
return (steps[A_MOTOR] - steps[B_MOTOR]) >> 1;
return (steps->values[A_MOTOR] - steps->values[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)
static coord_data_t *corexy_convert_array_steps_to_mpos (coord_data_t *position, mpos_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;
position->x = corexy_convert_to_a_motor_steps(steps) / settings.axis[X_AXIS].steps_per_mm;
position->y = 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;
position->values[idx] = steps->values[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)
static inline coord_data_t *transform_from_cartesian (coord_data_t *target, coord_data_t *position)
{
uint_fast8_t idx;
target[X_AXIS] = position[X_AXIS] + position[Y_AXIS];
target[Y_AXIS] = position[X_AXIS] - position[Y_AXIS];
target->x = position->x + position->y;
target->y = position->x - position->y;
for(idx = Z_AXIS; idx < N_AXIS; idx++)
target[idx] = position[idx];
target->values[idx] = position->values[idx];
return target;
}
// Transform position from motor (corexy) coordinate system to cartesian coordinate system
static inline float *transform_to_cartesian (float *target, float *position)
static inline coord_data_t *transform_to_cartesian (coord_data_t *target, coord_data_t *position)
{
uint_fast8_t idx;
target[X_AXIS] = (position[X_AXIS] + position[Y_AXIS]) * 0.5f;
target[Y_AXIS] = (position[X_AXIS] - position[Y_AXIS]) * 0.5f;
target->x = (position->x + position->y) * 0.5f;
target->y = (position->x - position->y) * 0.5f;
for(idx = Z_AXIS; idx < N_AXIS; idx++)
target[idx] = position[idx];
target->values[idx] = position->values[idx];
return target;
}
@@ -103,12 +103,12 @@ static void corexy_limits_set_target_pos (uint_fast8_t idx) // fn name?
switch(idx) {
case X_AXIS:
axis_position = corexy_convert_to_b_motor_steps(sys.position);
axis_position = corexy_convert_to_b_motor_steps((mpos_t *)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);
sys.position[A_MOTOR] = sys.position[B_MOTOR] = corexy_convert_to_a_motor_steps((mpos_t *)sys.position);
break;
default:
sys.position[idx] = 0;
@@ -123,11 +123,11 @@ static bool corexy_check_travel_limits (float *target, axes_signals_t axes, bool
if(is_cartesian)
return check_travel_limits(target, axes, true, envelope);
float cartesian_coords[N_AXIS];
coord_data_t cartesian_coords;
transform_to_cartesian(cartesian_coords, target);
transform_to_cartesian(&cartesian_coords, (coord_data_t *)target);
return check_travel_limits(cartesian_coords, axes, true, envelope);
return check_travel_limits(cartesian_coords.values, axes, true, envelope);
}
// Set machine positions for homed limit switches. Don't update non-homed axes.
@@ -140,11 +140,11 @@ static void corexy_limits_set_machine_positions (axes_signals_t cycle)
do {
if(cycle.mask & bit(--idx)) switch(idx) {
case X_AXIS:
sys.position[A_MOTOR] = corexy_convert_to_b_motor_steps(sys.position);
sys.position[A_MOTOR] = corexy_convert_to_b_motor_steps((mpos_t *)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[A_MOTOR] = corexy_convert_to_a_motor_steps((mpos_t *)sys.position);
sys.position[B_MOTOR] = sys.position[A_MOTOR];
break;
default:
@@ -163,12 +163,12 @@ static void corexy_limits_set_machine_positions (axes_signals_t cycle)
: lroundf(-pulloff->values[idx] * settings.axis[idx].steps_per_mm);
switch(idx) {
case X_AXIS:
off_axis_position = corexy_convert_to_b_motor_steps(sys.position);
off_axis_position = corexy_convert_to_b_motor_steps((mpos_t *)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);
off_axis_position = corexy_convert_to_a_motor_steps((mpos_t *)sys.position);
sys.position[A_MOTOR] = off_axis_position + set_axis_position;
sys.position[B_MOTOR] = off_axis_position - set_axis_position;
break;
@@ -180,46 +180,46 @@ static void corexy_limits_set_machine_positions (axes_signals_t cycle)
} while(idx);
}
static inline float get_distance (float *p0, float *p1)
static inline float get_distance (coord_data_t *p0, coord_data_t *p1)
{
uint_fast8_t idx = N_AXIS;
float distance = 0.0f;
do {
idx--;
distance += (p0[idx] - p1[idx]) * (p0[idx] - p1[idx]);
distance += (p0->values[idx] - p1->values[idx]) * (p0->values[idx] - p1->values[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 coord_data_t *kinematics_segment_line (coord_data_t *target, coord_data_t *position, plan_line_data_t *pl_data, bool init)
{
static uint_fast8_t iterations;
static float trsf[N_AXIS];
static coord_data_t trsf;
if(init) {
iterations = 2;
transform_from_cartesian(trsf, target);
transform_from_cartesian(&trsf, target);
if(!pl_data->condition.rapid_motion) {
uint_fast8_t idx;
float cpos[N_AXIS];
coord_data_t cpos;
cpos[X_AXIS] = (position[X_AXIS] + position[Y_AXIS]) * .5f;
cpos[Y_AXIS] = (position[X_AXIS] - position[Y_AXIS]) * .5f;
cpos.x = (position->x + position->y) * .5f;
cpos.y = (position->x - position->y) * .5f;
for(idx = Z_AXIS; idx < N_AXIS; idx++)
cpos[idx] = position[idx];
cpos.values[idx] = position->values[idx];
pl_data->feed_rate *= get_distance(trsf, position) / get_distance(target, cpos);
pl_data->feed_rate *= get_distance(&trsf, position) / get_distance(target, &cpos);
}
}
return iterations-- == 0 ? NULL : trsf;
return iterations-- == 0 ? NULL : &trsf;
}
static bool homing_cycle_validate (axes_signals_t cycle)
kinematics/delta.c
+77 -77
View File
@@ -3,7 +3,7 @@
Part of grblHAL
Copyright (c) 2023-2025 Terje Io
Copyright (c) 2023-2026 Terje Io
Transforms derived from mzavatsky at Trossen Robotics
https://hypertriangle.com/~alex/delta-robot-tutorial/
get_cuboid_envelope() derived from javascript code in
@@ -36,7 +36,7 @@
#include "../planner.h"
#include "../motion_control.h"
#include "../protocol.h"
#include "../kinematics.h"
#include "interface.h"
#define A_MOTOR X_AXIS
#define B_MOTOR Y_AXIS
@@ -125,28 +125,28 @@ static bool delta_calcAngleYZ (float x0, float y0, float z0, float *theta)
// inverse kinematics: cartesian position (pos) -> (target[A_MOTOR], target[B_MOTOR], target[C_MOTOR])
// returns false for non-existing position
static bool delta_calcInverse (coord_data_t *pos, float *target)
static bool delta_calcInverse (coord_data_t *pos, coord_data_t *target)
{
target[A_MOTOR] = target[B_MOTOR] = target[C_MOTOR] = 0.0f;
target->values[A_MOTOR] = target->values[B_MOTOR] = target->values[C_MOTOR] = 0.0f;
return delta_calcAngleYZ(pos->x, pos->y, pos->z, &target[A_MOTOR]) &&
delta_calcAngleYZ(pos->x * COS120 + pos->y * SIN120, pos->y * COS120 - pos->x * SIN120, pos->z, &target[B_MOTOR]) && // rotate coords to +120 deg
delta_calcAngleYZ(pos->x * COS120 - pos->y * SIN120, pos->y * COS120 + pos->x * SIN120, pos->z, &target[C_MOTOR]); // rotate coords to -120 deg
return delta_calcAngleYZ(pos->x, pos->y, pos->z, &target->values[A_MOTOR]) &&
delta_calcAngleYZ(pos->x * COS120 + pos->y * SIN120, pos->y * COS120 - pos->x * SIN120, pos->z, &target->values[B_MOTOR]) && // rotate coords to +120 deg
delta_calcAngleYZ(pos->x * COS120 - pos->y * SIN120, pos->y * COS120 + pos->x * SIN120, pos->z, &target->values[C_MOTOR]); // rotate coords to -120 deg
}
// Returns machine position in mm converted from system position.
static float *transform_to_cartesian (float *target, float *position)
static coord_data_t *transform_to_cartesian (coord_data_t *target, coord_data_t *position)
{
float y1 = -(machine.t + machine.cfg.rf * cosf(position[A_MOTOR]));
float z1 = -machine.cfg.rf * sinf(position[X_AXIS]);
float y1 = -(machine.t + machine.cfg.rf * cosf(position->values[A_MOTOR]));
float z1 = -machine.cfg.rf * sinf(position->x);
float y2 = (machine.t + machine.cfg.rf * cosf(position[B_MOTOR])) * SIN30;
float y2 = (machine.t + machine.cfg.rf * cosf(position->values[B_MOTOR])) * SIN30;
float x2 = y2 * TAN60;
float z2 = -machine.cfg.rf * sinf(position[Y_AXIS]);
float z2 = -machine.cfg.rf * sinf(position->y);
float y3 = (machine.t + machine.cfg.rf * cosf(position[C_MOTOR])) * SIN30;
float y3 = (machine.t + machine.cfg.rf * cosf(position->values[C_MOTOR])) * SIN30;
float x3 = -y3 * TAN60;
float z3 = -machine.cfg.rf * sinf(position[Z_AXIS]);
float z3 = -machine.cfg.rf * sinf(position->z);
float dnm = (y2 - y1) * x3 -(y3 - y1) * x2;
@@ -170,53 +170,53 @@ static float *transform_to_cartesian (float *target, float *position)
// 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
target->x = target->y = target->z = 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;
target->z = -0.5f * (b + sqrtf(d)) / a;
target->x = (a1 * target->z + b1) / dnm;
target->y = (a2 * target->z + b2) / dnm;
}
return target;
}
// Returns machine position in mm converted from system position steps.
static float *delta_convert_array_steps_to_mpos (float *position, int32_t *steps)
static coord_data_t *delta_convert_array_steps_to_mpos (coord_data_t *position, mpos_t *steps)
{
float mpos[N_AXIS];
coord_data_t mpos;
uint_fast8_t idx = N_AXIS;
do {
idx--;
mpos[idx] = steps[idx] / settings.axis[idx].steps_per_mm;
mpos.values[idx] = steps->values[idx] / settings.axis[idx].steps_per_mm;
} while(idx);
return transform_to_cartesian(position, mpos);
return transform_to_cartesian(position, &mpos);
}
// Transform absolute position from cartesian coordinate system to delta robot coordinate system
static float *transform_from_cartesian (float *target, float *position)
static coord_data_t *transform_from_cartesian (coord_data_t *target, coord_data_t *position)
{
delta_calcInverse((coord_data_t *)position, target);
delta_calcInverse(position, target);
return target;
}
static inline float get_distance (float *p0, float *p1)
static inline float get_distance (coord_data_t *p0, coord_data_t *p1)
{
uint_fast8_t idx = Z_AXIS + 1;
float distance = 0.0f;
do {
idx--;
distance += (p0[idx] - p1[idx]) * (p0[idx] - p1[idx]);
distance += (p0->values[idx] - p1->values[idx]) * (p0->values[idx] - p1->values[idx]);
} while(idx);
return sqrtf(distance);
}
// 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 coord_data_t *delta_segment_line (coord_data_t *target, coord_data_t *position, plan_line_data_t *pl_data, bool init)
{
static uint_fast16_t iterations;
static bool segmented;
@@ -228,20 +228,20 @@ static float *delta_segment_line (float *target, float *position, plan_line_data
if(init) {
jog_cancel = false;
memcpy(final_target.values, target, sizeof(final_target));
memcpy(&final_target, target, sizeof(final_target));
if(delta_calcInverse((coord_data_t *)target, mpos.values)) {
if(delta_calcInverse(target, &mpos)) {
if(!pl_data->condition.target_validated) {
pl_data->condition.target_validated = On;
pl_data->condition.target_valid = grbl.check_travel_limits(mpos.values, sys.soft_limits, false, &sys.work_envelope);
}
transform_to_cartesian(segment_target.values, position);
transform_to_cartesian(&segment_target, 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;
delta.x = target->x - segment_target.x;
delta.y = target->y - segment_target.y;
delta.z = target->z - segment_target.z;
distance = sqrtf(delta.x * delta.x + delta.y * delta.y + delta.z * delta.z);
@@ -283,11 +283,11 @@ static float *delta_segment_line (float *target, float *position, plan_line_data
} else
memcpy(&segment_target, &final_target, sizeof(coord_data_t));
if(!delta_calcInverse(&segment_target, mpos.values)) {
if(!delta_calcInverse(&segment_target, &mpos)) {
memcpy(&mpos, &machine.last_pos, sizeof(coord_data_t));
iterations = 0;
} else if(!pl_data->condition.rapid_motion && distance != 0.0f) {
float rate_multiplier = get_distance(mpos.values, machine.last_pos.values) / distance;
float rate_multiplier = get_distance(&mpos, &machine.last_pos) / distance;
pl_data->feed_rate *= rate_multiplier;
pl_data->rate_multiplier = 1.0f / rate_multiplier;
}
@@ -295,7 +295,7 @@ static float *delta_segment_line (float *target, float *position, plan_line_data
memcpy(&machine.last_pos, &mpos, sizeof(coord_data_t));
}
return iterations == 0 || jog_cancel ? NULL : mpos.values;
return iterations == 0 || jog_cancel ? NULL : &mpos;
}
static void get_cuboid_envelope (void)
@@ -303,7 +303,7 @@ static void get_cuboid_envelope (void)
float maxz = -machine.cfg.e - machine.cfg.f - machine.cfg.re - machine.cfg.rf;
float minz = -maxz;
float sr = 1.0f / 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 pos;
uint32_t idx, z;
coord_data_t mpos, home = {
.x = machine.cfg.home_angle,
@@ -311,16 +311,16 @@ static void get_cuboid_envelope (void)
.z = machine.cfg.home_angle
};
struct {
float pos[N_AXIS];
coord_data_t pos;
} r[8];
transform_to_cartesian(mpos.values, home.values);
transform_to_cartesian(&mpos, &home);
machine.home_z = mpos.z;
// find extents
for(z = 0; z < settings.axis[X_AXIS].steps_per_mm * 2.0f * M_PI ; ++z) {
pos[0] = pos[1] = pos[2] = sr * (float)z;
transform_to_cartesian(mpos.values, pos);
pos.values[0] = pos.values[1] = pos.values[2] = sr * (float)z;
transform_to_cartesian(&mpos, &pos);
if(!isnan(mpos.x)) {
if(minz > mpos.z)
minz = mpos.z;
@@ -332,7 +332,7 @@ static void get_cuboid_envelope (void)
maxz = machine.home_z;
if(machine.cfg.max_angle != 0.0f) {
home.x = home.y = home.z = machine.cfg.max_angle;
transform_to_cartesian(mpos.values, home.values);
transform_to_cartesian(&mpos, &home);
minz = mpos.z;
}
@@ -356,58 +356,58 @@ static void get_cuboid_envelope (void)
mpos.x = sum;
mpos.y = sum;
mpos.z = middlez + sum;
if((ok = delta_calcInverse(&mpos, r[0].pos))) {
if((ok = delta_calcInverse(&mpos, &r[0].pos))) {
mpos.y = -sum;
ok = delta_calcInverse(&mpos, r[1].pos);
ok = delta_calcInverse(&mpos, &r[1].pos);
}
if(ok) {
mpos.x = -sum;
ok = delta_calcInverse(&mpos, r[2].pos);
ok = delta_calcInverse(&mpos, &r[2].pos);
}
if(ok) {
mpos.y = sum;
ok = delta_calcInverse(&mpos, r[3].pos);
ok = delta_calcInverse(&mpos, &r[3].pos);
}
if(ok) {
mpos.x = sum;
mpos.z = middlez - sum;
ok = delta_calcInverse(&mpos, r[4].pos);
ok = delta_calcInverse(&mpos, &r[4].pos);
}
if(ok) {
mpos.y = -sum;
ok = delta_calcInverse(&mpos, r[5].pos);
ok = delta_calcInverse(&mpos, &r[5].pos);
}
if(ok) {
mpos.x = -sum;
ok = delta_calcInverse(&mpos, r[6].pos);
ok = delta_calcInverse(&mpos, &r[6].pos);
}
if(ok) {
mpos.y = sum;
ok = delta_calcInverse(&mpos, r[7].pos);
ok = delta_calcInverse(&mpos, &r[7].pos);
}
if(!ok) {
sum -= dist;
dist *= 0.5f;
} else for(idx = 0; idx < 8; ++idx) {
if(machine.min_angle[A_MOTOR] > r[idx].pos[A_MOTOR])
machine.min_angle[A_MOTOR] = r[idx].pos[A_MOTOR];
if(machine.max_angle[A_MOTOR] < r[idx].pos[A_MOTOR])
machine.max_angle[A_MOTOR] = r[idx].pos[A_MOTOR];
if(machine.min_angle[B_MOTOR] > r[idx].pos[B_MOTOR])
machine.min_angle[B_MOTOR] = r[idx].pos[B_MOTOR];
if(machine.max_angle[B_MOTOR] < r[idx].pos[B_MOTOR])
machine.max_angle[B_MOTOR] = r[idx].pos[B_MOTOR];
if(machine.min_angle[C_MOTOR] > r[idx].pos[C_MOTOR])
machine.min_angle[C_MOTOR] = r[idx].pos[C_MOTOR];
if(machine.max_angle[C_MOTOR] < r[idx].pos[C_MOTOR])
machine.max_angle[C_MOTOR] = r[idx].pos[C_MOTOR];
if(machine.min_angle[A_MOTOR] > r[idx].pos.values[A_MOTOR])
machine.min_angle[A_MOTOR] = r[idx].pos.values[A_MOTOR];
if(machine.max_angle[A_MOTOR] < r[idx].pos.values[A_MOTOR])
machine.max_angle[A_MOTOR] = r[idx].pos.values[A_MOTOR];
if(machine.min_angle[B_MOTOR] > r[idx].pos.values[B_MOTOR])
machine.min_angle[B_MOTOR] = r[idx].pos.values[B_MOTOR];
if(machine.max_angle[B_MOTOR] < r[idx].pos.values[B_MOTOR])
machine.max_angle[B_MOTOR] = r[idx].pos.values[B_MOTOR];
if(machine.min_angle[C_MOTOR] > r[idx].pos.values[C_MOTOR])
machine.min_angle[C_MOTOR] = r[idx].pos.values[C_MOTOR];
if(machine.max_angle[C_MOTOR] < r[idx].pos.values[C_MOTOR])
machine.max_angle[C_MOTOR] = r[idx].pos.values[C_MOTOR];
}
} while(original_dist > sum && dist > 0.1f);
@@ -429,8 +429,8 @@ static void get_cuboid_envelope (void)
home.x = home.y = 0;
home.z = sys.work_envelope.max.z;
if(delta_calcInverse(&home, pos))
machine.home_angle_cuboid = pos[A_MOTOR];
if(delta_calcInverse(&home, &pos))
machine.home_angle_cuboid = pos.values[A_MOTOR];
machine.min_angle[A_MOTOR] = machine.min_angle[B_MOTOR] = machine.min_angle[C_MOTOR] =
delta_settings.flags.home_to_cuboid_top ? machine.home_angle_cuboid : machine.cfg.home_angle;
@@ -438,28 +438,28 @@ static void get_cuboid_envelope (void)
machine.max_angle[A_MOTOR] = machine.max_angle[B_MOTOR] = machine.max_angle[C_MOTOR] = machine.cfg.max_angle;
// resolution
pos[A_MOTOR] = pos[B_MOTOR] = pos[C_MOTOR] = 0.0f;
transform_to_cartesian(r[0].pos, pos);
pos[A_MOTOR] = sr;
transform_to_cartesian(r[1].pos, pos);
pos.values[A_MOTOR] = pos.values[B_MOTOR] = pos.values[C_MOTOR] = 0.0f;
transform_to_cartesian(&r[0].pos, &pos);
pos.values[A_MOTOR] = sr;
transform_to_cartesian(&r[1].pos, &pos);
float x = r[0].pos[A_MOTOR] - r[1].pos[A_MOTOR];
float y = r[0].pos[B_MOTOR] - r[1].pos[B_MOTOR];
float x = r[0].pos.values[A_MOTOR] - r[1].pos.values[A_MOTOR];
float y = r[0].pos.values[B_MOTOR] - r[1].pos.values[B_MOTOR];
machine.resolution = sqrtf(x * x + y * y); // use as segment length (/ 2)?
}
static float *get_homing_target (float *target, float *position)
static coord_data_t *get_homing_target (coord_data_t *target, coord_data_t *position)
{
uint_fast8_t idx = Z_AXIS + 1;
do {
idx--;
if(homing_mode == HomingMode_Pulloff)
target[idx] = machine.home_pulloff = position[X_AXIS] / machine.resolution / settings.axis[X_AXIS].steps_per_mm;
target->values[idx] = machine.home_pulloff = position->x / machine.resolution / settings.axis[X_AXIS].steps_per_mm;
else if(homing_mode == HomingMode_Locate)
target[idx] = position[X_AXIS] / machine.resolution / settings.axis[X_AXIS].steps_per_mm;
target->values[idx] = position->x / machine.resolution / settings.axis[X_AXIS].steps_per_mm;
else
target[idx] = bit_istrue(settings.homing.dir_mask.value, bit(idx)) ? -M_PI : M_PI; // 0.5 revolution
target->values[idx] = bit_istrue(settings.homing.dir_mask.value, bit(idx)) ? -M_PI : M_PI; // 0.5 revolution
} while(idx);
return target;
@@ -588,7 +588,7 @@ static bool delta_check_travel_limits (float *target, axes_signals_t axes, bool
#endif
if(!is_cartesian) {
if(isnan(transform_to_cartesian(pos.values, target)[A_MOTOR]))
if(isnan(transform_to_cartesian(&pos, (coord_data_t *)target)->values[A_MOTOR]))
return false;
}
@@ -600,7 +600,7 @@ static bool delta_check_travel_limits (float *target, axes_signals_t axes, bool
if(!is_cartesian)
memcpy(&pos.values, target, sizeof(coord_data_t));
else if(delta_calcInverse((coord_data_t *)target, pos.values) || !pos_ok(&pos))
else if(delta_calcInverse((coord_data_t *)target, &pos) || !pos_ok(&pos))
return false;
if(sys.homed.mask) do {
@@ -658,7 +658,7 @@ static void delta_apply_travel_limits (float *target, float *position, work_enve
target[Y_AXIS] = position[Y_AXIS] + delta.y * length;
target[Z_AXIS] = position[Z_AXIS] + delta.z * length;
ok = delta_calcInverse((coord_data_t *)target, pos.values) && pos_ok(&pos);
ok = delta_calcInverse((coord_data_t *)target, &pos) && pos_ok(&pos);
if(dist > machine.resolution)
dist *= 0.5f;
kinematics/interface.h
+49
View File
@@ -0,0 +1,49 @@
/*
kinematics.h - kinematics interface (API)
Part of grblHAL
Copyright (c) 2019-2026 Terje Io
grblHAL 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.
grblHAL 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 grblHAL. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef _KINEMATICS_H_
#define _KINEMATICS_H_
#include "../nuts_bolts.h"
typedef coord_data_t *(*transform_steps_to_cartesian_ptr)(coord_data_t *position, mpos_t *steps);
typedef coord_data_t *(*transform_from_cartesian_ptr) (coord_data_t *target, coord_data_t *position);
typedef coord_data_t *(*segment_line_ptr) (coord_data_t *target, coord_data_t *position, plan_line_data_t *pl_data, bool init); // target is cartesian, position transformed
typedef uint_fast8_t (*limits_get_axis_mask_ptr)(uint_fast8_t idx);
typedef void (*limits_set_target_pos_ptr)(uint_fast8_t idx);
typedef void (*limits_set_machine_positions_ptr)(axes_signals_t cycle);
typedef bool (*homing_cycle_validate_ptr)(axes_signals_t cycle);
typedef float (*homing_cycle_get_feedrate_ptr)(axes_signals_t axes, float rate, homing_mode_t mode);
typedef struct {
transform_steps_to_cartesian_ptr transform_steps_to_cartesian;
transform_from_cartesian_ptr transform_from_cartesian;
segment_line_ptr segment_line; // target is cartesian, position transformed
limits_get_axis_mask_ptr limits_get_axis_mask;
limits_set_target_pos_ptr limits_set_target_pos;
limits_set_machine_positions_ptr limits_set_machine_positions;
homing_cycle_validate_ptr homing_cycle_validate;
homing_cycle_get_feedrate_ptr homing_cycle_get_feedrate;
} kinematics_t;
extern kinematics_t kinematics;
#endif
kinematics/polar.c
+30 -30
View File
@@ -31,7 +31,7 @@
#include "../hal.h"
#include "../settings.h"
#include "../planner.h"
#include "../kinematics.h"
#include "interface.h"
#define RADIUS_AXIS X_AXIS
#define POLAR_AXIS Y_AXIS
@@ -57,22 +57,22 @@ inline static float abs_angle (float ang)
}
// Returns machine position in mm converted from system position steps.
static float *transform_to_cartesian (float *target, float *position)
static coord_data_t *transform_to_cartesian (coord_data_t *target, coord_data_t *position)
{
uint_fast8_t idx = N_AXIS;
do {
switch(--idx) {
case X_AXIS:
target[X_AXIS] = cosf(position[POLAR_AXIS] * RADDEG) * position[RADIUS_AXIS];
target->x = cosf(position->values[POLAR_AXIS] * RADDEG) * position->values[RADIUS_AXIS];
break;
case Y_AXIS:
target[Y_AXIS] = sinf(position[POLAR_AXIS] * RADDEG) * position[RADIUS_AXIS];
target->y = sinf(position->values[POLAR_AXIS] * RADDEG) * position->values[RADIUS_AXIS];
break;
default:
target[idx] = position[idx]; // unchanged
target->values[idx] = position->values[idx]; // unchanged
break;
}
} while(idx);
@@ -81,63 +81,63 @@ static float *transform_to_cartesian (float *target, float *position)
}
// Returns machine position in mm converted from system position steps.
static float *polar_convert_array_steps_to_mpos (float *position, int32_t *steps)
static coord_data_t *polar_convert_array_steps_to_mpos (coord_data_t *position, mpos_t *steps)
{
coord_data_t cpos;
uint_fast8_t idx = N_AXIS;
do {
idx--;
cpos.values[idx] = steps[idx] / settings.axis[idx].steps_per_mm;
cpos.values[idx] = steps->values[idx] / settings.axis[idx].steps_per_mm;
} while(idx);
return transform_to_cartesian(position, cpos.values);
return transform_to_cartesian(position, &cpos);
}
// Transform absolute position from cartesian coordinate system to polar coordinate system
static float *transform_from_cartesian (float *target, float *position)
static coord_data_t *transform_from_cartesian (coord_data_t *target, coord_data_t *position)
{
float delta_ang; // the difference from the last and next angle
uint_fast8_t idx = N_AXIS - 1;
do {
target[idx] = position[idx];
target->values[idx] = position->values[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
target->values[RADIUS_AXIS] = hypot_f(position->x, position->y);
if (target->values[RADIUS_AXIS] == 0.0f) {
target->values[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;
target->values[POLAR_AXIS] = atan2f(position->y, position->x) * DEGRAD;
// no negative angles...we want the absolute angle not -90, use 270
target[POLAR_AXIS] = abs_angle(target[POLAR_AXIS]);
target->values[POLAR_AXIS] = abs_angle(target->values[POLAR_AXIS]);
}
delta_ang = target[POLAR_AXIS] - abs_angle(last_pos.values[POLAR_AXIS]);
delta_ang = target->values[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;
target->values[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);
target->values[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)
static inline float get_distance (coord_data_t *p0, coord_data_t *p1)
{
uint_fast8_t idx = N_AXIS;
float distance = 0.0f;
do {
idx--;
distance += (p0[idx] - p1[idx]) * (p0[idx] - p1[idx]);
distance += (p0->values[idx] - p1->values[idx]) * (p0->values[idx] - p1->values[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 coord_data_t *polar_segment_line (coord_data_t *target, coord_data_t *position, plan_line_data_t *pl_data, bool init)
{
static uint_fast16_t iterations;
static bool segmented;
@@ -151,13 +151,13 @@ static float *polar_segment_line (float *target, float *position, plan_line_data
jog_cancel = false;
r_offset = gc_get_offset(RADIUS_AXIS, false) * 2.0f; //??
memcpy(final_target.values, target, sizeof(final_target));
memcpy(&final_target, target, sizeof(coord_data_t));
transform_to_cartesian(segment_target.values, position);
transform_to_cartesian(&segment_target, 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;
delta.x = target->x - segment_target.x;
delta.y = target->y - segment_target.y;
delta.z = target->z - segment_target.z;
distance = sqrtf(delta.x * delta.x + delta.y * delta.y + delta.z * delta.z);
@@ -193,11 +193,11 @@ static float *polar_segment_line (float *target, float *position, plan_line_data
memcpy(&segment_target, &final_target, sizeof(coord_data_t));
segment_target.values[RADIUS_AXIS] -= r_offset;
transform_from_cartesian(cpos.values, segment_target.values);
transform_from_cartesian(&cpos, &segment_target);
segment_target.values[RADIUS_AXIS] += r_offset;
if(!pl_data->condition.rapid_motion && segmented) {
float rate_multiplier = get_distance(last_pos.values, cpos.values) / distance;
float rate_multiplier = get_distance(&last_pos, &cpos) / distance;
rate_multiplier = rate_multiplier == 0.0f ? 1.0 : (rate_multiplier < 0.5f ? 0.5f : rate_multiplier);
pl_data->feed_rate *= rate_multiplier;
pl_data->rate_multiplier = 1.0f / rate_multiplier;
@@ -206,7 +206,7 @@ static float *polar_segment_line (float *target, float *position, plan_line_data
memcpy(&last_pos, &cpos, sizeof(coord_data_t));
}
return iterations == 0 || jog_cancel ? NULL : cpos.values;
return iterations == 0 || jog_cancel ? NULL : &cpos;
}
static uint_fast8_t polar_limits_get_axis_mask (uint_fast8_t idx)
@@ -258,7 +258,7 @@ static void onProgramCompleted (program_flow_t program_flow, bool check_mode)
coord_data_t cpos;
memset(last_pos.values, 0, sizeof(coord_data_t));
transform_from_cartesian(cpos.values, gc_state.position);
transform_from_cartesian(&cpos, (coord_data_t *)gc_state.position);
memcpy(&last_pos, &cpos, sizeof(coord_data_t));
sys.position[POLAR_AXIS] = lroundf(last_pos.values[POLAR_AXIS] * settings.axis[POLAR_AXIS].steps_per_mm);
kinematics/wall_plotter.c
+46 -47
View File
@@ -34,7 +34,7 @@
#include "../hal.h"
#include "../settings.h"
#include "../planner.h"
#include "../kinematics.h"
#include "interface.h"
#define A_MOTOR X_AXIS // Must be X_AXIS
#define B_MOTOR Y_AXIS // Must be Y_AXIS
@@ -63,34 +63,34 @@ static settings_changed_ptr settings_changed;
// 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 coord_data_t *wp_convert_array_steps_to_mpos (coord_data_t *position, mpos_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;
len.a = (float)steps->values[A_MOTOR] / settings.axis[A_MOTOR].steps_per_mm;
len.b = (float)steps->values[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;
position->x = (machine.width_pow + len.a * len.a - len.b * len.b) / (2.0f * machine.width_mm);
len.a = machine.width_mm - position->x;
position->y = sqrtf(len.b * len.b - len.a * len.a );
position->z = (float)steps->z / 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)
static coord_data_t *transform_to_cartesian (coord_data_t *target, coord_data_t *position)
{
coord_t len;
len.a = position[A_MOTOR];
len.b = position[B_MOTOR];
len.a = position->values[A_MOTOR];
len.b = position->values[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];
target->x = (machine.width_pow + len.a * len.a - len.b * len.b) / (2.0f * machine.width_mm);
len.a = machine.width_mm - target->x;
target->y = sqrtf(len.b * len.b - len.a * len.a );
target->z = position->z;
return target;
}
@@ -98,49 +98,48 @@ static float *transform_to_cartesian (float *target, float *position)
// 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)
inline static float wp_convert_to_a_motor_steps (coord_data_t *target)
{
return sqrtf(target[A_MOTOR] * target[A_MOTOR] + target[B_MOTOR] * target[B_MOTOR]);
return sqrtf(target->values[A_MOTOR] * target->values[A_MOTOR] + target->values[B_MOTOR] * target->values[B_MOTOR]);
}
inline static float wp_convert_to_b_motor_steps (float *target)
inline static float wp_convert_to_b_motor_steps (coord_data_t *target)
{
float xpos = machine.width_mm - target[A_MOTOR];
float xpos = machine.width_mm - target->values[A_MOTOR];
return sqrtf(xpos * xpos + target[B_MOTOR] * target[B_MOTOR]);
return sqrtf(xpos * xpos + target->values[B_MOTOR] * target->values[B_MOTOR]);
}
// Transform absolute position from cartesian coordinate system to wall plotter coordinate system
static float *transform_from_cartesian (float *target, float *position)
static coord_data_t *transform_from_cartesian (coord_data_t *target, coord_data_t *position)
{
uint_fast8_t idx = N_AXIS - 1;
do {
target[idx] = position[idx];
target->values[idx] = position->values[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);
target->values[A_MOTOR] = wp_convert_to_a_motor_steps(position);
target->values[B_MOTOR] = wp_convert_to_b_motor_steps(position);
return target;
}
static inline float get_distance (float *p0, float *p1)
static inline float get_distance (coord_data_t *p0, coord_data_t *p1)
{
uint_fast8_t idx = Z_AXIS;
uint_fast8_t idx = N_AXIS;
float distance = 0.0f;
do {
idx--;
distance += (p0[idx] - p1[idx]) * (p0[idx] - p1[idx]);
distance += (p0->values[idx] - p1->values[idx]) * (p0->values[idx] - p1->values[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 coord_data_t *wp_segment_line (coord_data_t *target, coord_data_t *position, plan_line_data_t *pl_data, bool init)
{
static uint_fast16_t iterations;
static bool segmented;
@@ -155,11 +154,11 @@ static float *wp_segment_line (float *target, float *position, plan_line_data_t
memcpy(final_target.values, target, sizeof(final_target));
transform_to_cartesian(segment_target.values, position);
transform_to_cartesian(&segment_target, 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;
delta.x = target->x - segment_target.x;
delta.y = target->y - segment_target.y;
delta.z = target->z - segment_target.z;
float distance = sqrtf(delta.x * delta.x + delta.y * delta.y);
@@ -193,10 +192,10 @@ static float *wp_segment_line (float *target, float *position, plan_line_data_t
} else
memcpy(&segment_target, &final_target, sizeof(coord_data_t));
transform_from_cartesian(cpos.values, segment_target.values);
transform_from_cartesian(&cpos, &segment_target);
}
return iterations == 0 || jog_cancel ? NULL : cpos.values;
return iterations == 0 || jog_cancel ? NULL : &cpos;
}
@@ -208,20 +207,20 @@ static uint_fast8_t wp_limits_get_axis_mask (uint_fast8_t idx)
static void wp_limits_set_target_pos (uint_fast8_t idx) // fn name?
{
float xy[2];
coord_data_t xy;
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;
xy.x = sys.position[X_AXIS] / settings.axis[X_AXIS].steps_per_mm;
xy.y = 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);
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);
sys.position[A_MOTOR] = sys.position[B_MOTOR] = wp_convert_to_a_motor_steps(&xy);
break;
default:
sys.position[idx] = 0;
@@ -234,21 +233,21 @@ static void wp_limits_set_target_pos (uint_fast8_t idx) // fn name?
// NOTE: settings.max_travel[] is stored as a negative value.
static void wp_limits_set_machine_positions (axes_signals_t cycle)
{
float xy[2];
coord_data_t xy;
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;
xy.x = sys.position[X_AXIS] / settings.axis[X_AXIS].steps_per_mm;
xy.y = 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[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[A_MOTOR] = wp_convert_to_a_motor_steps(&xy);
sys.position[B_MOTOR] = sys.position[A_MOTOR];
break;
default:
@@ -267,12 +266,12 @@ static void wp_limits_set_machine_positions (axes_signals_t cycle)
: lroundf(-pulloff->values[idx] * settings.axis[idx].steps_per_mm);
switch(idx) {
case X_AXIS:
off_axis_position = wp_convert_to_b_motor_steps(xy);
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);
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;
machine_limits.c
+3 -3
View File
@@ -33,7 +33,7 @@
#include "tool_change.h"
#include "state_machine.h"
#ifdef KINEMATICS_API
#include "kinematics.h"
#include "kinematics/interface.h"
#endif
#include "config.h"
@@ -203,7 +203,7 @@ FLASHMEM static bool limits_pull_off (axes_signals_t axis, coord_data_t *distanc
// Bypass mc_line(). Directly plan homing motion.
#ifdef KINEMATICS_API
coord_data_t k_target;
plan_buffer_line(kinematics.transform_from_cartesian(k_target.values, target.values), &plan_data);
plan_buffer_line(kinematics.transform_from_cartesian(&k_target, &target)->values, &plan_data);
#else
plan_buffer_line(target.values, &plan_data);
#endif
@@ -386,7 +386,7 @@ FLASHMEM static bool homing_cycle (axes_signals_t cycle, axes_signals_t auto_squ
#ifdef KINEMATICS_API
coord_data_t k_target;
plan_buffer_line(kinematics.transform_from_cartesian(k_target.values, target.values), &plan_data); // Bypass mc_line(). Directly plan homing motion.;
plan_buffer_line(kinematics.transform_from_cartesian(&k_target, &target)->values, &plan_data); // Bypass mc_line(). Directly plan homing motion.;
#else
plan_buffer_line(target.values, &plan_data); // Bypass mc_line(). Directly plan homing motion.
#endif
motion_control.c
+3 -3
View File
@@ -37,7 +37,7 @@
#include "motion_control.h"
#include "tool_change.h"
#ifdef KINEMATICS_API
#include "kinematics.h"
#include "kinematics/interface.h"
#endif
#if ENABLE_BACKLASH_COMPENSATION
@@ -84,7 +84,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.0f;
target = kinematics.segment_line(target, plan_get_position(), pl_data, true);
target = kinematics.segment_line((coord_data_t *)target, (coord_data_t *)plan_get_position(), pl_data, true)->values;
#endif
// If enabled, check for soft limit violations. Placed here all line motions are picked up
@@ -110,7 +110,7 @@ bool mc_line (float *target, plan_line_data_t *pl_data)
// parser and planner are separate from the system machine positions, this is doable.
#ifdef KINEMATICS_API
while(kinematics.segment_line(target, NULL, pl_data, false)) {
while(kinematics.segment_line((coord_data_t *)target, NULL, pl_data, false)) {
#endif
#if ENABLE_BACKLASH_COMPENSATION
nuts_bolts.h
+1 -1
View File
@@ -326,7 +326,7 @@ typedef struct {
} coord_system_data_t;
typedef union {
int32_t value[N_AXIS];
int32_t values[N_AXIS];
struct {
int32_t x;
int32_t y;
spindle_control.c
+4 -3
View File
@@ -845,9 +845,10 @@ FLASHMEM bool spindle_restore (spindle_ptrs_t *spindle, spindle_state_t state, f
{
bool ok;
if(spindle->cap.laser) // When in laser mode, ignore spindle spin-up delay. Set to turn on laser when cycle starts.
ok = (sys.step_control.update_spindle_rpm = _spindle_set_state(spindle, state, 0.0f, 0));
else if(!(ok = spindle_check_state(spindle, state) && spindle->param->rpm == rpm))
if(spindle->cap.laser) { // When in laser mode, ignore spindle spin-up delay. Set to turn on laser when cycle starts.
if(!(ok = !settings.flags.disable_laser_during_hold))
ok = (sys.step_control.update_spindle_rpm = _spindle_set_state(spindle, state, 0.0f, 0));
} else if(!(ok = spindle_check_state(spindle, state) && spindle->param->rpm == rpm))
ok = spindle_set_state_wait(spindle, state, rpm, delay_ms);
return ok;
system.c
+2 -2
View File
@@ -30,7 +30,7 @@
#include "state_machine.h"
#include "machine_limits.h"
#ifdef KINEMATICS_API
#include "kinematics.h"
#include "kinematics/interface.h"
#endif
static uint8_t n_axis = N_AXIS;
@@ -1240,7 +1240,7 @@ __NOTE:__ If motor steps and machine position are not in the same coordinate fra
void system_convert_array_steps_to_mpos (float *position, int32_t *steps)
{
#ifdef KINEMATICS_API
kinematics.transform_steps_to_cartesian(position, steps);
kinematics.transform_steps_to_cartesian((coord_data_t *)position, (mpos_t *)steps);
#else
uint_fast8_t idx = N_AXIS;
do {
vfs.c
+50 -14
View File
@@ -23,6 +23,7 @@
#include <string.h>
#include <stdlib.h>
#include <errno.h>
#include "hal.h"
#include "vfs.h"
@@ -31,6 +32,8 @@
#undef feof
#endif
typedef void (*vfs_ptr)(void);
static vfs_mount_t *get_rootfs (void);
static inline vfs_mount_t *path_is_mount_dir (const char *path)
@@ -318,6 +321,24 @@ static const char *get_filename (vfs_mount_t *mount, const char *filename)
return filename;
}
static vfs_mount_t *mount_modifiable (const char *path, size_t op_fn)
{
vfs_mount_t *mount = get_mount((path = parse_path(path)));
if(mount == NULL || mount->mode.hidden)
vfs_errno = EFAULT;
else if(mount->mode.read_only)
vfs_errno = EROFS;
else if(*((vfs_ptr *)((uint8_t *)mount->vfs + op_fn)) == NULL)
vfs_errno = EPERM;
else if(!strncmp(mount->path, path, strlen(path)))
vfs_errno = EISDIR;
else
vfs_errno = 0;
return vfs_errno ? NULL : mount;
}
vfs_file_t *vfs_open (const char *filename, const char *mode)
{
vfs_file_t *file = NULL;
@@ -381,6 +402,13 @@ int vfs_seek (vfs_file_t *file, size_t offset)
return ((vfs_t *)(file->fs))->fseek(file, offset);
}
int vfs_truncate (vfs_file_t *file, size_t offset)
{
vfs_errno = ((vfs_t *)(file->fs))->ftruncate ? 0 : EPERM;
return vfs_errno ? -1 : ((vfs_t *)(file->fs))->ftruncate(file, offset);
}
bool vfs_eof (vfs_file_t *file)
{
vfs_errno = 0;
@@ -406,36 +434,44 @@ int vfs_rename (const char *from, const char *to)
int vfs_unlink (const char *filename)
{
int ret;
int ret = -1;
vfs_mount_t *mount; // TODO: test for dir?
vfs_mount_t *mount = get_mount(filename); // TODO: test for dir?
if((ret = mount ? mount->vfs->funlink(get_filename(mount, filename)) : -1) != -1 && vfs.on_fs_changed && !mount->mode.hidden)
vfs.on_fs_changed(mount->vfs);
if((mount = mount_modifiable(filename, offsetof(vfs_t, funlink)))) {
if((ret = mount->vfs->funlink(get_filename(mount, filename))) != -1 && vfs.on_fs_changed)
vfs.on_fs_changed(mount->vfs);
}
return ret;
}
int vfs_mkdir (const char *path)
{
int ret;
int ret = -1;
vfs_mount_t *mount;
vfs_mount_t *mount = get_mount(path);
if((ret = mount ? mount->vfs->fmkdir(get_filename(mount, path)) : -1) != -1 && vfs.on_fs_changed && !mount->mode.hidden)
vfs.on_fs_changed(mount->vfs);
if((mount = mount_modifiable(path, offsetof(vfs_t, fmkdir)))) {
if((ret = mount->vfs->fmkdir(get_filename(mount, path))) != -1 && vfs.on_fs_changed)
vfs.on_fs_changed(mount->vfs);
}
return ret;
}
int vfs_rmdir (const char *path)
{
int ret;
int ret = -1;
vfs_mount_t *mount;
vfs_mount_t *mount = get_mount(path);
if((mount = mount_modifiable(path, offsetof(vfs_t, frmdir)))) {
if((ret = mount ? mount->vfs->frmdir(get_filename(mount, path)) : -1) != -1 && vfs.on_fs_changed && !mount->mode.hidden)
vfs.on_fs_changed(mount->vfs);
vfs_stat_t st;
if(vfs_stat(path, &st) == 0 && !st.st_mode.directory)
vfs_errno = ENOTDIR;
else if((ret = mount->vfs->frmdir(get_filename(mount, path))) != -1 && vfs.on_fs_changed)
vfs.on_fs_changed(mount->vfs);
}
return ret;
}
vfs.h
+3
View File
@@ -127,6 +127,7 @@ typedef size_t (*vfs_write_ptr)(const void *buffer, size_t size, size_t count, v
typedef void (*vfs_close_ptr)(vfs_file_t *file);
typedef size_t (*vfs_ftell_ptr)(vfs_file_t *file);
typedef int (*vfs_fseek_ptr)(vfs_file_t *file, size_t offset);
typedef int (*vfs_ftruncate_ptr)(vfs_file_t *file, size_t length);
typedef bool (*vfs_eof_ptr)(vfs_file_t *file);
typedef int (*vfs_rename_ptr)(const char *from, const char *to);
typedef int (*vfs_unlink_ptr)(const char *filename);
@@ -155,6 +156,7 @@ typedef struct
vfs_write_ptr fwrite;
vfs_ftell_ptr ftell;
vfs_fseek_ptr fseek;
vfs_ftruncate_ptr ftruncate;
vfs_eof_ptr feof;
vfs_rename_ptr frename;
vfs_unlink_ptr funlink;
@@ -235,6 +237,7 @@ size_t vfs_write (const void *buffer, size_t size, size_t count, vfs_file_t *fil
int vfs_puts (const char *s, vfs_file_t *file);
size_t vfs_tell (vfs_file_t *file);
int vfs_seek (vfs_file_t *file, size_t offset);
int vfs_truncate (vfs_file_t *file, size_t length);
bool vfs_eof (vfs_file_t *file);
int vfs_rename (const char *from, const char *to);
int vfs_unlink (const char *filename);