mirror of
https://github.com/bdring/Grbl_Esp32.git
synced 2025-09-03 11:22:38 +02:00
- Kinematics and macro button features added
- refactored kinematics to make it easy to associate different types with a different machines. - Servo Axis: If it detects out of range calibration values, it returns them to default values - Added macro button feature - Added optional software debounce for control switches - Some clean up
This commit is contained in:
Bart Dring
@@ -38,28 +38,11 @@ volatile uint8_t sys_rt_exec_accessory_override; // Global realtime executor bit
|
||||
void setup() {
|
||||
|
||||
serial_init(); // Setup serial baud rate and interrupts
|
||||
settings_init(); // Load Grbl settings from EEPROM
|
||||
|
||||
|
||||
settings_init(); // Load Grbl settings from EEPROM
|
||||
|
||||
stepper_init(); // Configure stepper pins and interrupt timers
|
||||
system_ini(); // Configure pinout pins and pin-change interrupt (Renamed due to conflict with esp32 files)
|
||||
|
||||
system_ini(); // Configure pinout pins and pin-change interrupt (Renamed due to conflict with esp32 files)
|
||||
|
||||
//#ifdef ENABLE_BLUETOOTH
|
||||
// if $I has some text, that is the bluetooth name
|
||||
// This is a temporary convenience until a new setting is defined
|
||||
//char line[LINE_BUFFER_SIZE];
|
||||
//settings_read_build_info(line);
|
||||
//if (line[0] != '\0') {
|
||||
// // just send to serial because it is the only interface available
|
||||
// Serial.printf("Starting Bluetooth:%s", line);
|
||||
// bluetooth_init(line);
|
||||
//}
|
||||
//#endif
|
||||
|
||||
|
||||
|
||||
memset(sys_position,0,sizeof(sys_position)); // Clear machine position.
|
||||
|
||||
#ifdef USE_PEN_SERVO
|
||||
|
||||
@@ -47,7 +47,7 @@ Some features should not be changed. See notes below.
|
||||
// one configuration file by placing their specific defaults and pin map at the bottom of this file.
|
||||
// If doing so, simply comment out these two defines and see instructions below.
|
||||
#define DEFAULTS_GENERIC
|
||||
#define CPU_MAP_ESP32 // these are defined in cpu_map.h
|
||||
#define CPU_MAP_POLAR_COASTER // these are defined in cpu_map.h
|
||||
#define VERBOSE_HELP // adds addition help info, but could confuse some senders
|
||||
|
||||
|
||||
@@ -281,8 +281,10 @@ Some features should not be changed. See notes below.
|
||||
// This allows control pins to be ignored.
|
||||
// Since these are typically used on the pins that don't have pullups, they will float and cause
|
||||
// problems if not externally pulled up. Ignoring will always return not activated when read.
|
||||
#define IGNORE_CONTROL_PINS
|
||||
//#define IGNORE_CONTROL_PINS
|
||||
|
||||
//#define ENABLE_CONTROL_SW_DEBOUNCE // Default disabled. Uncomment to enable.
|
||||
#define CONTROL_SW_DEBOUNCE_PERIOD 32 // in milliseconds default 32 microseconds
|
||||
|
||||
|
||||
// Inverts select limit pin states based on the following mask. This effects all limit pin functions,
|
||||
|
||||
@@ -509,104 +509,131 @@
|
||||
#endif
|
||||
|
||||
#ifdef CPU_MAP_POLAR_COASTER // The Buildlog.net pen polar coaster controller V1
|
||||
#define CPU_MAP_NAME "CPU_MAP_POLAR_COASTER"
|
||||
#define CPU_MAP_NAME "CPU_MAP_POLAR_COASTER"
|
||||
|
||||
#define USE_KINEMATICS
|
||||
#include "polar_coaster.h"
|
||||
|
||||
#define X_STEP_PIN GPIO_NUM_15
|
||||
#define Y_STEP_PIN GPIO_NUM_2
|
||||
#define X_DIRECTION_PIN GPIO_NUM_25
|
||||
#define Y_DIRECTION_PIN GPIO_NUM_26
|
||||
|
||||
#define STEPPERS_DISABLE_PIN GPIO_NUM_17
|
||||
|
||||
#define X_LIMIT_PIN GPIO_NUM_4
|
||||
#define LIMIT_MASK B1
|
||||
|
||||
#define CONTROL_RESET_PIN GPIO_NUM_13
|
||||
|
||||
// If SPINDLE_PWM_PIN is commented out, this frees up the pin, but Grbl will still
|
||||
// use a virtual spindle. Do not comment out the other parameters for the spindle.
|
||||
//#define SPINDLE_PWM_PIN GPIO_NUM_17
|
||||
#define SPINDLE_PWM_CHANNEL 0
|
||||
// PWM Generator is based on 80,000,000 Hz counter
|
||||
// Therefor the freq determines the resolution
|
||||
// 80,000,000 / freq = max resolution
|
||||
// For 5000 that is 80,000,000 / 5000 = 16000
|
||||
// round down to nearest bit count for SPINDLE_PWM_MAX_VALUE = 13bits (8192)
|
||||
#define SPINDLE_PWM_BASE_FREQ 5000 // Hz
|
||||
#define SPINDLE_PWM_BIT_PRECISION 8 // be sure to match this with SPINDLE_PWM_MAX_VALUE
|
||||
#define SPINDLE_PWM_OFF_VALUE 0
|
||||
#define SPINDLE_PWM_MAX_VALUE 255 // (2^SPINDLE_PWM_BIT_PRECISION)
|
||||
|
||||
#ifndef SPINDLE_PWM_MIN_VALUE
|
||||
#define SPINDLE_PWM_MIN_VALUE 1 // Must be greater than zero.
|
||||
#endif
|
||||
|
||||
#define SPINDLE_PWM_RANGE (SPINDLE_PWM_MAX_VALUE-SPINDLE_PWM_MIN_VALUE)
|
||||
|
||||
#define USE_PEN_SERVO
|
||||
#define SERVO_PEN_PIN GPIO_NUM_16
|
||||
|
||||
// redefine some stuff from config.h
|
||||
#define HOMING_CYCLE_0 (1<<X_AXIS) // this 'bot only homes the X axis
|
||||
#ifdef HOMING_CYCLE_1
|
||||
#undef HOMING_CYCLE_1
|
||||
#endif
|
||||
#ifdef HOMING_CYCLE_2
|
||||
#undef HOMING_CYCLE_2
|
||||
#endif
|
||||
|
||||
#ifdef DEFAULTS_GENERIC
|
||||
#undef DEFAULTS_GENERIC // undefine generic then define each default below
|
||||
#endif
|
||||
|
||||
// defaults
|
||||
#define DEFAULT_STEP_PULSE_MICROSECONDS 3
|
||||
#define DEFAULT_STEPPER_IDLE_LOCK_TIME 255 // stay on
|
||||
|
||||
#define DEFAULT_STEPPING_INVERT_MASK 0 // uint8_t
|
||||
#define DEFAULT_DIRECTION_INVERT_MASK 2 // uint8_t
|
||||
#define DEFAULT_INVERT_ST_ENABLE 0 // boolean
|
||||
#define DEFAULT_INVERT_LIMIT_PINS 1 // boolean
|
||||
#define DEFAULT_INVERT_PROBE_PIN 0 // boolean
|
||||
|
||||
#define DEFAULT_STATUS_REPORT_MASK 1
|
||||
|
||||
#define DEFAULT_JUNCTION_DEVIATION 0.01 // mm
|
||||
#define DEFAULT_ARC_TOLERANCE 0.002 // mm
|
||||
#define DEFAULT_REPORT_INCHES 0 // false
|
||||
|
||||
#define DEFAULT_SOFT_LIMIT_ENABLE 0 // false
|
||||
#define DEFAULT_HARD_LIMIT_ENABLE 0 // false
|
||||
|
||||
#define DEFAULT_HOMING_ENABLE 1
|
||||
#define DEFAULT_HOMING_DIR_MASK 0 // move positive dir Z, negative X,Y
|
||||
#define DEFAULT_HOMING_FEED_RATE 200.0 // mm/min
|
||||
#define DEFAULT_HOMING_SEEK_RATE 1000.0 // mm/min
|
||||
#define DEFAULT_HOMING_DEBOUNCE_DELAY 250 // msec (0-65k)
|
||||
#define DEFAULT_HOMING_PULLOFF 3.0 // mm
|
||||
#define USE_RMT_STEPS
|
||||
|
||||
#define DEFAULT_SPINDLE_RPM_MAX 1000.0 // rpm
|
||||
#define DEFAULT_SPINDLE_RPM_MIN 0.0 // rpm
|
||||
|
||||
#define DEFAULT_LASER_MODE 0 // false
|
||||
|
||||
#define DEFAULT_X_STEPS_PER_MM 200.0
|
||||
#define DEFAULT_Y_STEPS_PER_MM 71.111
|
||||
#define DEFAULT_Z_STEPS_PER_MM 100.0 // This is percent in servo mode
|
||||
|
||||
#define DEFAULT_X_MAX_RATE 5000.0 // mm/min
|
||||
#define DEFAULT_Y_MAX_RATE 15000.0 // mm/min
|
||||
#define DEFAULT_Z_MAX_RATE 3000.0 // mm/min
|
||||
|
||||
#define DEFAULT_X_ACCELERATION (200.0*60*60) // 10*60*60 mm/min^2 = 10 mm/sec^2
|
||||
#define DEFAULT_Y_ACCELERATION (200.0*60*60) // 10*60*60 mm/min^2 = 10 mm/sec^2
|
||||
#define DEFAULT_Z_ACCELERATION (50.0*60*60)
|
||||
|
||||
#define DEFAULT_X_MAX_TRAVEL 50.0 // mm NOTE: Must be a positive value.
|
||||
#define DEFAULT_Y_MAX_TRAVEL 300.0 // mm NOTE: Must be a positive value.
|
||||
#define DEFAULT_Z_MAX_TRAVEL 100.0 // This is percent in servo mode
|
||||
|
||||
|
||||
#define X_STEP_PIN GPIO_NUM_15
|
||||
#define X_RMT_CHANNEL 0
|
||||
#define Y_STEP_PIN GPIO_NUM_2
|
||||
#define Y_RMT_CHANNEL 1
|
||||
#define X_DIRECTION_PIN GPIO_NUM_25
|
||||
#define Y_DIRECTION_PIN GPIO_NUM_26
|
||||
|
||||
#define STEPPERS_DISABLE_PIN GPIO_NUM_17
|
||||
|
||||
#ifndef USE_SERVO_AXES // maybe set in config.h
|
||||
#define USE_SERVO_AXES
|
||||
#endif
|
||||
|
||||
#define SERVO_Z_PIN GPIO_NUM_16
|
||||
#define SERVO_Z_CHANNEL_NUM 5
|
||||
#define SERVO_Z_RANGE_MIN 0.0
|
||||
#define SERVO_Z_RANGE_MAX 5.0
|
||||
#define SERVO_Z_HOMING_TYPE SERVO_HOMING_TARGET // during homing it will instantly move to a target value
|
||||
#define SERVO_Z_HOME_POS SERVO_Z_RANGE_MAX // move to max during homing
|
||||
#define SERVO_Z_MPOS false // will not use mpos, uses work coordinates
|
||||
|
||||
#define X_LIMIT_PIN GPIO_NUM_4
|
||||
#define LIMIT_MASK B1
|
||||
|
||||
#ifdef IGNORE_CONTROL_PINS // maybe set in config.h
|
||||
#undef IGNORE_CONTROL_PINS
|
||||
#endif
|
||||
|
||||
#ifndef ENABLE_CONTROL_SW_DEBOUNCE
|
||||
#define ENABLE_CONTROL_SW_DEBOUNCE
|
||||
#endif
|
||||
|
||||
#define MACRO_BUTTON_0_PIN GPIO_NUM_13
|
||||
#define MACRO_BUTTON_1_PIN GPIO_NUM_12
|
||||
#define MACRO_BUTTON_2_PIN GPIO_NUM_14
|
||||
|
||||
// If SPINDLE_PWM_PIN is commented out, this frees up the pin, but Grbl will still
|
||||
// use a virtual spindle. Do not comment out the other parameters for the spindle.
|
||||
//#define SPINDLE_PWM_PIN GPIO_NUM_17
|
||||
#define SPINDLE_PWM_CHANNEL 0
|
||||
// PWM Generator is based on 80,000,000 Hz counter
|
||||
// Therefor the freq determines the resolution
|
||||
// 80,000,000 / freq = max resolution
|
||||
// For 5000 that is 80,000,000 / 5000 = 16000
|
||||
// round down to nearest bit count for SPINDLE_PWM_MAX_VALUE = 13bits (8192)
|
||||
#define SPINDLE_PWM_BASE_FREQ 5000 // Hz
|
||||
#define SPINDLE_PWM_BIT_PRECISION 8 // be sure to match this with SPINDLE_PWM_MAX_VALUE
|
||||
#define SPINDLE_PWM_OFF_VALUE 0
|
||||
#define SPINDLE_PWM_MAX_VALUE 255 // (2^SPINDLE_PWM_BIT_PRECISION)
|
||||
|
||||
#ifndef SPINDLE_PWM_MIN_VALUE
|
||||
#define SPINDLE_PWM_MIN_VALUE 1 // Must be greater than zero.
|
||||
#endif
|
||||
|
||||
#define SPINDLE_PWM_RANGE (SPINDLE_PWM_MAX_VALUE-SPINDLE_PWM_MIN_VALUE)
|
||||
|
||||
//#define USE_PEN_SERVO
|
||||
//#define SERVO_PEN_PIN GPIO_NUM_16
|
||||
|
||||
// redefine some stuff from config.h
|
||||
#define HOMING_CYCLE_0 (1<<X_AXIS) // this 'bot only homes the X axis
|
||||
#ifdef HOMING_CYCLE_1
|
||||
#undef HOMING_CYCLE_1
|
||||
#endif
|
||||
#ifdef HOMING_CYCLE_2
|
||||
#undef HOMING_CYCLE_2
|
||||
#endif
|
||||
|
||||
#ifdef DEFAULTS_GENERIC
|
||||
#undef DEFAULTS_GENERIC // undefine generic then define each default below
|
||||
#endif
|
||||
|
||||
// defaults
|
||||
#define DEFAULT_STEP_PULSE_MICROSECONDS 3
|
||||
#define DEFAULT_STEPPER_IDLE_LOCK_TIME 255 // stay on
|
||||
|
||||
#define DEFAULT_STEPPING_INVERT_MASK 0 // uint8_t
|
||||
#define DEFAULT_DIRECTION_INVERT_MASK 2 // uint8_t
|
||||
#define DEFAULT_INVERT_ST_ENABLE 0 // boolean
|
||||
#define DEFAULT_INVERT_LIMIT_PINS 1 // boolean
|
||||
#define DEFAULT_INVERT_PROBE_PIN 0 // boolean
|
||||
|
||||
#define DEFAULT_STATUS_REPORT_MASK 2 // MPos enabled
|
||||
|
||||
#define DEFAULT_JUNCTION_DEVIATION 0.01 // mm
|
||||
#define DEFAULT_ARC_TOLERANCE 0.002 // mm
|
||||
#define DEFAULT_REPORT_INCHES 0 // false
|
||||
|
||||
#define DEFAULT_SOFT_LIMIT_ENABLE 0 // false
|
||||
#define DEFAULT_HARD_LIMIT_ENABLE 0 // false
|
||||
|
||||
#define DEFAULT_HOMING_ENABLE 1
|
||||
#define DEFAULT_HOMING_DIR_MASK 0 // move positive dir Z, negative X,Y
|
||||
#define DEFAULT_HOMING_FEED_RATE 200.0 // mm/min
|
||||
#define DEFAULT_HOMING_SEEK_RATE 1000.0 // mm/min
|
||||
#define DEFAULT_HOMING_DEBOUNCE_DELAY 250 // msec (0-65k)
|
||||
#define DEFAULT_HOMING_PULLOFF 3.0 // mm
|
||||
|
||||
#define DEFAULT_SPINDLE_RPM_MAX 1000.0 // rpm
|
||||
#define DEFAULT_SPINDLE_RPM_MIN 0.0 // rpm
|
||||
|
||||
#define DEFAULT_LASER_MODE 0 // false
|
||||
|
||||
#define DEFAULT_X_STEPS_PER_MM 200.0
|
||||
#define DEFAULT_Y_STEPS_PER_MM 71.111
|
||||
#define DEFAULT_Z_STEPS_PER_MM 100.0 // This is percent in servo mode
|
||||
|
||||
#define DEFAULT_X_MAX_RATE 5000.0 // mm/min
|
||||
#define DEFAULT_Y_MAX_RATE 15000.0 // mm/min
|
||||
#define DEFAULT_Z_MAX_RATE 3000.0 // mm/min
|
||||
|
||||
#define DEFAULT_X_ACCELERATION (200.0*60*60) // 10*60*60 mm/min^2 = 10 mm/sec^2
|
||||
#define DEFAULT_Y_ACCELERATION (200.0*60*60) // 10*60*60 mm/min^2 = 10 mm/sec^2
|
||||
#define DEFAULT_Z_ACCELERATION (50.0*60*60)
|
||||
|
||||
#define DEFAULT_X_MAX_TRAVEL 50.0 // mm NOTE: Must be a positive value.
|
||||
#define DEFAULT_Y_MAX_TRAVEL 300.0 // mm NOTE: Must be a positive value.
|
||||
#define DEFAULT_Z_MAX_TRAVEL 100.0 // This is percent in servo mode
|
||||
|
||||
#endif
|
||||
|
||||
@@ -1159,11 +1186,7 @@
|
||||
#endif
|
||||
|
||||
#ifdef CPU_MAP_FOO_6X
|
||||
#define CPU_MAP_NAME "CPU_MAP_FOO_6X"
|
||||
/*
|
||||
#define USE_KINEMATICS
|
||||
#include "kinematics.h"
|
||||
*/
|
||||
#define CPU_MAP_NAME "CPU_MAP_FOO_6X"
|
||||
|
||||
// Be sure to change to N_AXIS 6 in nuts_bolts.h
|
||||
#ifdef N_AXIS
|
||||
|
||||
@@ -1,91 +0,0 @@
|
||||
/*
|
||||
kinematics.cpp - Implements simple inverse kinematics for Grbl_ESP32
|
||||
Part of Grbl_ESP32
|
||||
|
||||
Copyright (c) 2019 Barton Dring @buildlog
|
||||
|
||||
|
||||
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/>.
|
||||
|
||||
Inverse kinematics determine the joint parameters required to get to a position
|
||||
in 3D space. Grbl will still work as 3 axes of steps, but these steps could
|
||||
represent angles, etc instead of linear units.
|
||||
|
||||
Unless forward kinematics are applied to the reporting, Grbl will report raw joint
|
||||
values instead of the normal Cartesian positions
|
||||
|
||||
How it works...
|
||||
|
||||
If you tell it to go to X10 Y10 Z10 in Cartesian space, for example, the equations
|
||||
will convert those values to the required joint values. In the case of a polar machine, X represents the radius,
|
||||
Y represents the polar degrees and Z would be unchanged.
|
||||
|
||||
In most cases, a straight line in Cartesian space could cause a curve in the new system.
|
||||
To fix this, the line is broken into very small segments and each segment is converted
|
||||
to the new space. While each segment is also distorted, the amount is so small it cannot be seen.
|
||||
|
||||
This segmentation is how normal Grbl draws arcs.
|
||||
|
||||
Feed Rate
|
||||
|
||||
Feed rate is given in steps/time. Due to the new coordinate units and non linearity issues, the
|
||||
feed rate may need to be adjusted. The ratio of the step distances in the original coordinate system
|
||||
determined and applied to the feed rate.
|
||||
|
||||
TODO:
|
||||
Add y offset, for completeness
|
||||
Add ZERO_NON_HOMED_AXES option
|
||||
|
||||
|
||||
*/
|
||||
#include "grbl.h"
|
||||
|
||||
#ifdef USE_KINEMATICS
|
||||
|
||||
void inverse_kinematics(float *target, plan_line_data_t *pl_data, float *position)
|
||||
{
|
||||
// use the current tool as a flag for converting for different machine mode.
|
||||
|
||||
// target: This is the target location for the move in machine space millimeters.
|
||||
// position: This is the previous position in machine space millimeters
|
||||
|
||||
float offsets[N_AXIS]; // current work offsets
|
||||
float new_system[N_AXIS]; // target location in the new system
|
||||
uint8_t index;
|
||||
|
||||
// skip kinematics... not implemented yet.
|
||||
mc_line(target, pl_data);
|
||||
return;
|
||||
|
||||
// All data is in machine space and axes have different offsets, so we need to apply the offset in corrections
|
||||
for (index = 0; index < N_AXIS; index++) {
|
||||
offsets[index] = gc_state.coord_system[index] + gc_state.coord_offset[index]; // offset from machine coordinate system
|
||||
}
|
||||
|
||||
switch (gc_state.tool) {
|
||||
case 1: // XYZ only ignore all ABC
|
||||
|
||||
break;
|
||||
case 2: // Convert XYZ to ABC (no motion on ABC)
|
||||
|
||||
break;
|
||||
default: // tool 0 or above 2 uses normal 6 axis code...no changes
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
#endif
|
||||
@@ -33,7 +33,6 @@ void mc_line_kins(float *target, plan_line_data_t *pl_data, float *position)
|
||||
#ifndef USE_KINEMATICS
|
||||
mc_line(target, pl_data);
|
||||
#else // else use kinematics
|
||||
//grbl_sendf(CLIENT_SERIAL, "[MSG:Kin Tool %d]\r\n", gc_state.tool);
|
||||
inverse_kinematics(target, pl_data, position);
|
||||
#endif
|
||||
}
|
||||
|
||||
213
Grbl_Esp32/polar_coaster.cpp
Normal file
213
Grbl_Esp32/polar_coaster.cpp
Normal file
@@ -0,0 +1,213 @@
|
||||
/*
|
||||
kinematics_polar_coaster.cpp - Implements simple inverse kinematics for Grbl_ESP32
|
||||
Part of Grbl_ESP32
|
||||
|
||||
Copyright (c) 2019 Barton Dring @buildlog
|
||||
|
||||
|
||||
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/>.
|
||||
|
||||
Inverse kinematics determine the joint parameters required to get to a position
|
||||
in 3D space. Grbl will still work as 3 axes of steps, but these steps could
|
||||
represent angles, etc instead of linear units.
|
||||
|
||||
Unless forward kinematics are applied to the reporting, Grbl will report raw joint
|
||||
values instead of the normal Cartesian positions
|
||||
|
||||
How it works...
|
||||
|
||||
If you tell it to go to X10 Y10 Z10 in Cartesian space, for example, the equations
|
||||
will convert those values to the required joint values. In the case of a polar machine, X represents the radius,
|
||||
Y represents the polar degrees and Z would be unchanged.
|
||||
|
||||
In most cases, a straight line in Cartesian space could cause a curve in the new system.
|
||||
To fix this, the line is broken into very small segments and each segment is converted
|
||||
to the new space. While each segment is also distorted, the amount is so small it cannot be seen.
|
||||
|
||||
This segmentation is how normal Grbl draws arcs.
|
||||
|
||||
Feed Rate
|
||||
|
||||
Feed rate is given in steps/time. Due to the new coordinate units and non linearity issues, the
|
||||
feed rate may need to be adjusted. The ratio of the step distances in the original coordinate system
|
||||
determined and applied to the feed rate.
|
||||
|
||||
TODO:
|
||||
Add y offset, for completeness
|
||||
Add ZERO_NON_HOMED_AXES option
|
||||
|
||||
|
||||
*/
|
||||
#include "grbl.h"
|
||||
|
||||
#ifdef USE_KINEMATICS
|
||||
|
||||
/*
|
||||
Apply inverse kinematics for a polar system
|
||||
|
||||
float target: The desired target location in machine space
|
||||
plan_line_data_t *pl_data: Plan information like feed rate, etc
|
||||
float *position: The previous "from" location of the move
|
||||
|
||||
Note: It is assumed only the radius axis (X) is homed and only X and Z have offsets
|
||||
|
||||
|
||||
*/
|
||||
void inverse_kinematics(float *target, plan_line_data_t *pl_data, float *position)
|
||||
{
|
||||
static float last_angle = 0;
|
||||
static float last_radius = 0;
|
||||
|
||||
float dx, dy, dz; // distances in each cartesian axis
|
||||
float p_dx, p_dy, p_dz; // distances in each polar axis
|
||||
|
||||
float dist, polar_dist; // the distances in both systems...used to determine feed rate
|
||||
float new_feedrate; //
|
||||
|
||||
uint32_t segment_count; // number of segments the move will be broken in to.
|
||||
float seg_target[N_AXIS]; // The target of the current segment
|
||||
|
||||
float polar[N_AXIS]; // target location in polar coordinates
|
||||
|
||||
float x_offset = gc_state.coord_system[X_AXIS]+gc_state.coord_offset[X_AXIS]; // offset from machine coordinate system
|
||||
float z_offset = gc_state.coord_system[Z_AXIS]+gc_state.coord_offset[Z_AXIS]; // offset from machine coordinate system
|
||||
|
||||
//grbl_sendf(CLIENT_SERIAL, "Position: %4.2f %4.2f %4.2f \r\n", position[X_AXIS] - x_offset, position[Y_AXIS], position[Z_AXIS]);
|
||||
//grbl_sendf(CLIENT_SERIAL, "Target: %4.2f %4.2f %4.2f \r\n", target[X_AXIS] - x_offset, target[Y_AXIS], target[Z_AXIS]);
|
||||
|
||||
// calculate cartesian move distance for each axis
|
||||
dx = target[X_AXIS] - position[X_AXIS];
|
||||
dy = target[Y_AXIS] - position[Y_AXIS];
|
||||
dz = target[Z_AXIS] - position[Z_AXIS];
|
||||
|
||||
// calculate the total X,Y axis move distance
|
||||
// Z axis is the same in both coord systems, so it is ignored
|
||||
dist = sqrt( (dx * dx) + (dy * dy) + (dz * dz));
|
||||
|
||||
if (pl_data->condition & PL_COND_FLAG_RAPID_MOTION) {
|
||||
segment_count = 1; // rapid G0 motion is not used to draw, so skip the segmentation
|
||||
} else {
|
||||
segment_count = ceil(dist / SEGMENT_LENGTH); // determine the number of segments we need ... round up so there is at least 1
|
||||
}
|
||||
|
||||
dist /= segment_count; // segment distance
|
||||
|
||||
|
||||
for(uint32_t segment = 1; segment <= segment_count; segment++) {
|
||||
// determine this segment's target
|
||||
seg_target[X_AXIS] = position[X_AXIS] + (dx / float(segment_count) * segment) - x_offset;
|
||||
seg_target[Y_AXIS] = position[Y_AXIS] + (dy / float(segment_count) * segment);
|
||||
seg_target[Z_AXIS] = position[Z_AXIS] + (dz / float(segment_count) * segment) - z_offset;
|
||||
|
||||
calc_polar(seg_target, polar, last_angle);
|
||||
|
||||
// begin determining new feed rate
|
||||
|
||||
// calculate move distance for each axis
|
||||
p_dx = polar[RADIUS_AXIS] - last_radius;
|
||||
p_dy = polar[POLAR_AXIS] - last_angle;
|
||||
p_dz = dz;
|
||||
|
||||
polar_dist = sqrt( (p_dx * p_dx) + (p_dy * p_dy) +(p_dz * p_dz)); // calculate the total move distance
|
||||
|
||||
if (polar_dist == 0 || dist == 0) { // prevent 0 feed rate
|
||||
polar_dist = dist; // default to same feed rate
|
||||
}
|
||||
|
||||
pl_data->feed_rate *= polar_dist / dist; // apply the distance ratio between coord systems
|
||||
|
||||
// end determining new feed rate
|
||||
|
||||
polar[RADIUS_AXIS] += x_offset;
|
||||
polar[Z_AXIS] += z_offset;
|
||||
|
||||
last_radius = polar[RADIUS_AXIS];
|
||||
last_angle = polar[POLAR_AXIS];
|
||||
|
||||
//grbl_sendf(CLIENT_SERIAL, "Polar: %4.2f \r\n", polar[POLAR_AXIS]);
|
||||
//grbl_sendf(CLIENT_SERIAL, "Radius: %4.2f \r\n", polar[RADIUS_AXIS]);
|
||||
|
||||
mc_line(polar, pl_data);
|
||||
}
|
||||
}
|
||||
|
||||
// helper functions
|
||||
|
||||
/*******************************************
|
||||
* Calculate polar values from Cartesian values
|
||||
* float target_xyz: An array of target axis positions in Cartesian (xyz) space
|
||||
* float polar: An array to return the polar values
|
||||
* float last_angle: The polar angle of the "from" point.
|
||||
*
|
||||
* Angle calculated is 0 to 360, but you don't want a line to go from 350 to 10. This would
|
||||
* be a long line backwards. You want it to go from 350 to 370. The same is true going the other way.
|
||||
*
|
||||
* This means the angle could accumulate to very high positive or negative values over the coarse of
|
||||
* a long job.
|
||||
*
|
||||
*/
|
||||
void calc_polar(float *target_xyz, float *polar, float last_angle)
|
||||
{
|
||||
float delta_ang; // the difference from the last and next angle
|
||||
|
||||
target_xyz[X_AXIS] *= -1.0; // compensate for Polar Coaster's radial axis being mirrored (right side) from normal 0deg
|
||||
|
||||
// determine the new polar values
|
||||
polar[POLAR_AXIS] = atan2(target_xyz[Y_AXIS], target_xyz[X_AXIS]) * 180.0 / M_PI;
|
||||
|
||||
// no negative angles...we want the absolute angle not -90, use 270
|
||||
if (polar[POLAR_AXIS] < 0.0) {
|
||||
polar[POLAR_AXIS] = 360.0 + polar[POLAR_AXIS];
|
||||
}
|
||||
|
||||
polar[RADIUS_AXIS] = hypot_f(target_xyz[X_AXIS], target_xyz[Y_AXIS]);
|
||||
|
||||
polar[Z_AXIS] = target_xyz[Z_AXIS]; // Z is unchanged
|
||||
|
||||
delta_ang = polar[POLAR_AXIS] - fmod(last_angle, 360.0); // what is the difference from the last angle
|
||||
|
||||
// if the delta is above 180 degrees it means we are crossing the 0 degree line
|
||||
if ( fabs(delta_ang) <= 180.0) {
|
||||
polar[POLAR_AXIS] = last_angle + delta_ang;
|
||||
} else {
|
||||
if (fmod(last_angle, 360.0) > 180.0) { // crossing zer clockwise
|
||||
polar[POLAR_AXIS] = last_angle + delta_ang + 360.0;
|
||||
} else { // rossing zero counter clockwise
|
||||
polar[POLAR_AXIS] = last_angle - (360.0 - delta_ang);
|
||||
}
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
// Polar coaster has macro buttons, this handles those button pushes.
|
||||
void user_defined_macro(uint8_t index)
|
||||
{
|
||||
//grbl_sendf(CLIENT_SERIAL, "[MSG: Macro #%d]\r\n", index);
|
||||
switch (index) {
|
||||
case CONTROL_PIN_INDEX_MACRO_0:
|
||||
Serial2Socket.push("$H\r"); // home machine
|
||||
break;
|
||||
case CONTROL_PIN_INDEX_MACRO_1:
|
||||
Serial2Socket.push("[ESP220]/1.nc\r"); // run SD card file 1.nc
|
||||
break;
|
||||
case CONTROL_PIN_INDEX_MACRO_2:
|
||||
Serial2Socket.push("[ESP220]/2.nc\r"); // run SD card file 2.nc
|
||||
break;
|
||||
case CONTROL_PIN_INDEX_MACRO_3:
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1,5 +1,5 @@
|
||||
/*
|
||||
kinematics.h - Implements simple kinematics for Grbl_ESP32
|
||||
kinematics_polar_coaster.h - Implements simple kinematics for Grbl_ESP32
|
||||
Part of Grbl_ESP32
|
||||
|
||||
Copyright (c) 2019 Barton Dring @buildlog
|
||||
@@ -19,9 +19,18 @@
|
||||
along with Grbl. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#define RADIUS_AXIS 0
|
||||
#define POLAR_AXIS 1
|
||||
|
||||
#define SEGMENT_LENGTH 0.5 // segment length in mm
|
||||
|
||||
#ifndef kinematics_h
|
||||
#define kinematics_h
|
||||
#define kinematics_h
|
||||
|
||||
#include "grbl.h"
|
||||
|
||||
void inverse_kinematics(float *target, plan_line_data_t *pl_data, float *position);
|
||||
void calc_polar(float *target_xyz, float *polar, float last_angle);
|
||||
void user_defined_macro(uint8_t index);
|
||||
|
||||
#endif
|
||||
@@ -276,7 +276,9 @@ bool ServoAxis::_cal_is_valid()
|
||||
|
||||
if ( (settings.steps_per_mm[_axis] < SERVO_CAL_MIN) || (settings.steps_per_mm[_axis] > SERVO_CAL_MAX) ) {
|
||||
if (_showError) {
|
||||
grbl_sendf(CLIENT_SERIAL, "[MSG:Servo cal ($10%d) Error: %4.4f s/b between %.2f and %.2f]\r\n", _axis, settings.steps_per_mm[_axis], SERVO_CAL_MIN, SERVO_CAL_MAX);
|
||||
grbl_sendf(CLIENT_SERIAL, "[MSG:Servo calibration ($10%d) value error. Reset to 100]\r\n", _axis);
|
||||
settings.steps_per_mm[_axis] = 100;
|
||||
write_global_settings();
|
||||
}
|
||||
settingsOK = false;
|
||||
}
|
||||
@@ -284,12 +286,19 @@ bool ServoAxis::_cal_is_valid()
|
||||
// Note: Max travel is set positive via $$, but stored as a negative number
|
||||
if ( (settings.max_travel[_axis] < -SERVO_CAL_MAX) || (settings.max_travel[_axis] > -SERVO_CAL_MIN) ) {
|
||||
if (_showError) {
|
||||
grbl_sendf(CLIENT_SERIAL, "[MSG:Servo cal ($13%d) Error: %4.4f s/b between %.2f and %.2f]\r\n", _axis, -settings.max_travel[_axis], SERVO_CAL_MIN, SERVO_CAL_MAX);
|
||||
grbl_sendf(CLIENT_SERIAL, "[MSG:Servo calibration ($13%d) value error. Reset to 100]\r\n", _axis);
|
||||
settings.max_travel[_axis] = -100;
|
||||
write_global_settings();
|
||||
}
|
||||
settingsOK = false;
|
||||
}
|
||||
|
||||
_showError = false; // to show error once
|
||||
|
||||
if (! settingsOK) {
|
||||
write_global_settings(); // they were changed so write them to
|
||||
}
|
||||
|
||||
return settingsOK;
|
||||
}
|
||||
|
||||
|
||||
@@ -21,6 +21,8 @@
|
||||
#include "grbl.h"
|
||||
#include "config.h"
|
||||
|
||||
xQueueHandle control_sw_queue; // used by control switch debouncing
|
||||
|
||||
void system_ini() // Renamed from system_init() due to conflict with esp32 files
|
||||
{
|
||||
// setup control inputs
|
||||
@@ -42,6 +44,38 @@ void system_ini() // Renamed from system_init() due to conflict with esp32 files
|
||||
pinMode(CONTROL_CYCLE_START_PIN, INPUT);
|
||||
attachInterrupt(digitalPinToInterrupt(CONTROL_CYCLE_START_PIN), isr_control_inputs, CHANGE);
|
||||
#endif
|
||||
|
||||
#ifdef MACRO_BUTTON_0_PIN
|
||||
pinMode(MACRO_BUTTON_0_PIN, INPUT_PULLUP);
|
||||
attachInterrupt(digitalPinToInterrupt(MACRO_BUTTON_0_PIN), isr_control_inputs, CHANGE);
|
||||
#endif
|
||||
|
||||
#ifdef MACRO_BUTTON_1_PIN
|
||||
pinMode(MACRO_BUTTON_1_PIN, INPUT_PULLUP);
|
||||
attachInterrupt(digitalPinToInterrupt(MACRO_BUTTON_1_PIN), isr_control_inputs, CHANGE);
|
||||
#endif
|
||||
|
||||
#ifdef MACRO_BUTTON_2_PIN
|
||||
pinMode(MACRO_BUTTON_2_PIN, INPUT_PULLUP);
|
||||
attachInterrupt(digitalPinToInterrupt(MACRO_BUTTON_2_PIN), isr_control_inputs, CHANGE);
|
||||
#endif
|
||||
|
||||
#ifdef MACRO_BUTTON_3_PIN
|
||||
pinMode(MACRO_BUTTON_3_PIN, INPUT_PULLUP);
|
||||
attachInterrupt(digitalPinToInterrupt(MACRO_BUTTON_3_PIN), isr_control_inputs, CHANGE);
|
||||
#endif
|
||||
|
||||
#ifdef ENABLE_CONTROL_SW_DEBOUNCE
|
||||
// setup task used for debouncing
|
||||
control_sw_queue = xQueueCreate(10, sizeof( int ));
|
||||
|
||||
xTaskCreate(controlCheckTask,
|
||||
"controlCheckTask",
|
||||
2048,
|
||||
NULL,
|
||||
5, // priority
|
||||
NULL);
|
||||
#endif
|
||||
|
||||
#endif
|
||||
//customize pin definition if needed
|
||||
@@ -50,32 +84,33 @@ void system_ini() // Renamed from system_init() due to conflict with esp32 files
|
||||
#endif
|
||||
}
|
||||
|
||||
#ifdef ENABLE_CONTROL_SW_DEBOUNCE
|
||||
// this is the debounce task
|
||||
void controlCheckTask(void *pvParameters)
|
||||
{
|
||||
while(true) {
|
||||
int evt;
|
||||
xQueueReceive(control_sw_queue, &evt, portMAX_DELAY); // block until receive queue
|
||||
vTaskDelay( CONTROL_SW_DEBOUNCE_PERIOD / portTICK_PERIOD_MS ); // delay a while
|
||||
|
||||
uint8_t pin = system_control_get_state();
|
||||
if (pin) {
|
||||
system_exec_control_pin(pin);
|
||||
}
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
void IRAM_ATTR isr_control_inputs()
|
||||
{
|
||||
|
||||
uint8_t pin = system_control_get_state();
|
||||
if (pin) {
|
||||
if (bit_istrue(pin,CONTROL_PIN_INDEX_RESET))
|
||||
{
|
||||
grbl_send(CLIENT_SERIAL, "[MSG:Reset via control pin]\r\n"); // help debug reason for reset
|
||||
mc_reset();
|
||||
}
|
||||
else if (bit_istrue(pin,CONTROL_PIN_INDEX_CYCLE_START))
|
||||
{
|
||||
bit_true(sys_rt_exec_state, EXEC_CYCLE_START);
|
||||
|
||||
}
|
||||
else if (bit_istrue(pin,CONTROL_PIN_INDEX_FEED_HOLD))
|
||||
{
|
||||
bit_true(sys_rt_exec_state, EXEC_FEED_HOLD);
|
||||
|
||||
}
|
||||
else if (bit_istrue(pin,CONTROL_PIN_INDEX_SAFETY_DOOR))
|
||||
{
|
||||
bit_true(sys_rt_exec_state, EXEC_SAFETY_DOOR);
|
||||
|
||||
}
|
||||
}
|
||||
{
|
||||
#ifdef ENABLE_CONTROL_SW_DEBOUNCE
|
||||
// we will start a task that will recheck the switches after a small delay
|
||||
int evt;
|
||||
xQueueSendFromISR(control_sw_queue, &evt, NULL);
|
||||
#else
|
||||
uint8_t pin = system_control_get_state();
|
||||
system_exec_control_pin(pin);
|
||||
#endif
|
||||
}
|
||||
|
||||
// Executes user startup script, if stored.
|
||||
@@ -425,6 +460,27 @@ uint8_t system_control_get_state()
|
||||
if (digitalRead(CONTROL_CYCLE_START_PIN)) { control_state |= CONTROL_PIN_INDEX_CYCLE_START; }
|
||||
#endif
|
||||
|
||||
#ifdef MACRO_BUTTON_0_PIN
|
||||
defined_pin_mask |= CONTROL_PIN_INDEX_MACRO_0;
|
||||
if (digitalRead(MACRO_BUTTON_0_PIN)) { control_state |= CONTROL_PIN_INDEX_MACRO_0; }
|
||||
#endif
|
||||
|
||||
#ifdef MACRO_BUTTON_1_PIN
|
||||
defined_pin_mask |= CONTROL_PIN_INDEX_MACRO_1;
|
||||
if (digitalRead(MACRO_BUTTON_1_PIN)) { control_state |= CONTROL_PIN_INDEX_MACRO_1; }
|
||||
#endif
|
||||
|
||||
#ifdef MACRO_BUTTON_2_PIN
|
||||
defined_pin_mask |= CONTROL_PIN_INDEX_MACRO_2;
|
||||
if (digitalRead(MACRO_BUTTON_2_PIN)) { control_state |= CONTROL_PIN_INDEX_MACRO_2; }
|
||||
#endif
|
||||
|
||||
#ifdef MACRO_BUTTON_3_PIN
|
||||
defined_pin_mask |= CONTROL_PIN_INDEX_MACRO_3;
|
||||
if (digitalRead(MACRO_BUTTON_3_PIN)) { control_state |= CONTROL_PIN_INDEX_MACRO_3; }
|
||||
#endif
|
||||
|
||||
|
||||
#ifdef INVERT_CONTROL_PIN_MASK
|
||||
control_state ^= (INVERT_CONTROL_PIN_MASK & defined_pin_mask);
|
||||
#endif
|
||||
@@ -444,6 +500,44 @@ uint8_t get_limit_pin_mask(uint8_t axis_idx)
|
||||
return 0;
|
||||
}
|
||||
|
||||
// execute the function of the control pin
|
||||
void system_exec_control_pin(uint8_t pin) {
|
||||
|
||||
if (bit_istrue(pin,CONTROL_PIN_INDEX_RESET)) {
|
||||
grbl_send(CLIENT_SERIAL, "[MSG:Reset via control pin]\r\n"); // help debug reason for reset
|
||||
mc_reset();
|
||||
}
|
||||
else if (bit_istrue(pin,CONTROL_PIN_INDEX_CYCLE_START)) {
|
||||
bit_true(sys_rt_exec_state, EXEC_CYCLE_START);
|
||||
}
|
||||
else if (bit_istrue(pin,CONTROL_PIN_INDEX_FEED_HOLD)) {
|
||||
bit_true(sys_rt_exec_state, EXEC_FEED_HOLD);
|
||||
}
|
||||
else if (bit_istrue(pin,CONTROL_PIN_INDEX_SAFETY_DOOR)) {
|
||||
bit_true(sys_rt_exec_state, EXEC_SAFETY_DOOR);
|
||||
}
|
||||
#ifdef MACRO_BUTTON_0_PIN
|
||||
else if (pin == 96) {
|
||||
user_defined_macro(CONTROL_PIN_INDEX_MACRO_0); // function must be implemented by user
|
||||
}
|
||||
#endif
|
||||
#ifdef MACRO_BUTTON_1_PIN
|
||||
else if (pin == 80) {
|
||||
user_defined_macro(CONTROL_PIN_INDEX_MACRO_1); // function must be implemented by user
|
||||
}
|
||||
#endif
|
||||
#ifdef MACRO_BUTTON_2_PIN
|
||||
else if (pin == 48) {
|
||||
user_defined_macro(CONTROL_PIN_INDEX_MACRO_2); // function must be implemented by user
|
||||
}
|
||||
#endif
|
||||
#ifdef MACRO_BUTTON_3_PIN
|
||||
else if (bit_istrue(pin,CONTROL_PIN_INDEX_MACRO_3)) {
|
||||
user_defined_macro(CONTROL_PIN_INDEX_MACRO_3); // function must be implemented by user
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
// CoreXY calculation only. Returns x or y-axis "steps" based on CoreXY motor steps.
|
||||
int32_t system_convert_corexy_to_x_axis_steps(int32_t *steps)
|
||||
{
|
||||
|
||||
@@ -134,6 +134,10 @@ extern system_t sys;
|
||||
#define CONTROL_PIN_INDEX_RESET bit(1)
|
||||
#define CONTROL_PIN_INDEX_FEED_HOLD bit(2)
|
||||
#define CONTROL_PIN_INDEX_CYCLE_START bit(3)
|
||||
#define CONTROL_PIN_INDEX_MACRO_0 bit(4)
|
||||
#define CONTROL_PIN_INDEX_MACRO_1 bit(5)
|
||||
#define CONTROL_PIN_INDEX_MACRO_2 bit(6)
|
||||
#define CONTROL_PIN_INDEX_MACRO_3 bit(7)
|
||||
//#else
|
||||
//#define N_CONTROL_PIN 3
|
||||
//#define CONTROL_PIN_INDEX_RESET bit(0)
|
||||
@@ -217,5 +221,9 @@ void system_clear_exec_accessory_overrides();
|
||||
int32_t system_convert_corexy_to_x_axis_steps(int32_t *steps);
|
||||
int32_t system_convert_corexy_to_y_axis_steps(int32_t *steps);
|
||||
|
||||
// A task that runs after a control switch interrupt for debouncing.
|
||||
void controlCheckTask(void *pvParameters);
|
||||
void system_exec_control_pin(uint8_t pin);
|
||||
|
||||
|
||||
#endif
|
||||
|
||||
Reference in New Issue
Block a user