mirror of
https://github.com/grblHAL/core.git
synced 2026-08-18 08:57:24 +08:00
677 lines
25 KiB
C
677 lines
25 KiB
C
/*
|
|
maslow.c - 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 grblHAL by Terje Io.
|
|
*/
|
|
|
|
#include "grbl.h"
|
|
|
|
#ifdef MASLOW_ROUTER
|
|
|
|
#include <math.h>
|
|
|
|
#include "driver.h"
|
|
|
|
#include "settings.h"
|
|
#include "planner.h"
|
|
#include "nvs_buffer.h"
|
|
#include "kinematics.h"
|
|
#include "maslow.h"
|
|
#include "report.h"
|
|
|
|
#define A_MOTOR X_AXIS // Must be X_AXIS
|
|
#define B_MOTOR Y_AXIS // Must be Y_AXIS
|
|
|
|
typedef struct {
|
|
float halfWidth; //Half the machine width
|
|
float halfHeight; //Half the machine height
|
|
float xCordOfMotor;
|
|
float xCordOfMotor_x4;
|
|
float xCordOfMotor_x2_pow;
|
|
float yCordOfMotor;
|
|
float height_to_bit; //distance between sled attach point and bit
|
|
} machine_t;
|
|
|
|
static machine_t machine = {0};
|
|
|
|
uint_fast8_t selected_motor = A_MOTOR;
|
|
maslow_settings_t maslow;
|
|
maslow_hal_t maslow_hal = {0};
|
|
static nvs_address_t nvs_address;
|
|
|
|
static const maslow_settings_t maslow_defaults = {
|
|
.pid[A_MOTOR].Kp = MASLOW_A_KP,
|
|
.pid[A_MOTOR].Ki = MASLOW_A_KI,
|
|
.pid[A_MOTOR].Kd = MASLOW_A_KD,
|
|
.pid[A_MOTOR].Imax = MASLOW_A_IMAX,
|
|
|
|
.pid[B_MOTOR].Kp = MASLOW_B_KP,
|
|
.pid[B_MOTOR].Ki = MASLOW_B_KI,
|
|
.pid[B_MOTOR].Kd = MASLOW_B_KD,
|
|
.pid[B_MOTOR].Imax = MASLOW_B_IMAX,
|
|
|
|
.pid[Z_AXIS].Kp = MASLOW_Z_KP,
|
|
.pid[Z_AXIS].Ki = MASLOW_Z_KI,
|
|
.pid[Z_AXIS].Kd = MASLOW_Z_KD,
|
|
.pid[Z_AXIS].Imax = MASLOW_Z_IMAX,
|
|
|
|
.chainOverSprocket = MASLOW_CHAINOVERSPROCKET,
|
|
.machineWidth = MASLOW_MACHINEWIDTH,
|
|
.machineHeight = MASLOW_MACHINEHEIGHT,
|
|
.distBetweenMotors = MASLOW_DISTBETWEENMOTORS,
|
|
|
|
.motorOffsetY = MASLOW_MOTOROFFSETY,
|
|
.chainSagCorrection = MASLOW_CHAINSAGCORRECTION,
|
|
.leftChainTolerance = MASLOW_LEFTCHAINTOLERANCE,
|
|
.rightChainTolerance = MASLOW_RIGHTCHAINTOLERANCE,
|
|
.rotationDiskRadius = MASLOW_ROTATIONDISKRADIUS,
|
|
|
|
.chainLength = MASLOW_CHAINLENGTH,
|
|
.sledHeight = MASLOW_SLEDHEIGHT,
|
|
.sledWidth = MASLOW_SLEDWIDTH,
|
|
|
|
.XcorrScaling = MASLOW_ACORRSCALING,
|
|
.YcorrScaling = MASLOW_BCORRSCALING
|
|
};
|
|
|
|
static status_code_t set_axis_setting (setting_id_t setting, float value);
|
|
static float get_axis_setting (setting_id_t setting);
|
|
static void maslow_settings_load (void);
|
|
static void maslow_settings_restore (void);
|
|
|
|
static const setting_detail_t maslow_settings[] = {
|
|
#if maslow_MIXED_DRIVERS
|
|
{ Setting_maslowDriver, Group_MotorDriver, "maslow driver", NULL, Format_AxisMask, NULL, NULL, NULL, Setting_NonCore, &maslow.driver_enable.mask },
|
|
#endif
|
|
{ (setting_id_t)Maslow_ChainOverSprocket, Group_MotorDriver, "Chain over sprocket", NULL, Format_Integer, NULL, NULL, NULL, Setting_NonCore, &maslow.chainOverSprocket, NULL },
|
|
{ (setting_id_t)Maslow_MachineWidth, Group_MotorDriver, "Machine width", "mm", Format_Decimal, "###0.0", NULL, NULL, Setting_NonCore, &maslow.machineWidth, NULL },
|
|
{ (setting_id_t)Maslow_MachineHeight, Group_MotorDriver, "Machine height", "mm", Format_Decimal, "###0.0", NULL, NULL, Setting_NonCore, &maslow.machineHeight, NULL },
|
|
{ (setting_id_t)Maslow_DistBetweenMotors, Group_MotorDriver, "Distance between motors", "mm", Format_Decimal, NULL, NULL, NULL, Setting_NonCore, &maslow.distBetweenMotors, NULL },
|
|
{ (setting_id_t)Maslow_MotorOffsetY, Group_MotorDriver, "Motor offset Y", "mm", Format_Decimal, "###0.0", NULL, NULL, Setting_NonCore, &maslow.motorOffsetY, NULL },
|
|
{ (setting_id_t)Maslow_AcorrScaling, Group_MotorDriver, "Acorr Scaling", NULL, Format_Decimal, "###0.0", NULL, NULL, Setting_NonCore, &maslow.XcorrScaling, NULL },
|
|
{ (setting_id_t)Maslow_BcorrScaling, Group_MotorDriver, "BcorrScaling", NULL, Format_Decimal, "###0.0", NULL, NULL, Setting_NonCore, &maslow.XcorrScaling, NULL },
|
|
{ (setting_id_t)AxisSetting_MaslowKP, Group_Axis0, "?-axis KP", NULL, Format_Decimal, "###0.0", NULL, NULL, Setting_NonCoreFn, set_axis_setting, get_axis_setting },
|
|
{ (setting_id_t)AxisSetting_MaslowKI, Group_Axis0, "?-axis KI", NULL, Format_Decimal, "###0.0", NULL, NULL, Setting_NonCoreFn, set_axis_setting, get_axis_setting },
|
|
{ (setting_id_t)AxisSetting_MaslowKD, Group_Axis0, "?-axis KIt", NULL, Format_Decimal, "###0.0", NULL, NULL, Setting_NonCoreFn, set_axis_setting, get_axis_setting },
|
|
{ (setting_id_t)AxisSetting_MaslowIMax, Group_Axis0, "?-axis I Max", "ma", Format_Decimal, "###0.0", NULL, NULL, Setting_NonCoreFn, set_axis_setting, get_axis_setting }
|
|
};
|
|
|
|
static void maslow_settings_save (void)
|
|
{
|
|
hal.nvs.memcpy_to_nvs(nvs_address, (uint8_t *)&maslow, sizeof(maslow_settings_t), true);
|
|
}
|
|
|
|
static setting_details_t details = {
|
|
.settings = maslow_settings,
|
|
.n_settings = sizeof(maslow_settings) / sizeof(setting_detail_t),
|
|
.load = maslow_settings_load,
|
|
.save = maslow_settings_save,
|
|
.restore = maslow_settings_restore
|
|
};
|
|
|
|
static setting_details_t *on_get_settings (void)
|
|
{
|
|
return &details;
|
|
}
|
|
|
|
static status_code_t set_axis_setting (setting_id_t setting, float value)
|
|
{
|
|
status_code_t status = Status_OK;
|
|
|
|
if((setting_id_t)setting >= Setting_AxisSettingsBase && (setting_id_t)setting <= Setting_AxisSettingsMax) {
|
|
|
|
uint_fast16_t base_idx = (uint_fast16_t)setting - (uint_fast16_t)Setting_AxisSettingsBase;
|
|
uint_fast8_t axis_idx = base_idx % AXIS_SETTINGS_INCREMENT;
|
|
|
|
if(axis_idx < N_AXIS) switch((base_idx - axis_idx) / AXIS_SETTINGS_INCREMENT) {
|
|
|
|
case AxisSetting_MaslowKP:
|
|
status = Status_OK;
|
|
maslow.pid[axis_idx].Kp = value;
|
|
break;
|
|
|
|
case AxisSetting_MaslowKI:
|
|
status = Status_OK;
|
|
maslow.pid[axis_idx].Ki = value;
|
|
break;
|
|
|
|
case AxisSetting_MaslowKD:
|
|
status = Status_OK;
|
|
maslow.pid[axis_idx].Kd = value;
|
|
break;
|
|
|
|
case AxisSetting_MaslowIMax:
|
|
status = Status_OK;
|
|
maslow.pid[axis_idx].Imax = value;
|
|
|
|
default:
|
|
status = Status_Unhandled;
|
|
break;
|
|
}
|
|
}
|
|
|
|
return status;
|
|
}
|
|
|
|
static float get_axis_setting (setting_id_t setting)
|
|
{
|
|
float value = 0;
|
|
|
|
if (setting >= Setting_AxisSettingsBase && setting <= Setting_AxisSettingsMax) {
|
|
|
|
uint_fast16_t base_idx = (uint_fast16_t)setting - (uint_fast16_t)Setting_AxisSettingsBase;
|
|
uint_fast8_t axis_idx = base_idx % AXIS_SETTINGS_INCREMENT;
|
|
|
|
if(axis_idx < N_AXIS) switch((base_idx - axis_idx) / AXIS_SETTINGS_INCREMENT) {
|
|
|
|
case AxisSetting_MaslowKP:
|
|
value = maslow.pid[axis_idx].Kp;
|
|
break;
|
|
|
|
case AxisSetting_MaslowKI:
|
|
value = maslow.pid[axis_idx].Ki;
|
|
break;
|
|
|
|
case AxisSetting_MaslowKD:
|
|
value = maslow.pid[axis_idx].Kd;
|
|
break;
|
|
|
|
case AxisSetting_MaslowIMax:
|
|
value = maslow.pid[axis_idx].Imax;
|
|
break;
|
|
}
|
|
}
|
|
|
|
return value;
|
|
}
|
|
|
|
static void maslow_settings_restore (void)
|
|
{
|
|
memcpy(&maslow, &maslow_defaults, sizeof(maslow_settings_t));
|
|
|
|
hal.nvs.memcpy_to_nvs(hal.nvs.driver_area.address, (uint8_t *)&maslow, sizeof(maslow_settings_t), true);
|
|
}
|
|
|
|
static void maslow_settings_load (void)
|
|
{
|
|
if(hal.nvs.memcpy_from_nvs((uint8_t *)&maslow, nvs_address, sizeof(maslow_settings_t), true) != NVS_TransferResult_OK)
|
|
maslow_settings_restore();
|
|
}
|
|
|
|
/** End settings handling **/
|
|
|
|
void recomputeGeometry()
|
|
{
|
|
/*
|
|
Some variables are computed on initialization for the geometry of the machine to reduce overhead,
|
|
calling this function regenerates those values.
|
|
*/
|
|
machine.halfWidth = (maslow.machineWidth / 2.0f);
|
|
machine.halfHeight = (maslow.machineHeight / 2.0f);
|
|
machine.xCordOfMotor = (maslow.distBetweenMotors / 2.0f);
|
|
machine.yCordOfMotor = (machine.halfHeight + maslow.motorOffsetY);
|
|
machine.xCordOfMotor_x4 = machine.xCordOfMotor * 4.0f;
|
|
machine.xCordOfMotor_x2_pow = powf((machine.xCordOfMotor * 2.0f), 2.0f);
|
|
}
|
|
|
|
// limit motion to stay within table (in mm)
|
|
void verifyValidTarget (float* xTarget, float* yTarget)
|
|
{
|
|
//If the target point is beyond one of the edges of the board, the machine stops at the edge
|
|
|
|
recomputeGeometry();
|
|
// no limits for now
|
|
// *xTarget = (*xTarget < -halfWidth) ? -halfWidth : (*xTarget > halfWidth) ? halfWidth : *xTarget;
|
|
// *yTarget = (*yTarget < -halfHeight) ? -halfHeight : (*yTarget > halfHeight) ? halfHeight : *yTarget;
|
|
|
|
}
|
|
|
|
// Maslow CNC calculation only. Returns x or y-axis "steps" based on Maslow motor steps.
|
|
// converts current position two-chain intersection (steps) into x / y cartesian in STEPS..
|
|
static void maslow_convert_array_steps_to_mpos (float *position, int32_t *steps)
|
|
{
|
|
float a_len = ((float)steps[A_MOTOR] / settings.axis[A_MOTOR].steps_per_mm);
|
|
float b_len = ((float)steps[B_MOTOR] / settings.axis[B_MOTOR].steps_per_mm);
|
|
|
|
a_len = (machine.xCordOfMotor_x2_pow - powf(b_len, 2.0f) + powf(a_len, 2.0f)) / machine.xCordOfMotor_x4;
|
|
position[X_AXIS] = a_len - machine.xCordOfMotor;
|
|
a_len = maslow.distBetweenMotors - a_len;
|
|
position[Y_AXIS] = machine.yCordOfMotor - sqrtf(powf(b_len, 2.0f) - powf(a_len, 2.0f));
|
|
position[Z_AXIS] = steps[Z_AXIS] / settings.axis[Z_AXIS].steps_per_mm;
|
|
|
|
// back out any correction factor
|
|
position[X_AXIS] /= maslow.XcorrScaling;
|
|
position[Y_AXIS] /= maslow.YcorrScaling;
|
|
//
|
|
}
|
|
|
|
// calculate left and right (A_MOTOR/B_MOTOR) chain lengths from X-Y cartesian coordinates (in mm)
|
|
// target is an absolute position in the frame
|
|
inline static void triangularInverse (int32_t *target_steps, float *target)
|
|
{
|
|
//Confirm that the coordinates are on the table
|
|
// verifyValidTarget(&xTarget, &yTarget);
|
|
|
|
// scale target (absolute position) by any correction factor
|
|
double xxx = (double)target[A_MOTOR] * (double)maslow.XcorrScaling;
|
|
double yyy = (double)target[B_MOTOR] * (double)maslow.YcorrScaling;
|
|
double yyp = pow((double)machine.yCordOfMotor - yyy, 2.0);
|
|
|
|
//Calculate motor axes length to the bit
|
|
target_steps[A_MOTOR] = (int32_t)lround(sqrt(pow((double)machine.xCordOfMotor + xxx, 2.0f) + yyp) * settings.axis[A_MOTOR].steps_per_mm);
|
|
target_steps[B_MOTOR] = (int32_t)lround(sqrt(pow((double)machine.xCordOfMotor - xxx, 2.0f) + yyp) * settings.axis[B_MOTOR].steps_per_mm);
|
|
}
|
|
|
|
// Transform absolute position from cartesian coordinate system (mm) to maslow coordinate system (step)
|
|
static void maslow_target_to_steps (int32_t *target_steps, float *target)
|
|
{
|
|
uint_fast8_t idx = N_AXIS - 1;
|
|
|
|
do {
|
|
target_steps[idx] = lroundf(target[idx] * settings.axis[idx].steps_per_mm);
|
|
} while(--idx > Y_AXIS);
|
|
|
|
triangularInverse(target_steps, target);
|
|
}
|
|
|
|
static uint_fast8_t maslow_limits_get_axis_mask (uint_fast8_t idx)
|
|
{
|
|
return ((idx == A_MOTOR) || (idx == B_MOTOR)) ? (bit(X_AXIS) | bit(Y_AXIS)) : bit(idx);
|
|
}
|
|
|
|
// MASLOW is circular in motion, so long lines must be divided up
|
|
static bool maslow_segment_line (float *target, plan_line_data_t *pl_data, bool init)
|
|
{
|
|
static uint_fast16_t iterations;
|
|
static bool segmented;
|
|
static float delta[N_AXIS], segment_target[N_AXIS];
|
|
// static plan_line_data_t plan;
|
|
|
|
uint_fast8_t idx = N_AXIS;
|
|
|
|
if(init) {
|
|
|
|
float max_delta = 0.0f;
|
|
|
|
do {
|
|
idx--;
|
|
delta[idx] = target[idx] - gc_state.position[idx];
|
|
max_delta = max(max_delta, fabsf(delta[idx]));
|
|
} while(idx);
|
|
|
|
if((segmented = !(pl_data->condition.rapid_motion || pl_data->condition.jog_motion) &&
|
|
max_delta > MAX_SEG_LENGTH_MM && !(delta[X_AXIS] == 0.0f && delta[Y_AXIS] == 0.0f))) {
|
|
|
|
idx = N_AXIS;
|
|
iterations = (uint_fast16_t)ceilf(max_delta / MAX_SEG_LENGTH_MM);
|
|
|
|
memcpy(segment_target, gc_state.position, sizeof(segment_target));
|
|
// memcpy(&plan, pl_data, sizeof(plan_line_data_t));
|
|
|
|
do {
|
|
delta[--idx] /= (float)iterations;
|
|
target[idx] = gc_state.position[idx];
|
|
} while(idx);
|
|
|
|
} else
|
|
iterations = 1;
|
|
|
|
iterations++; // return at least one iteration
|
|
|
|
} else {
|
|
|
|
iterations--;
|
|
|
|
if(segmented && iterations) do {
|
|
idx--;
|
|
segment_target[idx] += delta[idx];
|
|
target[idx] = segment_target[idx];
|
|
// memcpy(pl_data, &plan, sizeof(plan_line_data_t));
|
|
} while(idx);
|
|
|
|
}
|
|
|
|
return iterations != 0;
|
|
}
|
|
|
|
static void maslow_limits_set_target_pos (uint_fast8_t idx) // fn name?
|
|
{
|
|
/*
|
|
int32_t axis_position;
|
|
float position[3];
|
|
maslow_convert_array_steps_to_mpos(position, sys.position);
|
|
|
|
float aCl,bCl; // set initial chain lengths to table center when $HOME
|
|
void triangularInverse(float ,float , float* , float* );
|
|
|
|
x_axis.axis_Position = 0;
|
|
x_axis.target = 0;
|
|
x_axis.target_PS = 0;
|
|
x_axis.Integral = 0;
|
|
y_axis.axis_Position = 0;
|
|
y_axis.target = 0;
|
|
y_axis.target_PS = 0;
|
|
y_axis.Integral = 0;
|
|
z_axis.axis_Position = 0;
|
|
z_axis.target = 0;
|
|
z_axis.target_PS = 0;
|
|
z_axis.Integral = 0;
|
|
set_axis_position = 0; // force to center of table -- its a Maslow thing
|
|
|
|
triangularInverse((float)(set_axis_position), (float)(set_axis_position), &aCl, &bCl);
|
|
sys.position[A_MOTOR] = (int32_t) lround(aCl * settings.steps_per_mm[A_MOTOR]);
|
|
sys.position[B_MOTOR] = (int32_t) lround(bCl * settings.steps_per_mm[B_MOTOR]);
|
|
sys.position[Z_AXIS] = set_axis_position;
|
|
|
|
store_current_machine_pos(); // reset all the way out to stored space
|
|
sys.step_control = STEP_CONTROL_NORMAL_OP; // Return step control to normal operation.
|
|
return;
|
|
|
|
sys.position[idx] = set_axis_position;
|
|
|
|
switch(idx) {
|
|
case X_AXIS:
|
|
axis_position = system_convert_maslow_to_y_axis_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] = system_convert_maslow_to_x_axis_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 maslow_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] = system_convert_maslow_to_y_axis_steps(sys.position);
|
|
sys.position[B_MOTOR] = - sys.position[A_MOTOR];
|
|
break;
|
|
case Y_AXIS:
|
|
sys.position[A_MOTOR] = system_convert_maslow_to_x_axis_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.max_travel[idx] + settings.homing.pulloff) * settings.steps_per_mm[idx])
|
|
: lroundf(-settings.homing.pulloff * settings.steps_per_mm[idx]);
|
|
switch(idx) {
|
|
case X_AXIS:
|
|
off_axis_position = system_convert_maslow_to_y_axis_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 = system_convert_maslow_to_x_axis_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);
|
|
*/
|
|
}
|
|
|
|
// TODO: format output in grbl fashion: [...]
|
|
status_code_t maslow_tuning (sys_state_t state, char *line)
|
|
{
|
|
status_code_t retval = Status_OK;
|
|
|
|
if(line[1] == 'M') switch(line[2]) {
|
|
|
|
case 'C': // commit driver setting changes to non-volatile storage
|
|
settings_dirty.is_dirty = settings_dirty.driver_settings = true;
|
|
break;
|
|
|
|
case 'X':
|
|
selected_motor = A_MOTOR;
|
|
hal.stream.write("X-Axis Selected" ASCII_EOL);
|
|
break;
|
|
|
|
case 'Y':
|
|
selected_motor = B_MOTOR;
|
|
hal.stream.write("Y-Axis Selected" ASCII_EOL);
|
|
break;
|
|
|
|
case 'Z':
|
|
selected_motor = Z_AXIS;
|
|
if(maslow_hal.get_debug_data(selected_motor))
|
|
hal.stream.write("Z-Axis Selected" ASCII_EOL);
|
|
else {
|
|
selected_motor = A_MOTOR;
|
|
hal.stream.write("Z-Axis is not PID controlled, switched to A motor" ASCII_EOL);
|
|
}
|
|
break;
|
|
|
|
case 'G':
|
|
maslow_hal.pos_enable(true);
|
|
break;
|
|
|
|
case 'R': // reset current position
|
|
maslow_hal.reset_pid(selected_motor);
|
|
break;
|
|
|
|
case '+': // Move
|
|
maslow_hal.move(selected_motor, 10000);
|
|
break;
|
|
|
|
case '-': // Move
|
|
maslow_hal.move(selected_motor, -10000);
|
|
break;
|
|
|
|
case '*': // Move
|
|
maslow_hal.move(selected_motor, 10);
|
|
break;
|
|
|
|
case '/': // Move
|
|
maslow_hal.move(selected_motor, -10);
|
|
break;
|
|
|
|
case 'I':
|
|
case 'M':
|
|
case 'D':
|
|
case 'P':
|
|
case 'S':
|
|
case 'A':;
|
|
if(line[3] == '=' && line[4] != '\0') {
|
|
float parameter;
|
|
uint_fast8_t counter = 4;
|
|
|
|
if(!read_float(line, &counter, ¶meter))
|
|
retval = Status_BadNumberFormat;
|
|
|
|
else switch(line[2]) {
|
|
|
|
case 'P':
|
|
maslow.pid[selected_motor].Kp = parameter;
|
|
hal.stream.write("Kp == ");
|
|
hal.stream.write(ftoa(maslow.pid[selected_motor].Kp, 3));
|
|
hal.stream.write(ASCII_EOL);
|
|
break;
|
|
|
|
case 'D':
|
|
maslow.pid[selected_motor].Kd = parameter;
|
|
hal.stream.write("Kd == ");
|
|
hal.stream.write(ftoa(maslow.pid[selected_motor].Kd, 3));
|
|
hal.stream.write(ASCII_EOL);
|
|
break;
|
|
|
|
case 'I':
|
|
maslow.pid[selected_motor].Ki = parameter;
|
|
hal.stream.write("Ki == ");
|
|
hal.stream.write(ftoa(maslow.pid[selected_motor].Ki, 3));
|
|
hal.stream.write(ASCII_EOL);
|
|
maslow_hal.pid_settings_changed(selected_motor);
|
|
break;
|
|
|
|
case 'M':
|
|
maslow.pid[selected_motor].Imax = parameter;
|
|
hal.stream.write("Imax == ");
|
|
hal.stream.write(ftoa(maslow.pid[selected_motor].Imax, 3));
|
|
hal.stream.write(ASCII_EOL);
|
|
maslow_hal.pid_settings_changed(selected_motor);
|
|
break;
|
|
|
|
case 'S':
|
|
{
|
|
maslow_hal.tuning_enable(true);
|
|
int32_t sz = maslow_hal.set_step_size(selected_motor, (int32_t)parameter);
|
|
hal.stream.write("S == ");
|
|
hal.stream.write(ftoa((float)sz, 0));
|
|
hal.stream.write(ASCII_EOL);
|
|
}
|
|
break;
|
|
|
|
case 'A': // test kinematics - from X,Y mm to A,B steps back to X,Y mm
|
|
{
|
|
float xyz[N_AXIS];
|
|
int32_t abz[N_AXIS];
|
|
recomputeGeometry();
|
|
xyz[X_AXIS] = parameter;
|
|
if(line[counter++] == ',' && line[counter] != '\0') {
|
|
if(!read_float(line, &counter, &xyz[Y_AXIS]))
|
|
retval = Status_BadNumberFormat;
|
|
} else
|
|
retval = Status_BadNumberFormat;
|
|
|
|
if(retval == Status_OK) {
|
|
|
|
triangularInverse(abz, xyz);
|
|
hal.stream.write("[KINEMATICSTRANSFORM: X,Y = ");
|
|
hal.stream.write(ftoa(xyz[X_AXIS], 3));
|
|
hal.stream.write(",");
|
|
hal.stream.write(ftoa(xyz[Y_AXIS], 3));
|
|
hal.stream.write(" -> A,B steps: ");
|
|
hal.stream.write(uitoa((uint32_t)abz[A_MOTOR]));
|
|
hal.stream.write(",");
|
|
hal.stream.write(uitoa((uint32_t)abz[B_MOTOR]));
|
|
|
|
maslow_convert_array_steps_to_mpos(xyz, abz);
|
|
hal.stream.write(" -> X,Y = ");
|
|
hal.stream.write(ftoa(xyz[X_AXIS], 3));
|
|
hal.stream.write(",");
|
|
hal.stream.write(ftoa(xyz[Y_AXIS], 3));
|
|
hal.stream.write("]" ASCII_EOL);
|
|
}
|
|
}
|
|
break;
|
|
}
|
|
} else
|
|
retval = Status_BadNumberFormat;
|
|
break;
|
|
|
|
default:
|
|
{
|
|
maslow_debug_t *debug = maslow_hal.get_debug_data(selected_motor);
|
|
|
|
hal.stream.write("[AXISPID:");
|
|
hal.stream.write(axis_letter[selected_motor]);
|
|
hal.stream.write(": Kp = ");
|
|
hal.stream.write(ftoa(maslow.pid[selected_motor].Kp, 3));
|
|
hal.stream.write(" Ki = ");
|
|
hal.stream.write(ftoa(maslow.pid[selected_motor].Ki, 3));
|
|
hal.stream.write(" Kd = ");
|
|
hal.stream.write(ftoa(maslow.pid[selected_motor].Kd, 3));
|
|
hal.stream.write(" Imax = ");
|
|
hal.stream.write(ftoa(maslow.pid[selected_motor].Imax, 3));
|
|
|
|
hal.stream.write("]\r\n[PIDDATA:err=");
|
|
hal.stream.write(ftoa(debug->Error, 0));
|
|
hal.stream.write("\t\ti=");
|
|
hal.stream.write(ftoa(debug->Integral, 0));
|
|
hal.stream.write("\tiT=");
|
|
hal.stream.write(ftoa(debug->iterm, 0));
|
|
hal.stream.write("\td=");
|
|
hal.stream.write(ftoa(debug->DiffTerm, 0));
|
|
// hal.stream.write("\tV=");
|
|
// hal.stream.write(ftoa(debug->totalSpeed, 0));
|
|
hal.stream.write("\txCMD=");
|
|
hal.stream.write(ftoa(debug->speed, 0));
|
|
// hal.stream.write("\tyCMD=");
|
|
// hal.stream.write(uitoa(motor[Y_AXIS]->speed));
|
|
// hal.stream.write("\tzCMD=");
|
|
// hal.stream.write(uitoa(motor[Z_AXIS]->speed));
|
|
hal.stream.write("]" ASCII_EOL);
|
|
}
|
|
break;
|
|
} else
|
|
retval = Status_Unhandled;
|
|
|
|
return retval;
|
|
}
|
|
|
|
// Initialize API pointers & machine parameters for Maslow router kinematics
|
|
bool maslow_init (void)
|
|
{
|
|
float xy[2] = {0.0f, 0.0f};
|
|
|
|
if((nvs_address = nvs_alloc(sizeof(maslow_settings_t)))) {
|
|
|
|
details.on_get_settings = grbl.on_get_settings;
|
|
grbl.on_get_settings = on_get_settings;
|
|
|
|
recomputeGeometry();
|
|
triangularInverse(sys.position, xy);
|
|
|
|
selected_motor = A_MOTOR;
|
|
|
|
kinematics.limits_set_target_pos = maslow_limits_set_target_pos;
|
|
kinematics.limits_get_axis_mask = maslow_limits_get_axis_mask;
|
|
kinematics.limits_set_machine_positions = maslow_limits_set_machine_positions;
|
|
kinematics.plan_target_to_steps = maslow_target_to_steps;
|
|
kinematics.convert_array_steps_to_mpos = maslow_convert_array_steps_to_mpos;
|
|
kinematics.segment_line = maslow_segment_line;
|
|
|
|
grbl.on_unknown_sys_command = maslow_tuning;
|
|
}
|
|
|
|
return nvs_address != 0;
|
|
}
|
|
|
|
#endif
|
|
|