commit cdca5ad55cc9b6076401a841583bd53e3baf1481 Author: Terje Io Date: Mon Feb 22 12:29:17 2021 +0100 test check in diff --git a/.gitattributes b/.gitattributes new file mode 100644 index 0000000..eb84b66 --- /dev/null +++ b/.gitattributes @@ -0,0 +1,18 @@ +# Auto detect text files and perform LF normalization +* text=auto eol=lf + + +# Custom for Visual Studio +*.cs diff=csharp + +# Standard to msysgit +*.doc diff=astextplain +*.DOC diff=astextplain +*.docx diff=astextplain +*.DOCX diff=astextplain +*.dot diff=astextplain +*.DOT diff=astextplain +*.pdf diff=astextplain +*.PDF diff=astextplain +*.rtf diff=astextplain +*.RTF diff=astextplain diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..572825f --- /dev/null +++ b/.gitignore @@ -0,0 +1,43 @@ +# Windows thumbnail cache files +Thumbs.db +ehthumbs.db +ehthumbs_vista.db + +# Folder config file +Desktop.ini + +# Recycle Bin used on file shares +$RECYCLE.BIN/ + +# Windows Installer files +*.cab +*.msi +*.msm +*.msp + +# Windows shortcuts +*.lnk + +# Subversion folders + +.svn + +# Eclipse folders + +.settings + +# STM32 .ioc files + +*.ioc + +# Build folder + +build + +# +# ========================= +# Operating System Files +# ========================= + +.vscode/settings.json +.vscode/c_cpp_properties.json diff --git a/alarms.h b/alarms.h new file mode 100644 index 0000000..5d10396 --- /dev/null +++ b/alarms.h @@ -0,0 +1,76 @@ +/* + alarms.h - + + Part of grblHAL + + Copyright (c) 2017-2021 Terje Io + Copyright (c) 2011-2016 Sungeun K. Jeon for Gnea Research LLC + Copyright (c) 2009-2011 Simen Svale Skogsrud + + 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 . +*/ + +#ifndef _ALARMS_H_ +#define _ALARMS_H_ + +// Alarm executor codes. Valid values (1-255). Zero is reserved. +typedef enum { + Alarm_None = 0, + Alarm_HardLimit = 1, + Alarm_SoftLimit = 2, + Alarm_AbortCycle = 3, + Alarm_ProbeFailInitial = 4, + Alarm_ProbeFailContact = 5, + Alarm_HomingFailReset = 6, + Alarm_HomingFailDoor = 7, + Alarm_FailPulloff = 8, + Alarm_HomingFailApproach = 9, + Alarm_EStop = 10, + Alarm_HomingRequried = 11, + Alarm_LimitsEngaged = 12, + Alarm_ProbeProtect = 13, + Alarm_Spindle = 14, + Alarm_HomingFailAutoSquaringApproach = 15, + Alarm_SelftestFailed = 16, + Alarm_MotorFault = 17 +} alarm_code_t; + +typedef struct { + alarm_code_t id; + const char *name; + const char *description; +} alarm_detail_t; + +PROGMEM static const alarm_detail_t alarm_detail[] = { + { Alarm_HardLimit, "Hard limit", "Hard limit has been triggered. Machine position is likely lost due to sudden halt. Re-homing is highly recommended." }, + { Alarm_SoftLimit, "Soft limit", "Soft limit alarm. G-code motion target exceeds machine travel. Machine position retained. Alarm may be safely unlocked." }, + { Alarm_AbortCycle, "Abort during cycle", "Reset while in motion. Machine position is likely lost due to sudden halt. Re-homing is highly recommended." }, + { Alarm_ProbeFailInitial, "Probe fail", "Probe fail. Probe is not in the expected initial state before starting probe cycle when G38.2 and G38.3 is not triggered and G38.4 and G38.5 is triggered." }, + { Alarm_ProbeFailContact, "Probe fail", "Probe fail. Probe did not contact the workpiece within the programmed travel for G38.2 and G38.4." }, + { Alarm_HomingFailReset, "Homing fail", "Homing fail. The active homing cycle was reset." }, + { Alarm_HomingFailDoor, "Homing fail", "Homing fail. Safety door was opened during homing cycle." }, + { Alarm_FailPulloff, "Homing fail", "Homing fail. Pull off travel failed to clear limit switch. Try increasing pull-off setting or check wiring." }, + { Alarm_HomingFailApproach, "Homing fail", "Homing fail. Could not find limit switch within search distances. Try increasing max travel, decreasing pull-off distance, or check wiring." }, + { Alarm_EStop, "EStop", "EStop asserted. Clear and reset" }, + { Alarm_HomingRequried, "Homing required", "Homing required. Execute homing command ($H) to continue." }, + { Alarm_LimitsEngaged, "Limit switch engaged", "Limit switch engaged. Clear before continuing." }, + { Alarm_ProbeProtect, "Probe protection triggered", "Probe protection triggered. Clear before continuing." }, + { Alarm_Spindle, "Spindle at speed timeout", "Spindle at speed timeout. Clear before continuing." }, + { Alarm_HomingFailAutoSquaringApproach, "Homing fail", "Homing fail. Could not find second limit switch for auto squared axis within search distances. Try increasing max travel, decreasing pull-off distance, or check wiring." }, + { Alarm_SelftestFailed, "Selftest failed", "Power on selftest (POS) failed." }, + { Alarm_MotorFault, "Motor fault", "Motor fault." } +}; + +#endif + diff --git a/config.h b/config.h new file mode 100644 index 0000000..3762954 --- /dev/null +++ b/config.h @@ -0,0 +1,614 @@ +/* + config.h - compile time configuration and default setting values + + Part of grblHAL + + Copyright (c) 2020-2021 Terje Io + + Grbl is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + Grbl is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with Grbl. If not, see . +*/ + +// This file contains compile-time configurations for Grbl's internal system. For the most part, +// users will not need to directly modify these, but they are here for specific needs, i.e. +// performance tuning or adjusting to non-typical machines. + +// IMPORTANT: Any changes here requires a full re-compiling of the source code to propagate them. + +#ifndef _GRBL_CONFIG_H_ +#define _GRBL_CONFIG_H_ + +// Compile time only default configuration + +// Number of axes supported: minimum 3, maximum 6 +// If more than 3 axes are required a compliant driver must be provided +//#define N_AXIS 3 // Number of axes + +// Defines compatibility level with the grbl 1.1 protocol. +// Additional G- and M-codes are not disabled except when level is set to >= 10. +// This does not apply to G- and M-codes dependent on driver and/or configuration settings disabled by stting level > 1. +// Set to 0 for reporting itself as "GrblHAL" with protocol extensions enabled. +// Set to 1 to disable some extensions, and for reporting itself as "Grbl". +// Set to 2 to disable new settings as well, use #define parameters for setting default values. +// These can be found in in this file and in defaults.h. +// Set to 10 to also disable new coordinate system offsets (G59.1 - G59.3) and some $# report extensions. +// NOTE: if switching to a level > 1 please reset non-volatile storage with $RST=* after reflashing! +#define COMPATIBILITY_LEVEL 0 + +//#define KINEMATICS_API // Remove comment to add HAL entry points for custom kinematics + +// Enable Maslow router kinematics. +// Experimental - testing required and homing needs to be worked out. +//#define MASLOW_ROUTER // Default disabled. Uncomment to enable. + +// Enable wall plotter kinematics. +// Experimental - testing required and homing needs to be worked out. +//#define WALL_PLOTTER // Default disabled. Uncomment to enable. + +// Enable CoreXY kinematics. Use ONLY with CoreXY machines. +// IMPORTANT: If homing is enabled, you must reconfigure the homing cycle #defines above to +// #define HOMING_CYCLE_0 X_AXIS_BIT and #define HOMING_CYCLE_1 Y_AXIS_BIT +// NOTE: This configuration option alters the motion of the X and Y axes to principle of operation +// defined at (http://corexy.com/theory.html). Motors are assumed to positioned and wired exactly as +// described, if not, motions may move in strange directions. Grbl requires the CoreXY A and B motors +// have the same steps per mm internally. +//#define COREXY // Default disabled. Uncomment to enable. + +// Add a short delay for each block processed in Check Mode to +// avoid overwhelming the sender with fast reply messages. +// This is likely to happen when streaming is done via a protocol where +// the speed is not limited to 115200 baud. An example is native USB streaming. +//#define CHECK_MODE_DELAY 0 // ms + +// This option enables the safety door switch. A safety door, when triggered, +// immediately forces a feed hold and then safely de-energizes the machine. Resuming is blocked until +// the safety door is re-engaged. When it is, Grbl will re-energize the machine and then resume on the +// previous tool path, as if nothing happened. +// #define ENABLE_SAFETY_DOOR_INPUT_PIN // Default disabled. Uncomment to enable. + +// After the safety door switch has been toggled and restored, this setting sets the power-up delay +// between restoring the spindle and coolant and resuming the cycle. +//#define SAFETY_DOOR_SPINDLE_DELAY 4.0f // Float (seconds) +//#define SAFETY_DOOR_COOLANT_DELAY 1.0f // Float (seconds) + +// Control signals bit definitions and mask. +// NOTE: these definitions are only referenced in this file. Do NOT change! +#define SIGNALS_RESET_BIT (1<<0) +#define SIGNALS_FEEDHOLD_BIT (1<<1) +#define SIGNALS_CYCLESTART_BIT (1<<2) +#define SIGNALS_SAFETYDOOR_BIT (1<<3) +#define SIGNALS_BLOCKDELETE_BIT (1<<4) +#define SIGNALS_STOPDISABLE_BIT (1<<5) +#define SIGNALS_ESTOP_BIT (1<<6) +#define SIGNALS_PROBE_CONNECTED_BIT (1<<7) +#define SIGNALS_MOTOR_FAULT_BIT (1<<8) +#define SIGNALS_BITMASK (SIGNALS_RESET_BIT|SIGNALS_FEEDHOLD_BIT|SIGNALS_CYCLESTART_BIT|SIGNALS_SAFETYDOOR_BIT|SIGNALS_BLOCKDELETE_BIT|SIGNALS_STOPDISABLE_BIT|SIGNALS_ESTOP_BIT|SIGNALS_PROBE_CONNECTED_BIT|SIGNALS_MOTOR_FAULT_BIT) +/**/ + +// --------------------------------------------------------------------------------------- +// ADVANCED CONFIGURATION OPTIONS: + +// Enables code for debugging purposes. Not for general use and always in constant flux. +// #define DEBUG // Uncomment to enable. Default disabled. +// #define DEBUGOUT // Uncomment to add HAL entry point for debug output. + +// If spindle RPM is set by high-level commands to a spindle controller (eg. via Modbus) or the driver supports closed loop +// spindle RPM control either uncomment the #define SPINDLE_RPM_CONTROLLED below or add SPINDLE_RPM_CONTROLLED as predefined symbol +// on the compiler command line. This will send spindle speed as a RPM value instead of a PWM value to the driver. +//#define SPINDLE_RPM_CONTROLLED + + +// Some status report data isn't necessary for realtime, only intermittently, because the values don't +// change often. The following macros configures how many times a status report needs to be called before +// the associated data is refreshed and included in the status report. However, if one of these value +// changes, Grbl will automatically include this data in the next status report, regardless of what the +// count is at the time. This helps reduce the communication overhead involved with high frequency reporting +// and agressive streaming. There is also a busy and an idle refresh count, which sets up Grbl to send +// refreshes more often when its not doing anything important. With a good GUI, this data doesn't need +// to be refreshed very often, on the order of a several seconds. +// NOTE: WCO refresh must be 2 or greater. OVERRIDE refresh must be 1 or greater. +//#define REPORT_OVERRIDE_REFRESH_BUSY_COUNT 20 // (1-255) +//#define REPORT_OVERRIDE_REFRESH_IDLE_COUNT 10 // (1-255) Must be less than or equal to the busy count +//#define REPORT_WCO_REFRESH_BUSY_COUNT 30 // (2-255) +//#define REPORT_WCO_REFRESH_IDLE_COUNT 10 // (2-255) Must be less than or equal to the busy count + +// The temporal resolution of the acceleration management subsystem. A higher number gives smoother +// acceleration, particularly noticeable on machines that run at very high feedrates, but may negatively +// impact performance. The correct value for this parameter is machine dependent, so it's advised to +// set this only as high as needed. Approximate successful values can widely range from 50 to 200 or more. +// NOTE: Changing this value also changes the execution time of a segment in the step segment buffer. +// When increasing this value, this stores less overall time in the segment buffer and vice versa. Make +// certain the step segment buffer is increased/decreased to account for these changes. +//#define ACCELERATION_TICKS_PER_SECOND 100 + +// Sets the maximum step rate allowed to be written as a Grbl setting. This option enables an error +// check in the settings module to prevent settings values that will exceed this limitation. The maximum +// step rate is strictly limited by the CPU speed and will change if something other than an AVR running +// at 16MHz is used. +// NOTE: For now disabled, will enable if flash space permits. +//#define MAX_STEP_RATE_HZ 30000 // Hz + +// With this enabled, Grbl sends back an echo of the line it has received, which has been pre-parsed (spaces +// removed, capitalized letters, no comments) and is to be immediately executed by Grbl. Echoes will not be +// sent upon a line buffer overflow, but should for all normal lines sent to Grbl. For example, if a user +// sendss the line 'g1 x1.032 y2.45 (test comment)', Grbl will echo back in the form '[echo: G1X1.032Y2.45]'. +// NOTE: Only use this for debugging purposes!! When echoing, this takes up valuable resources and can effect +// performance. If absolutely needed for normal operation, the serial write buffer should be greatly increased +// to help minimize transmission waiting within the serial write protocol. +//#define REPORT_ECHO_LINE_RECEIVED // Default disabled. Uncomment to enable. + +// Sets which axis the tool length offset is applied. Assumes the spindle is always parallel with +// the selected axis with the tool oriented toward the negative direction. In other words, a positive +// tool length offset value is subtracted from the current location. +//#define TOOL_LENGTH_OFFSET_AXIS Z_AXIS // Default z-axis. Valid values are X_AXIS, Y_AXIS, or Z_AXIS. + +// Minimum planner junction speed. Sets the default minimum junction speed the planner plans to at +// every buffer block junction, except for starting from rest and end of the buffer, which are always +// zero. This value controls how fast the machine moves through junctions with no regard for acceleration +// limits or angle between neighboring block line move directions. This is useful for machines that can't +// tolerate the tool dwelling for a split second, i.e. 3d printers or laser cutters. If used, this value +// should not be much greater than zero or to the minimum value necessary for the machine to work. +//#define MINIMUM_JUNCTION_SPEED 0.0f // (mm/min) + +// Sets the minimum feed rate the planner will allow. Any value below it will be set to this minimum +// value. This also ensures that a planned motion always completes and accounts for any floating-point +// round-off errors. Although not recommended, a lower value than 1.0 mm/min will likely work in smaller +// machines, perhaps to 0.1mm/min, but your success may vary based on multiple factors. +// #define MINIMUM_FEED_RATE 1.0f // (mm/min) + +// Number of arc generation iterations by small angle approximation before exact arc trajectory +// correction with expensive sin() and cos() calculations. This parameter maybe decreased if there +// are issues with the accuracy of the arc generations, or increased if arc execution is getting +// bogged down by too many trig calculations. +//#define N_ARC_CORRECTION 12 // Integer (1-255) + +// The arc G2/3 g-code standard is problematic by definition. Radius-based arcs have horrible numerical +// errors when arc at semi-circles(pi) or full-circles(2*pi). Offset-based arcs are much more accurate +// but still have a problem when arcs are full-circles (2*pi). This define accounts for the floating +// point issues when offset-based arcs are commanded as full circles, but get interpreted as extremely +// small arcs with around machine epsilon (1.2e-7rad) due to numerical round-off and precision issues. +// This define value sets the machine epsilon cutoff to determine if the arc is a full-circle or not. +// NOTE: Be very careful when adjusting this value. It should always be greater than 1.2e-7 but not too +// much greater than this. The default setting should capture most, if not all, full arc error situations. +//#define ARC_ANGULAR_TRAVEL_EPSILON 5E-7f // Float (radians) + +// Default constants for G5 Cubic splines +// +//#define BEZIER_MIN_STEP 0.002f +//#define BEZIER_MAX_STEP 0.1f +//#define BEZIER_SIGMA 0.1f + +// Time delay increments performed during a dwell. The default value is set at 50ms, which provides +// a maximum time delay of roughly 55 minutes, more than enough for most any application. Increasing +// this delay will increase the maximum dwell time linearly, but also reduces the responsiveness of +// run-time command executions, like status reports, since these are performed between each dwell +// time step. +//#define DWELL_TIME_STEP 50 // Integer (1-255) (milliseconds) + +// The number of linear motions in the planner buffer to be planned at any give time. The vast +// majority of RAM that Grbl uses is based on this buffer size. Only increase if there is extra +// available RAM, like when re-compiling for MCU with ample amounts of RAM. Or decrease if the MCU begins to +// crash due to the lack of available RAM or if the CPU is having trouble keeping up with planning +// new incoming motions as they are executed. +// #define BLOCK_BUFFER_SIZE 36 // Uncomment to override default in planner.h. + +// Governs the size of the intermediary step segment buffer between the step execution algorithm +// and the planner blocks. Each segment is set of steps executed at a constant velocity over a +// fixed time defined by ACCELERATION_TICKS_PER_SECOND. They are computed such that the planner +// block velocity profile is traced exactly. The size of this buffer governs how much step +// execution lead time there is for other Grbl processes have to compute and do their thing +// before having to come back and refill this buffer, currently at ~50msec of step moves. +// #define SEGMENT_BUFFER_SIZE 10 // Uncomment to override default in stepper.h. + +// Configures the position after a probing cycle during Grbl's check mode. Disabled sets +// the position to the probe target, when enabled sets the position to the start position. +// #define SET_CHECK_MODE_PROBE_TO_START // Default disabled. Uncomment to enable. + +// Force Grbl to check the state of the hard limit switches when the processor detects a pin +// change inside the hard limit ISR routine. By default, Grbl will trigger the hard limits +// alarm upon any pin change, since bouncing switches can cause a state check like this to +// misread the pin. When hard limits are triggered, they should be 100% reliable, which is the +// reason that this option is disabled by default. Only if your system/electronics can guarantee +// that the switches don't bounce, we recommend enabling this option. This will help prevent +// triggering a hard limit when the machine disengages from the switch. +// NOTE: This option has no effect if SOFTWARE_DEBOUNCE is enabled. +// #define HARD_LIMIT_FORCE_STATE_CHECK // Default disabled. Uncomment to enable. + +// Adjusts homing cycle search and locate scalars. These are the multipliers used by Grbl's +// homing cycle to ensure the limit switches are engaged and cleared through each phase of +// the cycle. The search phase uses the axes max-travel setting times the SEARCH_SCALAR to +// determine distance to look for the limit switch. Once found, the locate phase begins and +// uses the homing pull-off distance setting times the LOCATE_SCALAR to pull-off and re-engage +// the limit switch. +// NOTE: Both of these values must be greater than 1.0 to ensure proper function. +// #define HOMING_AXIS_SEARCH_SCALAR 1.5f // Uncomment to override defaults in limits.c. +// #define HOMING_AXIS_LOCATE_SCALAR 10.0f // Uncomment to override defaults in limits.c. + +// Enable the '$RST=*', '$RST=$', and '$RST=#' non-volatile storage restore commands. There are cases where +// these commands may be undesirable. Simply comment the desired macro to disable it. +// NOTE: See SETTINGS_RESTORE_ALL macro for customizing the `$RST=*` command. +//#define DISABLE_RESTORE_NVS_WIPE_ALL // '$RST=*' Default enabled. Uncomment to disable. +//#define DISABLE_RESTORE_NVS_DEFAULT_SETTINGS // '$RST=$' Default enabled. Uncomment to disable. +//#define DISABLE_RESTORE_NVS_CLEAR_PARAMETERS // '$RST=#' Default enabled. Uncomment to disable. +//#define DISABLE_RESTORE_DRIVER_PARAMETERS // '$RST=&' Default enabled. Uncomment to disable. For drivers that implements non-generic settings. + +// Defines the non-volatile data restored upon a settings version change and `$RST=*` command. Whenever the +// the settings or other non-volatile data structure changes between Grbl versions, Grbl will automatically +// wipe and restore the non-volatile data. These macros controls what data is wiped and restored. This is useful +// particularily for OEMs that need to retain certain data. For example, the BUILD_INFO string can be +// written into non-volatile storage via a separate program to contain product data. Altering these +// macros to not restore the build info non-volatile storage will ensure this data is retained after firmware upgrades. +//#define SETTINGS_RESTORE_DEFAULTS 0 // Default enabled, uncomment to disable +//#define SETTINGS_RESTORE_PARAMETERS 0 // Default enabled, uncomment to disable +//#define SETTINGS_RESTORE_STARTUP_LINES 0 // Default enabled, uncomment to disable +//#define SETTINGS_RESTORE_BUILD_INFO 0 // Default enabled, uncomment to disable +//#define SETTINGS_RESTORE_DRIVER_PARAMETERS 0 // Default enabled, uncomment to disable + +// Enable the '$I=(string)' build info write command. If disabled, any existing build info data must +// be placed into non-volatile storage via external means with a valid checksum value. This macro option is useful +// to prevent this data from being over-written by a user, when used to store OEM product data. +// NOTE: If disabled and to ensure Grbl can never alter the build info line, you'll also need to enable +// the SETTING_RESTORE_ALL macro above and remove SETTINGS_RESTORE_BUILD_INFO from the mask. +// NOTE: See the included grblWrite_BuildInfo.ino example file to write this string seperately. +// #define DISABLE_BUILD_INFO_WRITE_COMMAND // '$I=' Default enabled. Uncomment to disable. + +// Enables and configures Grbl's sleep mode feature. If the spindle or coolant are powered and Grbl +// is not actively moving or receiving any commands, a sleep timer will start. If any data or commands +// are received, the sleep timer will reset and restart until the above condition are not satisfied. +// If the sleep timer elaspes, Grbl will immediately execute the sleep mode by shutting down the spindle +// and coolant and entering a safe sleep state. If parking is enabled, Grbl will park the machine as +// well. While in sleep mode, only a hard/soft reset will exit it and the job will be unrecoverable. +// NOTE: Sleep mode is a safety feature, primarily to address communication disconnect problems. To +// keep Grbl from sleeping, employ a stream of '?' status report commands as a connection "heartbeat". +//#define SLEEP_ENABLE // Default disabled. Uncomment to enable. +//#define SLEEP_DURATION 5.0f // Number of minutes before sleep mode is entered. + +// Disable non-volatile storage emulation/buffering in RAM (allocated from heap) +// Can be used for MCUs with no non-volatile storage or as buffer in order to avoid writing to +// non-volatile storage when not in idle state. +// The buffer will be written to non-volatile storage when in idle state. +//#define BUFFER_NVSDATA_DISABLE + +//#define ENABLE_BACKLASH_COMPENSATION + +// End compile time only default configuration + +// When the HAL driver supports spindle sync then this option sets the number of pulses per revolution +// for the spindle encoder. Depending on the driver this may lead to the "spindle at speed" detection +// beeing enabled. When this is enabled grbl will wait for the spindle to reach the programmed speed +// before continue processing. NOTE: Currently there is no timeout for this wait. +// Default value is 0, meaning spindle sync is disabled +//#define DEFAULT_SPINDLE_PPR 0 // Pulses per revolution. Default 0. + +// This option will automatically disable the laser during a feed hold by invoking a spindle stop +// override immediately after coming to a stop. However, this also means that the laser still may +// be reenabled by disabling the spindle stop override, if needed. This is purely a safety feature +// to ensure the laser doesn't inadvertently remain powered while at a stop and cause a fire. +//#define DEFAULT_DISABLE_LASER_DURING_HOLD // Default enabled. Uncomment to disable. + +// This option is for what should happen on resume from feed hold. +// Default action is to restore spindle and coolant status (if overridden), this contradicts the +// behaviour of industrial controllers but is in line with earlier versions of Grbl. +//#define DEFAULT_NO_RESTORE_AFTER_FEED_HOLD // Default enabled. Uncomment to disable. + +// When Grbl powers-cycles or is hard reset with the MCU reset button, Grbl boots up with no ALARM +// by default. This is to make it as simple as possible for new users to start using Grbl. When homing +// is enabled and a user has installed limit switches, Grbl will boot up in an ALARM state to indicate +// Grbl doesn't know its position and to force the user to home before proceeding. This option forces +// Grbl to always initialize into an ALARM state regardless of homing or not. This option is more for +// OEMs and LinuxCNC users that would like this power-cycle behavior. +//#define DEFAULT_FORCE_INITIALIZATION_ALARM // Default disabled. Uncomment to enable. + +// At power-up or a reset, Grbl will check the limit switch states to ensure they are not active +// before initialization. If it detects a problem and the hard limits setting is enabled, Grbl will +// simply message the user to check the limits and enter an alarm state, rather than idle. Grbl will +// not throw an alarm message. +//#define DEFAULT_CHECK_LIMITS_AT_INIT // Default disabled. Uncomment to enable. + +// Configure options for the parking motion, if enabled. +//#define DEFAULT_PARKING_AXIS Z_AXIS // Define which axis that performs the parking motion +//#define DEFAULT_PARKING_TARGET -5.0f // Parking axis target. In mm, as machine coordinate [-max_travel,0]. +//#define DEFAULT_PARKING_RATE 500.0f // Parking fast rate after pull-out in mm/min. +//#define DEFAULT_PARKING_PULLOUT_RATE 100.0f // Pull-out/plunge slow feed rate in mm/min. +//#define DEFAULT_PARKING_PULLOUT_INCREMENT 5.0f // Spindle pull-out and plunge distance in mm. Incremental distance. + // Must be positive value or equal to zero. + + +// Enables a special set of M-code commands that enables and disables the parking motion. +// These are controlled by `M56`, `M56 P1`, or `M56 Px` to enable and `M56 P0` to disable. +// The command is modal and will be set after a planner sync. Since it is g-code, it is +// executed in sync with g-code commands. It is not a real-time command. +// NOTE: PARKING_ENABLE is required. By default, M56 is active upon initialization. Use +// DEACTIVATE_PARKING_UPON_INIT to set M56 P0 as the power-up default. +//#define DEFAULT_ENABLE_PARKING_OVERRIDE_CONTROL // Default disabled. Uncomment to enable +//#define DEFAULT_DEACTIVATE_PARKING_UPON_INIT // Default disabled. Uncomment to enable. + +// Using printable ASCII characters for realtime commands can cause issues with +// files containing such characters in comments or settings. If the GCode sender support the +// use of the top-bit set alternatives for these then they may be disabled. +// NOTE: support for the top-bit set alternatives is always enabled. +// NOTE: when disabled they are still active outside of comments and $ settings +// allowing their use from manual input, eg. from a terminal or MDI. +//#define DEFAULT_NO_LEGACY_RTCOMMANDS // Default disabled. Uncomment to enable. + +//#define DEFAULT_TOOLCHANGE_MODE 0 // 0 = Normal mode, 1 = Manual change, 2 = Manual change @ G59.3, 3 = Manual change and probe sensor @ G59.3 - sets TLO +//#define DEFAULT_TOOLCHANGE_PROBING_DISTANCE 30 // max probing distance in mm for mode 3 +//#define DEFAULT_TOOLCHANGE_FEED_RATE 25.0f // mm/min +//#define DEFAULT_TOOLCHANGE_SEEK_RATE 200.0f // mm/min +//#define DEFAULT_TOOLCHANGE_PULLOFF_RATE 200.0f // mm/min + +// By default, Grbl sets all input pins to normal-low operation with their internal pull-up resistors +// enabled. This simplifies the wiring for users by requiring only a normally closed (NC) switch connected +// to ground. It is not recommended to use normally-open (NO) switches as this increases the risk +// of electrical noise spuriously triggering the inputs. If normally-open (NO) switches are used the +// logic of the input signals should be be inverted with the invert settings below. +// The following options disable the internal pull-up resistors, and switches must be now connect to Vcc +// instead of ground. +// WARNING: When the pull-ups are disabled, this might require additional wiring with pull-down resistors! +// Please check driver code and/or documentation. +// #define DISABLE_LIMIT_PINS_PULL_UP_MASK AXES_BITMASK +// #define DISABLE_LIMIT_PINS_PULL_UP_MASK (X_AXIS_BIT|Y_AXIS_BIT) +// #define DISABLE_CONTROL_PINS_PULL_UP_MASK SIGNALS_BITMASK +// #define DISABLE_CONTROL_PINS_PULL_UP_MASK (SIGNALS_SAFETYDOOR_BIT|SIGNALS_RESET_BIT) +// #define DISABLE_PROBE_PIN_PULL_UP + +// If your machine has two limits switches wired in parallel to one axis, you will need to enable +// this feature. Since the two switches are sharing a single pin, there is no way for Grbl to tell +// which one is enabled. This option only effects homing, where if a limit is engaged, Grbl will +// alarm out and force the user to manually disengage the limit switch. Otherwise, if you have one +// limit switch for each axis, don't enable this option. By keeping it disabled, you can perform a +// homing cycle while on the limit switch and not have to move the machine off of it. +//#define DEFAULT_LIMITS_TWO_SWITCHES_ON_AXES // Default disabled. Uncomment to enable. + +// By default, Grbl disables feed rate overrides for all G38.x probe cycle commands. Although this +// may be different than some pro-class machine control, it's arguable that it should be this way. +// Most probe sensors produce different levels of error that is dependent on rate of speed. By +// keeping probing cycles to their programmed feed rates, the probe sensor should be a lot more +// repeatable. If needed, you can disable this behavior by uncommenting the define below. +//#define ALLOW_FEED_OVERRIDE_DURING_PROBE_CYCLES // Default disabled. Uncomment to enable. + +// Inverts logic of the stepper enable signal(s). +// NOTE: Not universally available for individual axes - check driver documentation. +// Specify at least X_AXIS_BIT if a common enable signal is used. +// #define INVERT_ST_ENABLE_MASK (X_AXIS_BIT|Y_AXIS_BIT|Z_AXIS_BIT) // Default disabled. Uncomment to enable. +// Mask to be OR'ed with stepper disable signal(s). Axes configured will not be disabled. +// NOTE: Not universally available for individual axes - check driver documentation. +// Specify at least X_AXIS_BIT if a common enable signal is used. +//#define ST_DEENERGIZE_MASK (X_AXIS_BIT|Y_AXIS_BIT|Z_AXIS_BIT) // Default disabled. Uncomment to enable. +//#define DEFAULT_STEPPING_INVERT_MASK 0 +//#define DEFAULT_DIRECTION_INVERT_MASK 0 + +// Inverts logic of the input signals based on a mask. This essentially means you are using +// normally-open (NO) switches on the specified pins, rather than the default normally-closed (NC) switches. +// NOTE: The first option will invert all control pins. The second option is an example of +// inverting only a few pins. See the start of this file for other signal definitions. +// #define INVERT_CONTROL_PIN_MASK SIGNALS_BITMASK // Default disabled. Uncomment to enable. +// #define INVERT_CONTROL_PIN_MASK (SIGNALS_SAFETYDOOR_BIT|SIGNALS_RESET_BIT) // Default disabled. Uncomment to enable. +// #define INVERT_LIMIT_PIN_MASK AXES_BITMASK // Default disabled. Uncomment to enable. Uncomment to enable. +// #define INVERT_LIMIT_PIN_MASK (X_AXIS_BIT|Y_AXIS_BIT) // Default disabled. Uncomment to enable. +// For inverting the probe pin use DEFAULT_INVERT_PROBE_PIN in defaults.h + +// Inverts the spindle enable pin from low-disabled/high-enabled to low-enabled/high-disabled. Useful +// for some pre-built electronic boards. +// NOTE: If VARIABLE_SPINDLE is enabled(default), this option has no effect as the PWM output and +// spindle enable are combined to one pin. If you need both this option and spindle speed PWM, +// uncomment the config option USE_SPINDLE_DIR_AS_ENABLE_PIN below. +// #define INVERT_SPINDLE_ENABLE_PIN 1 // Default disabled. Uncomment to enable. +// #define INVERT_SPINDLE_CCW_PIN 1 // Default disabled. Uncomment to enable. NOTE: not supported by all drivers. +// #define INVERT_SPINDLE_PWM_PIN 1 // Default disabled. Uncomment to enable. NOTE: not supported by all drivers. + +// Inverts the selected coolant pin from low-disabled/high-enabled to low-enabled/high-disabled. Useful +// for some pre-built electronic boards. +// #define INVERT_COOLANT_FLOOD_PIN 1 // Default disabled. Uncomment to enable. +// #define INVERT_COOLANT_MIST_PIN 1 // Default disabled. Note: Enable M7 mist coolant in config.h + + +// Used by variable spindle output only. This forces the PWM output to a minimum duty cycle when enabled. +// The PWM pin will still read 0V when the spindle is disabled. Most users will not need this option, but +// it may be useful in certain scenarios. This minimum PWM settings coincides with the spindle rpm minimum +// setting, like rpm max to max PWM. This is handy if you need a larger voltage difference between 0V disabled +// and the voltage set by the minimum PWM for minimum rpm. This difference is 0.02V per PWM value. So, when +// minimum PWM is at 1, only 0.02 volts separate enabled and disabled. At PWM 5, this would be 0.1V. Keep +// in mind that you will begin to lose PWM resolution with increased minimum PWM values, since you have less +// and less range over the total 255 PWM levels to signal different spindle speeds. +// NOTE: Compute duty cycle at the minimum PWM by this equation: (% duty cycle)=(SPINDLE_PWM_MIN_VALUE/255)*100 +//#define DEFAULT_SPINDLE_PWM_MIN_VALUE 5.0f // Default disabled. Uncomment to enable. Must be greater than zero. Integer (1-255). + +// Number of homing cycles performed after when the machine initially jogs to limit switches. +// This help in preventing overshoot and should improve repeatability. This value should be one or +// greater. +//#define DEFAULT_N_HOMING_LOCATE_CYCLE 1 // Integer (1-127) + + +// In Grbl v0.9 and prior, there is an old outstanding bug where the `WPos:` work position reported +// may not correlate to what is executing, because `WPos:` is based on the g-code parser state, which +// can be several motions behind. This option forces the planner buffer to empty, sync, and stop +// motion whenever there is a command that alters the work coordinate offsets `G10,G43.1,G92,G54-59`. +// This is the simplest way to ensure `WPos:` is always correct. Fortunately, it's exceedingly rare +// that any of these commands are used need continuous motions through them. +//#define DEFAULT_NO_FORCE_BUFFER_SYNC_DURING_WCO_CHANGE // Default enabled. Uncomment to disable. + +// Upon a successful probe cycle, this option provides immediately feedback of the probe coordinates +// through an automatically generated message. If disabled, users can still access the last probe +// coordinates through Grbl '$#' print parameters. +//#define DEFAULT_NO_REPORT_PROBE_COORDINATES 1 // Default enabled. Uncomment to disable. + +// The status report change for Grbl v1.1 and after also removed the ability to disable/enable most data +// fields from the report. This caused issues for GUI developers, who've had to manage several scenarios +// and configurations. The increased efficiency of the new reporting style allows for all data fields to +// be sent without potential performance issues. +// NOTE: The options below are here only provide a way to disable certain data fields if a unique +// situation demands it, but be aware GUIs may depend on this data. If disabled, it may not be compatible. +//#define DEFAULT_REPORT_MACHINE_POSITION // Default disabled. Uncomment to enable. + +#if COMPATIBILITY_LEVEL == 0 +// Number of tools in ATC tool table, comment out to disable +// #define N_TOOLS 8 +#endif + +// Max number of entries in log for PID data reporting, to be used for tuning +//#define PID_LOG 1000 // Default disabled. Uncomment to enable. + +//#define DEFAULT_NO_REPORT_BUFFER_STATE +//#define DEFAULT_NO_REPORT_LINE_NUMBERS +//#define DEFAULT_NO_REPORT_CURRENT_FEED_SPEED +//#define DEFAULT_NO_REPORT_PIN_STATE +//#define DEFAULT_NO_REPORT_WORK_COORD_OFFSET +//#define DEFAULT_NO_REPORT_OVERRIDES +//#define DEFAULT_REPORT_PARSER_STATE +//#define DEFAULT_REPORT_ALARM_SUBSTATE + +//#define DEFAULT_X_STEPS_PER_MM 250.0f +//#define DEFAULT_Y_STEPS_PER_MM 250.0f +//#define DEFAULT_Z_STEPS_PER_MM 250.0f +//#define DEFAULT_X_MAX_RATE 500.0f // mm/min +//#define DEFAULT_Y_MAX_RATE 500.0f // mm/min +//#define DEFAULT_Z_MAX_RATE 500.0f // mm/min +//#define DEFAULT_X_ACCELERATION (10.0*60*60) // 10*60*60 mm/min^2 = 10 mm/sec^2 +//#define DEFAULT_Y_ACCELERATION (10.0*60*60) // 10*60*60 mm/min^2 = 10 mm/sec^2 +//#define DEFAULT_Z_ACCELERATION (10.0*60*60) // 10*60*60 mm/min^2 = 10 mm/sec^2 +//#define DEFAULT_X_MAX_TRAVEL 200.0f // mm NOTE: Must be a positive value. +//#define DEFAULT_Y_MAX_TRAVEL 200.0f // mm NOTE: Must be a positive value. +//#define DEFAULT_Z_MAX_TRAVEL 200.0f // mm NOTE: Must be a positive value. +//#define DEFAULT_X_CURRENT 0.0 // amps +//#define DEFAULT_Y_CURRENT 0.0 // amps +//#define DEFAULT_Z_CURRENT 0.0 // amps +//#define DEFAULT_A_CURRENT 0.0 // amps +//#define DEFAULT_SPINDLE_PWM_FREQ 5000 // Hz +//#define DEFAULT_SPINDLE_PWM_OFF_VALUE 0.0f // Percent +//#define DEFAULT_SPINDLE_PWM_MAX_VALUE 100.0f // Percent +//#define DEFAULT_SPINDLE_AT_SPEED_TOLERANCE 0.0f // Percent - 0 means not checked +//#define DEFAULT_SPINDLE_RPM_MAX 1000.0 // rpm +//#define DEFAULT_SPINDLE_RPM_MIN 0.0 // rpm +//#define DEFAULT_SPINDLE_PWM_ACTION 0 // 0 = NONE, 1 = ENABLE_OFF_WITH_ZERO_SPEED, 2 = +//#define DEFAULT_STEP_PULSE_MICROSECONDS 10.0f +//#define DEFAULT_STEP_PULSE_DELAY 5.0f // uncomment to set default > 0.0f +//#define DEFAULT_STEPPER_IDLE_LOCK_TIME 25 // msec (0-65535, 255 keeps steppers enabled) +//#define DEFAULT_JUNCTION_DEVIATION 0.01f // mm +//#define DEFAULT_ARC_TOLERANCE 0.002f // mm +//#define DEFAULT_REPORT_INCHES +//#define DEFAULT_INVERT_LIMIT_PINS +//#define DEFAULT_SOFT_LIMIT_ENABLE +//#define DEFAULT_JOG_LIMIT_ENABLE +//#define DEFAULT_HARD_LIMIT_ENABLE +//#define DEFAULT_INVERT_PROBE_PIN +//#define DEFAULT_LASER_MODE +//#define DEFAULT_LATHE_MODE +//#define DEFAULT_HOMING_ENABLE +//#define DEFAULT_HOMING_ALLOW_MANUAL +//#define DEFAULT_HOMING_DIR_MASK 0 // move positive dir +//#define DEFAULT_HOMING_FEED_RATE 25.0f // mm/min +//#define DEFAULT_HOMING_SEEK_RATE 500.0f // mm/min +//#define DEFAULT_HOMING_DEBOUNCE_DELAY 250 // msec (0-65k) +//#define DEFAULT_HOMING_PULLOFF 1.0f // mm + +//#define DEFAULT_A_STEPS_PER_MM 250.0f +//#define DEFAULT_A_MAX_RATE 500.0f // mm/min +//#define DEFAULT_A_ACCELERATION (10.0*60*60) // 10*60*60 mm/min^2 = 10 mm/sec^2 +//#define DEFAULT_A_MAX_TRAVEL 200.0f // mm + +//#define DEFAULT_B_STEPS_PER_MM 250.0f +//#define DEFAULT_B_MAX_RATE 500.0f // mm/min +//#define DEFAULT_B_ACCELERATION (10.0*60*60) // 10*60*60 mm/min^2 = 10 mm/sec^2 +//#define DEFAULT_B_MAX_TRAVEL 200.0f // mm + +//#define DEFAULT_C_STEPS_PER_MM 250.0f +//#define DEFAULT_C_MAX_RATE 500.0f // mm/min +//#define DEFAULT_C_ACCELERATION (10.0*60*60) // 10*60*60 mm/min^2 = 10 mm/sec^2 +//#define DEFAULT_C_MAX_TRAVEL 200.0f // mm + +//#define DEFAULT_G73_RETRACT 0.1f // mm + +#ifdef DEFAULT_HOMING_ENABLE + +// Number of homing cycles performed after when the machine initially jogs to limit switches. +// This help in preventing overshoot and should improve repeatability. This value should be one or +// greater. +//#define DEFAULT_N_HOMING_LOCATE_CYCLE 1 // Integer (1-127) + +// If homing is enabled, homing init lock sets Grbl into an alarm state upon power up or a soft reset. +// This forces the user to perform the homing cycle before doing anything else. This is +// mainly a safety feature to remind the user to home, since position is unknown to Grbl. +//#define DEFAULT_HOMING_INIT_LOCK // Default disabled. Uncomment to enable. + +// If homing init lock is enabled this sets Grbl into an alarm state upon power up or a soft reset. +// To allow a soft reset to override the lock uncomment the line below. +//#define DEFAULT_HOMING_OVERRIDE_LOCKS // Default disabled. Uncomment to enable. + +// Define the homing cycle patterns with bitmasks. The homing cycle first performs a search mode +// to quickly engage the limit switches, followed by a slower locate mode, and finished by a short +// pull-off motion to disengage the limit switches. The following HOMING_CYCLE_x defines are executed +// in order starting with suffix 0 and completes the homing routine for the specified-axes only. If +// an axis is omitted from the defines, it will not home, nor will the system update its position. +// Meaning that this allows for users with non-standard cartesian machines, such as a lathe (x then z, +// with no y), to configure the homing cycle behavior to their needs. +// NOTE: The homing cycle is designed to allow sharing of limit pins, if the axes are not in the same +// cycle, but this requires some pin settings changes in cpu_map.h file. For example, the default homing +// cycle can share the Z limit pin with either X or Y limit pins, since they are on different cycles. +// By sharing a pin, this frees up a precious IO pin for other purposes. In theory, all axes limit pins +// may be reduced to one pin, if all axes are homed with seperate cycles, or vice versa, all three axes +// on separate pin, but homed in one cycle. Also, it should be noted that the function of hard limits +// will not be affected by pin sharing. +// NOTE: Defaults are set for a traditional 3-axis CNC machine. Z-axis first to clear, followed by X & Y. + +//#define HOMING_CYCLE_0 (Z_AXIS_BIT) // REQUIRED: First move Z to clear workspace. +//#define HOMING_CYCLE_1 (X_AXIS_BIT|Y_AXIS_BIT) // OPTIONAL: Then move X,Y at the same time. +//#define HOMING_CYCLE_2 0 // OPTIONAL: Uncomment and add axes mask to enable +#if N_AXIS > 3 +//#define HOMING_CYCLE_3 0 // OPTIONAL: Uncomment and add axes mask to enable +//#define HOMING_CYCLE_4 0 // OPTIONAL: Uncomment and add axes mask to enable +//#define HOMING_CYCLE_5 0 // OPTIONAL: Uncomment and add axes mask to enable +#endif + +// Enables single axis homing commands. $HX, $HY, and $HZ for X, Y, and Z-axis homing. The full homing +// cycle is still invoked by the $H command. This is disabled by default. It's here only to address +// users that need to switch between a two-axis and three-axis machine. This is actually very rare. +// If you have a two-axis machine, DON'T USE THIS. Instead, just alter the homing cycle for two-axes. +//#define HOMING_SINGLE_AXIS_COMMANDS // Default disabled. Uncomment to enable. + +// After homing, Grbl will set by default the entire machine space into negative space, as is typical +// for professional CNC machines, regardless of where the limit switches are located. Set this +// define to 1 to force Grbl to always set the machine origin at the homed location despite switch orientation. +//#define HOMING_FORCE_SET_ORIGIN // Default disabled. Uncomment to enable. + +// To prevent the homing cycle from racking the dual axis, when one limit triggers before the +// other due to switch failure or noise, the homing cycle will automatically abort if the second +// motor's limit switch does not trigger within the three distance parameters defined below. +// Axis length percent will automatically compute a fail distance as a percentage of the max +// travel of the other non-dual axis, i.e. if dual axis select is X_AXIS at 5.0%, then the fail +// distance will be computed as 5.0% of y-axis max travel. Fail distance max and min are the +// limits of how far or little a valid fail distance is. +//#define DUAL_AXIS_HOMING_FAIL_AXIS_LENGTH_PERCENT 5.0f // Float (percent) +//#define DUAL_AXIS_HOMING_FAIL_DISTANCE_MAX 25.0f // Float (mm) +//#define DUAL_AXIS_HOMING_FAIL_DISTANCE_MIN 2.5f // Float (mm) + +// Enables and configures parking motion methods upon a safety door state. Primarily for OEMs +// that desire this feature for their integrated machines. At the moment, Grbl assumes that +// the parking motion only involves one axis, although the parking implementation was written +// to be easily refactored for any number of motions on different axes by altering the parking +// source code. At this time, Grbl only supports parking one axis (typically the Z-axis) that +// moves in the positive direction upon retracting and negative direction upon restoring position. +// The motion executes with a slow pull-out retraction motion, power-down, and a fast park. +// Restoring to the resume position follows these set motions in reverse: fast restore to +// pull-out position, power-up with a time-out, and plunge back to the original position at the +// slower pull-out rate. +// NOTE: Still a work-in-progress. Machine coordinates must be in all negative space and +// does not work with HOMING_FORCE_SET_ORIGIN enabled. Parking motion also moves only in +// positive direction. +//#define DEFAULT_PARKING_ENABLE // Default disabled. Uncomment to enable. + +// End default values for run time configurable settings + +#endif // DEFAULT_HOMING_ENABLE + +#endif diff --git a/coolant_control.c b/coolant_control.c new file mode 100644 index 0000000..ef4b6cc --- /dev/null +++ b/coolant_control.c @@ -0,0 +1,55 @@ +/* + coolant_control.c - coolant control methods + + Part of grblHAL + + Copyright (c) 2016-2021 Terje Io + Copyright (c) 2012-2016 Sungeun K. Jeon for Gnea Research LLC + + Grbl is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + Grbl is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with Grbl. If not, see . +*/ + +#include +#include + +#include "hal.h" +#include "protocol.h" +#include "coolant_control.h" +#include "state_machine.h" + +// Main program only. Immediately sets flood coolant running state and also mist coolant, +// if enabled. Also sets a flag to report an update to a coolant state. +// Called by coolant toggle override, parking restore, parking retract, sleep mode, g-code +// parser program end, and g-code parser coolant_sync(). +void coolant_set_state (coolant_state_t mode) +{ + if (!ABORTED) { // Block during abort. + hal.coolant.set_state(mode); + sys.report.coolant = On; // Set to report change immediately + } +} + + +// G-code parser entry-point for setting coolant state. Forces a planner buffer sync and bails +// if an abort or check-mode is active. +bool coolant_sync (coolant_state_t mode) +{ + bool ok = true; + if (state_get() != STATE_CHECK_MODE) { + if((ok = protocol_buffer_synchronize())) // Ensure coolant changes state when specified in program. + coolant_set_state(mode); + } + + return ok; +} diff --git a/coolant_control.h b/coolant_control.h new file mode 100644 index 0000000..e744274 --- /dev/null +++ b/coolant_control.h @@ -0,0 +1,46 @@ +/* + coolant_control.h - spindle control methods + + Part of grblHAL + + Copyright (c) 2012-2016 Sungeun K. Jeon for Gnea Research LLC + + Grbl is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + Grbl is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with Grbl. If not, see . +*/ + +#ifndef _COOLANT_CONTROL_H_ +#define _COOLANT_CONTROL_H_ + +typedef union { + uint8_t value; + uint8_t mask; + struct { + uint8_t flood :1, + mist :1, + shower :1, + trough_spindle :1, + reserved4 :1, + reserved5 :1, + reserved6 :1, + reserved7 :1; + }; +} coolant_state_t; + +// Sets the coolant pins according to state specified. +void coolant_set_state(coolant_state_t mode); + +// G-code parser entry-point for setting coolant states. Checks for and executes additional conditions. +bool coolant_sync(coolant_state_t mode); + +#endif diff --git a/corexy.c b/corexy.c new file mode 100644 index 0000000..2780e41 --- /dev/null +++ b/corexy.c @@ -0,0 +1,166 @@ +/* + corexy.c - corexy kinematics implementation + + Part of grblHAL + + Copyright (c) 2019 Terje Io + Copyright (c) 2011-2016 Sungeun K. Jeon for Gnea Research LLC + + Grbl is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + Grbl is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with Grbl. If not, see . +*/ + +#include "grbl.h" + +#ifdef COREXY + +#include "settings.h" +#include "planner.h" +#include "kinematics.h" + +// CoreXY motor assignments. DO NOT ALTER. +// NOTE: If the A and B motor axis bindings are changed, this effects the CoreXY equations. +#define A_MOTOR X_AXIS // Must be X_AXIS +#define B_MOTOR Y_AXIS // Must be Y_AXIS + +// Returns x or y-axis "steps" based on CoreXY motor steps. +inline static int32_t corexy_convert_to_a_motor_steps (int32_t *steps) +{ + return (steps[A_MOTOR] + steps[B_MOTOR]) >> 1; +} + +inline static int32_t corexy_convert_to_b_motor_steps (int32_t *steps) +{ + return (steps[A_MOTOR] - steps[B_MOTOR]) >> 1; +} + +// Returns machine position of axis 'idx'. Must be sent a 'step' array. +static void corexy_convert_array_steps_to_mpos (float *position, int32_t *steps) +{ + 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[Z_AXIS] = steps[Z_AXIS] / settings.axis[Z_AXIS].steps_per_mm; +} + +// Transform absolute position from cartesian coordinate system (mm) to corexy coordinate system (step) +static void corexy_target_to_steps (int32_t *target_steps, float *target) +{ + uint_fast8_t idx = N_AXIS; + int32_t a_steps, b_steps; + + do { + switch(--idx) { + case X_AXIS: + a_steps = lroundf(target[idx] * settings.axis[idx].steps_per_mm); + break; + + case Y_AXIS: + b_steps = lroundf(target[idx] * settings.axis[idx].steps_per_mm); + break; + + default: + target_steps[idx] = lroundf(target[idx] * settings.axis[idx].steps_per_mm); + break; + } + } while(idx); + + target_steps[A_MOTOR] = a_steps + b_steps; + target_steps[B_MOTOR] = a_steps - b_steps; +} + +static uint_fast8_t corexy_limits_get_axis_mask (uint_fast8_t idx) +{ + return ((idx == A_MOTOR) || (idx == B_MOTOR)) ? (bit(X_AXIS) | bit(Y_AXIS)) : bit(idx); +} + + +static void corexy_limits_set_target_pos (uint_fast8_t idx) // fn name? +{ + int32_t axis_position; + + switch(idx) { + case X_AXIS: + axis_position = corexy_convert_to_b_motor_steps(sys.position); + sys.position[A_MOTOR] = axis_position; + sys.position[B_MOTOR] = -axis_position; + break; + case Y_AXIS: + sys.position[A_MOTOR] = sys.position[B_MOTOR] = corexy_convert_to_a_motor_steps(sys.position); + break; + default: + sys.position[idx] = 0; + break; + } +} + + +// Set machine positions for homed limit switches. Don't update non-homed axes. +// NOTE: settings.max_travel[] is stored as a negative value. +static void corexy_limits_set_machine_positions (axes_signals_t cycle) +{ + uint_fast8_t idx = N_AXIS; + + if(settings.homing.flags.force_set_origin) { + if (cycle.mask & bit(--idx)) do { + switch(--idx) { + case X_AXIS: + sys.position[A_MOTOR] = corexy_convert_to_b_motor_steps(sys.position); + sys.position[B_MOTOR] = - sys.position[A_MOTOR]; + break; + case Y_AXIS: + sys.position[A_MOTOR] = corexy_convert_to_a_motor_steps(sys.position); + sys.position[B_MOTOR] = sys.position[A_MOTOR]; + break; + default: + sys.position[idx] = 0; + break; + } + } while (idx); + } else do { + if (cycle.mask & bit(--idx)) { + int32_t off_axis_position; + int32_t set_axis_position = bit_istrue(settings.homing.dir_mask.value, bit(idx)) + ? lroundf((settings.axis[idx].max_travel + settings.homing.pulloff) * settings.axis[idx].steps_per_mm) + : lroundf(-settings.homing.pulloff * settings.axis[idx].steps_per_mm); + switch(idx) { + case X_AXIS: + off_axis_position = corexy_convert_to_b_motor_steps(sys.position); + sys.position[A_MOTOR] = set_axis_position + off_axis_position; + sys.position[B_MOTOR] = set_axis_position - off_axis_position; + break; + case Y_AXIS: + off_axis_position = corexy_convert_to_a_motor_steps(sys.position); + sys.position[A_MOTOR] = off_axis_position + set_axis_position; + sys.position[B_MOTOR] = off_axis_position - set_axis_position; + break; + default: + sys.position[idx] = set_axis_position; + break; + } + } + } while(idx); +} + + +// Initialize API pointers for CoreXY kinematics +void corexy_init (void) +{ + kinematics.limits_set_target_pos = corexy_limits_set_target_pos; + kinematics.limits_get_axis_mask = corexy_limits_get_axis_mask; + kinematics.limits_set_machine_positions = corexy_limits_set_machine_positions; + kinematics.plan_target_to_steps = corexy_target_to_steps; + kinematics.convert_array_steps_to_mpos = corexy_convert_array_steps_to_mpos; +} + +#endif + diff --git a/corexy.h b/corexy.h new file mode 100644 index 0000000..7d2b00f --- /dev/null +++ b/corexy.h @@ -0,0 +1,29 @@ +/* + corexy.c - corexy kinematics implementation + + Part of grblHAL + + Copyright (c) 2019 Terje Io + Copyright (c) 2011-2016 Sungeun K. Jeon for Gnea Research LLC + + Grbl is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + Grbl is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with Grbl. If not, see . +*/ + +#ifndef _COREXY_H_ +#define _COREXY_H_ + +// Initialize HAL pointers for CoreXY kinematics +void corexy_init (void); + +#endif diff --git a/crossbar.h b/crossbar.h new file mode 100644 index 0000000..1b650b2 --- /dev/null +++ b/crossbar.h @@ -0,0 +1,42 @@ +/* + crossbar.h - signal crossbar definitions + + Not used by the core. + + Part of grblHAL + + Copyright (c) 2021 Terje Io + + Grbl is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + Grbl is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with Grbl. If not, see . +*/ + +#ifndef _CROSSBAR_H_ +#define _CROSSBAR_H_ + +typedef bool (*xbar_get_value_ptr)(void); +typedef void (*xbar_set_value_ptr)(bool on); +typedef void (*xbar_event_ptr)(bool on); +typedef void (*xbar_config_ptr)(void *cfg_data); + +typedef struct { + uint32_t function; + void *port; + uint8_t bit; + xbar_config_ptr config; + xbar_get_value_ptr get_value; + xbar_set_value_ptr set_value; + xbar_event_ptr on_event; +} xbar_t; + +#endif diff --git a/defaults.h b/defaults.h new file mode 100644 index 0000000..74549ec --- /dev/null +++ b/defaults.h @@ -0,0 +1,599 @@ +/* + defaults.h - defaults settings configuration file + + Do NOT modify this file unless adding settings! + Changes to these default values should be made in config.h or by declaring compiler symbols. + + NOTE: This file is only used by settings.c and should NOT be referenced elsewhere! + + Part of grblHAL + + Copyright (c) 2017-2020 Terje Io + Copyright (c) 2012-2016 Sungeun K. Jeon for Gnea Research LLC + + Grbl is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + Grbl is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with Grbl. If not, see . +*/ + +#ifndef _DEFAULTS_H_ +#define _DEFAULTS_H_ + +#include "config.h" + +#ifndef BUILD_INFO +#define BUILD_INFO "" +#endif + +// Note: DEFAULT_ACCELERATION is only referenced in this file +#define DEFAULT_ACCELERATION (10.0f * 60.0f * 60.0f) // 10*60*60 mm/min^2 = 10 mm/sec^2 + +#ifdef DEFAULT_REPORT_MACHINE_POSITION +#undef DEFAULT_REPORT_MACHINE_POSITION +#define DEFAULT_REPORT_MACHINE_POSITION 1 +#else +#define DEFAULT_REPORT_MACHINE_POSITION 0 +#endif + +#ifdef DEFAULT_NO_REPORT_BUFFER_STATE +#undef DEFAULT_NO_REPORT_BUFFER_STATE +#define DEFAULT_REPORT_BUFFER_STATE 0 +#else +#define DEFAULT_REPORT_BUFFER_STATE 1 +#endif + +#ifdef DEFAULT_NO_REPORT_LINE_NUMBERS +#undef DEFAULT_NO_REPORT_LINE_NUMBERS +#define DEFAULT_REPORT_LINE_NUMBERS 0 +#else +#define DEFAULT_REPORT_LINE_NUMBERS 1 +#endif + +#ifdef DEFAULT_NO_REPORT_CURRENT_FEED_SPEED +#undef DEFAULT_NO_REPORT_CURRENT_FEED_SPEED +#define DEFAULT_REPORT_CURRENT_FEED_SPEED 0 +#else +#define DEFAULT_REPORT_CURRENT_FEED_SPEED 1 +#endif + +#ifdef DEFAULT_NO_REPORT_PIN_STATE +#undef DEFAULT_NO_REPORT_PIN_STATE +#define DEFAULT_REPORT_PIN_STATE 0 +#else +#define DEFAULT_REPORT_PIN_STATE 1 +#endif + +#ifdef DEFAULT_NO_REPORT_WORK_COORD_OFFSET +#undef DEFAULT_NO_REPORT_WORK_COORD_OFFSET +#define DEFAULT_REPORT_WORK_COORD_OFFSET 0 +#else +#define DEFAULT_REPORT_WORK_COORD_OFFSET 1 +#endif + +#ifdef DEFAULT_NO_REPORT_OVERRIDES +#undef DEFAULT_NO_REPORT_OVERRIDES +#define DEFAULT_REPORT_OVERRIDES 0 +#else +#define DEFAULT_REPORT_OVERRIDES 1 +#endif + +#ifdef DEFAULT_NO_REPORT_PROBE_COORDINATES +#undef DEFAULT_NO_REPORT_PROBE_COORDINATES +#define DEFAULT_REPORT_PROBE_COORDINATES 0 +#else +#define DEFAULT_REPORT_PROBE_COORDINATES 1 +#endif + +#ifdef DEFAULT_NO_FORCE_BUFFER_SYNC_DURING_WCO_CHANGE +#undef DEFAULT_NO_FORCE_BUFFER_SYNC_DURING_WCO_CHANGE +#define DEFAULT_REPORT_SYNC_ON_WCO_CHANGE 0 +#else +#define DEFAULT_REPORT_SYNC_ON_WCO_CHANGE 1 +#endif + +#ifdef DEFAULT_REPORT_PARSER_STATE +#undef DEFAULT_REPORT_PARSER_STATE +#define DEFAULT_REPORT_PARSER_STATE 1 +#else +#define DEFAULT_REPORT_PARSER_STATE 0 +#endif + +#ifdef DEFAULT_REPORT_ALARM_SUBSTATE +#undef DEFAULT_REPORT_ALARM_SUBSTATE +#define DEFAULT_REPORT_ALARM_SUBSTATE 1 +#else +#define DEFAULT_REPORT_ALARM_SUBSTATE 0 +#endif + +#ifndef DEFAULT_X_STEPS_PER_MM +#define DEFAULT_X_STEPS_PER_MM 250.0f +#endif +#ifndef DEFAULT_Y_STEPS_PER_MM +#define DEFAULT_Y_STEPS_PER_MM 250.0f +#endif +#ifndef DEFAULT_Z_STEPS_PER_MM +#define DEFAULT_Z_STEPS_PER_MM 250.0f +#endif +#ifndef DEFAULT_X_MAX_RATE +#define DEFAULT_X_MAX_RATE 500.0f +#endif +#ifndef DEFAULT_Y_MAX_RATE +#define DEFAULT_Y_MAX_RATE 500.0f +#endif +#ifndef DEFAULT_Z_MAX_RATE +#define DEFAULT_Z_MAX_RATE 500.0f +#endif +#ifndef DEFAULT_X_ACCELERATION +#define DEFAULT_X_ACCELERATION DEFAULT_ACCELERATION +#endif +#ifndef DEFAULT_Y_ACCELERATION +#define DEFAULT_Y_ACCELERATION DEFAULT_ACCELERATION +#endif +#ifndef DEFAULT_Z_ACCELERATION +#define DEFAULT_Z_ACCELERATION DEFAULT_ACCELERATION +#endif +#ifndef DEFAULT_X_MAX_TRAVEL +#define DEFAULT_X_MAX_TRAVEL 200.0f +#endif +#ifndef DEFAULT_Y_MAX_TRAVEL +#define DEFAULT_Y_MAX_TRAVEL 200.0f +#endif +#ifndef DEFAULT_Z_MAX_TRAVEL +#define DEFAULT_Z_MAX_TRAVEL 200.0f +#endif +#ifndef DEFAULT_X_CURRENT +#define DEFAULT_X_CURRENT 0.0 +#endif +#ifndef DEFAULT_Y_CURRENT +#define DEFAULT_Y_CURRENT 0.0 +#endif +#ifndef DEFAULT_Z_CURRENT +#define DEFAULT_Z_CURRENT 0.0 +#endif +#ifndef DEFAULT_A_CURRENT +#define DEFAULT_A_CURRENT 0.0 +#endif +#ifndef DEFAULT_SPINDLE_PWM_FREQ +#define DEFAULT_SPINDLE_PWM_FREQ 5000 +#endif +#ifndef DEFAULT_SPINDLE_PWM_OFF_VALUE +#define DEFAULT_SPINDLE_PWM_OFF_VALUE 0.0f +#endif +#ifndef DEFAULT_SPINDLE_PWM_MIN_VALUE +#define DEFAULT_SPINDLE_PWM_MIN_VALUE 0.0f +#endif +#ifndef DEFAULT_SPINDLE_PWM_MAX_VALUE +#define DEFAULT_SPINDLE_PWM_MAX_VALUE 100.0f +#endif +#ifndef DEFAULT_SPINDLE_AT_SPEED_TOLERANCE +#define DEFAULT_SPINDLE_AT_SPEED_TOLERANCE 0.0f +#endif +#ifndef DEFAULT_SPINDLE_RPM_MAX +#define DEFAULT_SPINDLE_RPM_MAX 1000.0 +#endif +#ifndef DEFAULT_SPINDLE_RPM_MIN +#define DEFAULT_SPINDLE_RPM_MIN 0.0 +#endif +#ifndef DEFAULT_SPINDLE_PWM_ACTION +#define DEFAULT_SPINDLE_PWM_ACTION 0 +#endif +#ifndef DEFAULT_STEP_PULSE_MICROSECONDS +#define DEFAULT_STEP_PULSE_MICROSECONDS 10.0f +#endif +#ifndef DEFAULT_STEP_PULSE_DELAY +#define DEFAULT_STEP_PULSE_DELAY 0.0f +#endif +#ifndef DEFAULT_STEPPING_INVERT_MASK +#define DEFAULT_STEPPING_INVERT_MASK 0 +#endif +#ifndef DEFAULT_DIRECTION_INVERT_MASK +#define DEFAULT_DIRECTION_INVERT_MASK 0 +#endif +#ifndef DEFAULT_STEPPER_IDLE_LOCK_TIME +#define DEFAULT_STEPPER_IDLE_LOCK_TIME 25 +#endif +#ifndef DEFAULT_JUNCTION_DEVIATION +#define DEFAULT_JUNCTION_DEVIATION 0.01f +#endif +#ifndef DEFAULT_ARC_TOLERANCE +#define DEFAULT_ARC_TOLERANCE 0.002f +#endif + +#ifdef DEFAULT_INVERT_LIMIT_PINS +#undef DEFAULT_INVERT_LIMIT_PINS +#define INVERT_LIMIT_PIN_MASK AXES_BITMASK +#endif + +#ifdef DEFAULT_REPORT_INCHES +#undef DEFAULT_REPORT_INCHES +#define DEFAULT_REPORT_INCHES 1 +#else +#define DEFAULT_REPORT_INCHES 0 +#endif + +#ifdef DEFAULT_SOFT_LIMIT_ENABLE +#undef DEFAULT_SOFT_LIMIT_ENABLE +#define DEFAULT_SOFT_LIMIT_ENABLE 1 +#else +#define DEFAULT_SOFT_LIMIT_ENABLE 0 +#endif + +#ifdef DEFAULT_JOG_LIMIT_ENABLE +#undef DEFAULT_JOG_LIMIT_ENABLE +#define DEFAULT_JOG_LIMIT_ENABLE 1 +#else +#define DEFAULT_JOG_LIMIT_ENABLE 0 +#endif + +#ifdef DEFAULT_HARD_LIMIT_ENABLE +#undef DEFAULT_HARD_LIMIT_ENABLE +#define DEFAULT_HARD_LIMIT_ENABLE 1 +#else +#define DEFAULT_HARD_LIMIT_ENABLE 0 +#endif + +#ifdef DEFAULT_INVERT_PROBE_PIN +#undef DEFAULT_INVERT_PROBE_PIN +#define DEFAULT_INVERT_PROBE_PIN 1 +#else +#define DEFAULT_INVERT_PROBE_PIN 0 +#endif + +#ifndef DEFAULT_A_STEPS_PER_MM +#define DEFAULT_A_STEPS_PER_MM 250.0f +#endif +#ifndef DEFAULT_A_MAX_RATE +#define DEFAULT_A_MAX_RATE 500.0f +#endif +#ifndef DEFAULT_A_ACCELERATION +#define DEFAULT_A_ACCELERATION DEFAULT_ACCELERATION +#endif +#ifndef DEFAULT_A_MAX_TRAVEL +#define DEFAULT_A_MAX_TRAVEL 200.0f +#endif + +#ifndef DEFAULT_B_STEPS_PER_MM +#define DEFAULT_B_STEPS_PER_MM 250.0f +#endif +#ifndef DEFAULT_B_MAX_RATE +#define DEFAULT_B_MAX_RATE 500.0f +#endif +#ifndef DEFAULT_B_ACCELERATION +#define DEFAULT_B_ACCELERATION DEFAULT_ACCELERATION +#endif +#ifndef DEFAULT_B_MAX_TRAVEL +#define DEFAULT_B_MAX_TRAVEL 200.0f +#endif + +#ifndef DEFAULT_C_STEPS_PER_MM +#define DEFAULT_C_STEPS_PER_MM 250.0f +#endif +#ifndef DEFAULT_C_MAX_RATE +#define DEFAULT_C_MAX_RATE 500.0f +#endif +#ifndef DEFAULT_C_ACCELERATION +#define DEFAULT_C_ACCELERATION DEFAULT_ACCELERATION +#endif +#ifndef DEFAULT_C_MAX_TRAVEL +#define DEFAULT_C_MAX_TRAVEL 200.0f +#endif + +#ifndef DEFAULT_G73_RETRACT +#define DEFAULT_G73_RETRACT 0.1f +#endif + +#ifdef DEFAULT_LASER_MODE +#undef DEFAULT_LASER_MODE +#define DEFAULT_LASER_MODE 1 +#else +#define DEFAULT_LASER_MODE 0 +#endif + +#ifdef DEFAULT_LATHE_MODE +#undef DEFAULT_LATHE_MODE +#define DEFAULT_LATHE_MODE 1 +#else +#define DEFAULT_LATHE_MODE 0 +#endif + +#if DEFAULT_LASER_MODE && DEFAULT_LATHE_MODE +#error "Cannot enable laser and lathe mode at the same time!" +#endif + +#ifndef DEFAULT_SPINDLE_PPR +#define DEFAULT_SPINDLE_PPR 0. +#endif + +#ifndef DEFAULT_SPINDLE_P_GAIN +#define DEFAULT_SPINDLE_P_GAIN 1.0f +#endif +#ifndef DEFAULT_SPINDLE_I_GAIN +#define DEFAULT_SPINDLE_I_GAIN 0.01f +#endif +#ifndef DEFAULT_SPINDLE_D_GAIN +#define DEFAULT_SPINDLE_D_GAIN 0.0f +#endif +#ifndef DEFAULT_SPINDLE_I_MAX +#define DEFAULT_SPINDLE_I_MAX 10.0f +#endif + +#ifdef DEFAULT_SLEEP_ENABLE +#undef DEFAULT_SLEEP_ENABLE +#define DEFAULT_SLEEP_ENABLE 1 +#else +#define DEFAULT_SLEEP_ENABLE 0 +#endif + +#ifdef DEFAULT_DISABLE_LASER_DURING_HOLD +#undef DEFAULT_DISABLE_LASER_DURING_HOLD +#define DEFAULT_DISABLE_LASER_DURING_HOLD 0 +#else +#define DEFAULT_DISABLE_LASER_DURING_HOLD 1 +#endif + + +#ifdef DEFAULT_LIMITS_TWO_SWITCHES_ON_AXES +#undef DEFAULT_LIMITS_TWO_SWITCHES_ON_AXES +#define DEFAULT_LIMITS_TWO_SWITCHES_ON_AXES 1 +#else +#define DEFAULT_LIMITS_TWO_SWITCHES_ON_AXES 0 +#endif + +#ifdef DEFAULT_NO_RESTORE_AFTER_FEED_HOLD +#define DEFAULT_RESTORE_AFTER_FEED_HOLD 0 +#undef DEFAULT_NO_RESTORE_AFTER_FEED_HOLD +#else +#define DEFAULT_RESTORE_AFTER_FEED_HOLD 1 +#endif + +#ifdef DEFAULT_FORCE_INITIALIZATION_ALARM +#undef DEFAULT_FORCE_INITIALIZATION_ALARM +#define DEFAULT_FORCE_INITIALIZATION_ALARM 1 +#else +#define DEFAULT_FORCE_INITIALIZATION_ALARM 0 +#endif + +#ifdef DEFAULT_CHECK_LIMITS_AT_INIT +#undef DEFAULT_CHECK_LIMITS_AT_INIT +#define DEFAULT_CHECK_LIMITS_AT_INIT 1 +#else +#define DEFAULT_CHECK_LIMITS_AT_INIT 0 +#endif + +#ifdef DEFAULT_HOMING_INIT_LOCK +#undef DEFAULT_HOMING_INIT_LOCK +#define DEFAULT_HOMING_INIT_LOCK 1 +#else +#define DEFAULT_HOMING_INIT_LOCK 0 +#endif + +#ifdef HOMING_SINGLE_AXIS_COMMANDS +#undef HOMING_SINGLE_AXIS_COMMANDS +#define HOMING_SINGLE_AXIS_COMMANDS 1 +#else +#define HOMING_SINGLE_AXIS_COMMANDS 0 +#endif + +#ifdef HOMING_FORCE_SET_ORIGIN +#undef HOMING_FORCE_SET_ORIGIN +#define HOMING_FORCE_SET_ORIGIN 1 +#else +#define HOMING_FORCE_SET_ORIGIN 0 +#endif + +#ifdef DEFAULT_HOMING_OVERRIDE_LOCKS +#undef DEFAULT_HOMING_OVERRIDE_LOCKS +#define DEFAULT_HOMING_OVERRIDE_LOCKS 1 +#else +#define DEFAULT_HOMING_OVERRIDE_LOCKS 0 +#endif + +#ifndef DUAL_AXIS_HOMING_FAIL_AXIS_LENGTH_PERCENT +#define DUAL_AXIS_HOMING_FAIL_AXIS_LENGTH_PERCENT 5.0f +#endif +#ifndef DUAL_AXIS_HOMING_FAIL_DISTANCE_MAX +#define DUAL_AXIS_HOMING_FAIL_DISTANCE_MAX 25.0f // Float (mm) +#endif +#ifndef DUAL_AXIS_HOMING_FAIL_DISTANCE_MIN +#define DUAL_AXIS_HOMING_FAIL_DISTANCE_MIN 2.5f // Float (mm) +#endif + +#ifdef DEFAULT_PARKING_ENABLE +#undef DEFAULT_PARKING_ENABLE +#define DEFAULT_PARKING_ENABLE 1 +#else +#define DEFAULT_PARKING_ENABLE 0 +#endif + +#ifndef DEFAULT_PARKING_AXIS +#define DEFAULT_PARKING_AXIS Z_AXIS +#endif +#ifndef DEFAULT_PARKING_TARGET +#define DEFAULT_PARKING_TARGET -5.0f +#endif +#ifndef DEFAULT_PARKING_RATE +#define DEFAULT_PARKING_RATE 500.0f +#endif +#ifndef DEFAULT_PARKING_PULLOUT_RATE +#define DEFAULT_PARKING_PULLOUT_RATE 100.0f +#endif +#ifndef DEFAULT_PARKING_PULLOUT_INCREMENT +#define DEFAULT_PARKING_PULLOUT_INCREMENT 5.0f +#endif + +#ifdef DEFAULT_ENABLE_PARKING_OVERRIDE_CONTROL +#undef DEFAULT_ENABLE_PARKING_OVERRIDE_CONTROL +#define DEFAULT_ENABLE_PARKING_OVERRIDE_CONTROL 1 +#else +#define DEFAULT_ENABLE_PARKING_OVERRIDE_CONTROL 0 +#endif + +#ifdef DEFAULT_DEACTIVATE_PARKING_UPON_INIT +#undef DEFAULT_DEACTIVATE_PARKING_UPON_INIT +#define DEFAULT_DEACTIVATE_PARKING_UPON_INIT 1 +#else +#define DEFAULT_DEACTIVATE_PARKING_UPON_INIT 0 +#endif + +#ifdef ALLOW_FEED_OVERRIDE_DURING_PROBE_CYCLES +#undef ALLOW_FEED_OVERRIDE_DURING_PROBE_CYCLES +#define ALLOW_FEED_OVERRIDE_DURING_PROBE_CYCLES 1 +#else +#define ALLOW_FEED_OVERRIDE_DURING_PROBE_CYCLES 0 +#endif + +#ifdef DEFAULT_NO_LEGACY_RTCOMMANDS +#undef DEFAULT_NO_LEGACY_RTCOMMANDS +#define DEFAULT_LEGACY_RTCOMMANDS 0 +#else +#define DEFAULT_LEGACY_RTCOMMANDS 1 +#endif + +#ifndef DEFAULT_TOOLCHANGE_MODE +#define DEFAULT_TOOLCHANGE_MODE 0 +#endif +#ifndef DEFAULT_TOOLCHANGE_PROBING_DISTANCE +#define DEFAULT_TOOLCHANGE_PROBING_DISTANCE 30 +#endif +#ifndef DEFAULT_TOOLCHANGE_FEED_RATE +#define DEFAULT_TOOLCHANGE_FEED_RATE 25.0f +#endif +#ifndef DEFAULT_TOOLCHANGE_SEEK_RATE +#define DEFAULT_TOOLCHANGE_SEEK_RATE 200.0f +#endif +#ifndef DEFAULT_TOOLCHANGE_PULLOFF_RATE +#define DEFAULT_TOOLCHANGE_PULLOFF_RATE 100.0f +#endif + +#ifdef DEFAULT_HOMING_ENABLE +#undef DEFAULT_HOMING_ENABLE +#define DEFAULT_HOMING_ENABLE 1 +#else +#define DEFAULT_HOMING_ENABLE 0 +#endif + +#ifdef DEFAULT_HOMING_ALLOW_MANUAL +#undef DEFAULT_HOMING_ALLOW_MANUAL +#define DEFAULT_HOMING_ALLOW_MANUAL 1 +#else +#define DEFAULT_HOMING_ALLOW_MANUAL 0 +#endif + +#ifndef DEFAULT_HOMING_DIR_MASK +#define DEFAULT_HOMING_DIR_MASK 0 +#endif +#ifndef DEFAULT_HOMING_FEED_RATE +#define DEFAULT_HOMING_FEED_RATE 25.0f +#endif +#ifndef DEFAULT_HOMING_SEEK_RATE +#define DEFAULT_HOMING_SEEK_RATE 500.0f +#endif +#ifndef DEFAULT_HOMING_DEBOUNCE_DELAY +#define DEFAULT_HOMING_DEBOUNCE_DELAY 250 +#endif +#ifndef DEFAULT_HOMING_PULLOFF +#define DEFAULT_HOMING_PULLOFF 1.0f +#endif + +#ifndef DEFAULT_N_HOMING_LOCATE_CYCLE +#define DEFAULT_N_HOMING_LOCATE_CYCLE 1 +#endif + +#ifndef HOMING_CYCLE_0 +#define HOMING_CYCLE_0 (Z_AXIS_BIT) +#endif +#ifndef HOMING_CYCLE_1 +#define HOMING_CYCLE_1 (X_AXIS_BIT|Y_AXIS_BIT) +#endif +#ifndef HOMING_CYCLE_2 +#define HOMING_CYCLE_2 0 +#endif +#ifndef HOMING_CYCLE_3 +#define HOMING_CYCLE_3 0 +#endif +#ifndef HOMING_CYCLE_4 +#define HOMING_CYCLE_4 0 +#endif +#ifndef HOMING_CYCLE_5 +#define HOMING_CYCLE_5 0 +#endif + +#ifndef ST_DEENERGIZE_MASK +#define ST_DEENERGIZE_MASK 0 +#endif + +#ifndef INVERT_ST_ENABLE_MASK +#define INVERT_ST_ENABLE_MASK 0 +#endif + +#ifndef INVERT_LIMIT_PIN_MASK +#define INVERT_LIMIT_PIN_MASK 0 +#endif + +#ifndef INVERT_CONTROL_PIN_MASK +#define INVERT_CONTROL_PIN_MASK 0 +#endif + +#ifndef INVERT_SPINDLE_ENABLE_PIN +#define INVERT_SPINDLE_ENABLE_PIN 0 +#endif + +#ifndef INVERT_SPINDLE_CCW_PIN +#define INVERT_SPINDLE_CCW_PIN 0 +#endif + +#ifndef INVERT_SPINDLE_PWM_PIN +#define INVERT_SPINDLE_PWM_PIN 0 +#endif + +#ifndef DISABLE_LIMIT_PINS_PULL_UP_MASK +#define DISABLE_LIMIT_PINS_PULL_UP_MASK 0 +#endif + +#ifndef DISABLE_PROBE_PIN_PULL_UP +#define DISABLE_PROBE_PIN_PULL_UP 0 +#endif + +#ifndef DISABLE_CONTROL_PINS_PULL_UP_MASK +#define DISABLE_CONTROL_PINS_PULL_UP_MASK 0 +#endif + +#ifndef ALLOW_FEED_OVERRIDE_DURING_PROBE_CYCLES +#define ALLOW_FEED_OVERRIDE_DURING_PROBE_CYCLES 0 +#endif + +#ifndef INVERT_COOLANT_FLOOD_PIN +#define INVERT_COOLANT_FLOOD_PIN 0 +#endif + +#ifndef INVERT_COOLANT_MIST_PIN +#define INVERT_COOLANT_MIST_PIN 0 +#endif + +// --------------------------------------------------------------------------------------- +// COMPILE-TIME ERROR CHECKING OF DEFINE VALUES: + +#if DEFAULT_PARKING_ENABLE > 0 + #if DEFAULT_HOMING_FORCE_SET_ORIGIN > 0 + #error "HOMING_FORCE_SET_ORIGIN is not supported with PARKING_ENABLE at this time." + #endif +#endif + +#if DEFAULT_ENABLE_PARKING_OVERRIDE_CONTROL > 0 + #if DEFAULT_PARKING_ENABLE < 1 + #error "ENABLE_PARKING_OVERRIDE_CONTROL must be enabled with PARKING_ENABLE." + #endif +#endif + +// --------------------------------------------------------------------------------------- + +#endif diff --git a/errors.h b/errors.h new file mode 100644 index 0000000..d47fb52 --- /dev/null +++ b/errors.h @@ -0,0 +1,167 @@ +/* + errors.h - + + Part of grblHAL + + Copyright (c) 2017-2021 Terje Io + Copyright (c) 2011-2016 Sungeun K. Jeon for Gnea Research LLC + Copyright (c) 2009-2011 Simen Svale Skogsrud + + 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 . +*/ + +#ifndef _ERRORS_H_ +#define _ERRORS_H_ + +#include + +// Define Grbl status codes. Valid values (0-255) +typedef enum { + Status_OK = 0, + Status_ExpectedCommandLetter = 1, + Status_BadNumberFormat = 2, + Status_InvalidStatement = 3, + Status_NegativeValue = 4, + Status_HomingDisabled = 5, + Status_SettingStepPulseMin = 6, + Status_SettingReadFail = 7, + Status_IdleError = 8, + Status_SystemGClock = 9, + Status_SoftLimitError = 10, + Status_Overflow = 11, + Status_MaxStepRateExceeded = 12, + Status_CheckDoor = 13, + Status_LineLengthExceeded = 14, + Status_TravelExceeded = 15, + Status_InvalidJogCommand = 16, + Status_SettingDisabledLaser = 17, + Status_Reset = 18, + Status_NonPositiveValue = 19, + + Status_GcodeUnsupportedCommand = 20, + Status_GcodeModalGroupViolation = 21, + Status_GcodeUndefinedFeedRate = 22, + Status_GcodeCommandValueNotInteger = 23, + Status_GcodeAxisCommandConflict = 24, + Status_GcodeWordRepeated = 25, + Status_GcodeNoAxisWords = 26, + Status_GcodeInvalidLineNumber = 27, + Status_GcodeValueWordMissing = 28, + Status_GcodeUnsupportedCoordSys = 29, + Status_GcodeG53InvalidMotionMode = 30, + Status_GcodeAxisWordsExist = 31, + Status_GcodeNoAxisWordsInPlane = 32, + Status_GcodeInvalidTarget = 33, + Status_GcodeArcRadiusError = 34, + Status_GcodeNoOffsetsInPlane = 35, + Status_GcodeUnusedWords = 36, + Status_GcodeG43DynamicAxisError = 37, + Status_GcodeIllegalToolTableEntry = 38, + Status_GcodeValueOutOfRange = 39, + Status_GcodeToolChangePending = 40, + Status_GcodeSpindleNotRunning = 41, + Status_GcodeIllegalPlane = 42, + Status_GcodeMaxFeedRateExceeded = 43, + Status_GcodeRPMOutOfRange = 44, + Status_LimitsEngaged = 45, + Status_HomingRequired = 46, + Status_GCodeToolError = 47, + Status_ValueWordConflict = 48, + Status_SelfTestFailed = 49, + Status_EStop = 50, + Status_MotorFault = 51, + Status_SettingValueOutOfRange = 52, + Status_SettingDisabled = 53, + +// Some error codes as defined in bdring's ESP32 port + Status_SDMountError = 60, + Status_SDReadError = 61, + Status_SDFailedOpenDir = 62, + Status_SDDirNotFound = 63, + Status_SDFileEmpty = 64, + + Status_BTInitError = 70, + Status_Unhandled // For internal use only +} status_code_t; + +typedef struct { + status_code_t id; + const char *name; + const char *description; +} status_detail_t; + +PROGMEM static const status_detail_t status_detail[] = { + { Status_OK, "ok", NULL }, + { Status_ExpectedCommandLetter, "Expected command letter", "G-code words consist of a letter and a value. Letter was not found." }, + { Status_BadNumberFormat, "Bad number format", "Missing the expected G-code word value or numeric value format is not valid." }, + { Status_InvalidStatement, "Invalid statement", "Grbl '$' system command was not recognized or supported." }, + { Status_NegativeValue, "Value < 0", "Negative value received for an expected positive value." }, + { Status_HomingDisabled, "Homing disabled", "Homing cycle failure. Homing is not configured via settings." }, + { Status_SettingStepPulseMin, "Value < 2 microseconds", "Step pulse time must be greater or equal to 2 microseconds." }, + { Status_SettingReadFail, "EEPROM read fail. Using defaults", "An EEPROM read failed. Auto-restoring affected EEPROM to default values." }, + { Status_IdleError, "Not idle", "Grbl '$' command cannot be used unless Grbl is IDLE. Ensures smooth operation during a job." }, + { Status_SystemGClock, "G-code lock", "G-code commands are locked out during alarm or jog state." }, + { Status_SoftLimitError, "Homing not enabled", "Soft limits cannot be enabled without homing also enabled." }, + { Status_Overflow, "Line overflow", "Max characters per line exceeded. Received command line was not executed." }, + { Status_MaxStepRateExceeded, "Step rate > 30kHz", "Grbl '$' setting value cause the step rate to exceed the maximum supported." }, + { Status_CheckDoor, "Check Door", "Safety door detected as opened and door state initiated." }, + { Status_LineLengthExceeded, "Line length exceeded", "Build info or startup line exceeded EEPROM line length limit. Line not stored." }, + { Status_TravelExceeded, "Travel exceeded", "Jog target exceeds machine travel. Jog command has been ignored." }, + { Status_InvalidJogCommand, "Invalid jog command", "Jog command has no '=' or contains prohibited g-code." }, + { Status_SettingDisabledLaser, "Setting disabled", "Laser mode requires PWM output." }, + { Status_Reset, "Reset asserted", "" }, + { Status_NonPositiveValue, "Non positive value", "" }, + { Status_GcodeUnsupportedCommand, "Unsupported command", "Unsupported or invalid g-code command found in block." }, + { Status_GcodeModalGroupViolation, "Modal group violation", "More than one g-code command from same modal group found in block." }, + { Status_GcodeUndefinedFeedRate, "Undefined feed rate", "Feed rate has not yet been set or is undefined." }, + { Status_GcodeCommandValueNotInteger, "Invalid gcode ID:23", "G-code command in block requires an integer value." }, + { Status_GcodeAxisCommandConflict, "Invalid gcode ID:24", "More than one g-code command that requires axis words found in block." }, + { Status_GcodeWordRepeated, "Invalid gcode ID:25", "Repeated g-code word found in block." }, + { Status_GcodeNoAxisWords, "Invalid gcode ID:26", "No axis words found in block for g-code command or current modal state which requires them." }, + { Status_GcodeInvalidLineNumber, "Invalid gcode ID:27", "Line number value is invalid." }, + { Status_GcodeValueWordMissing, "Invalid gcode ID:28", "G-code command is missing a required value word." }, + { Status_GcodeUnsupportedCoordSys, "Invalid gcode ID:29", "G59.x work coordinate systems are not supported." }, + { Status_GcodeG53InvalidMotionMode, "Invalid gcode ID:30", "G53 only allowed with G0 and G1 motion modes." }, + { Status_GcodeAxisWordsExist, "Invalid gcode ID:31", "Axis words found in block when no command or current modal state uses them." }, + { Status_GcodeNoAxisWordsInPlane, "Invalid gcode ID:32", "G2 and G3 arcs require at least one in-plane axis word." }, + { Status_GcodeInvalidTarget, "Invalid gcode ID:33", "Motion command target is invalid." }, + { Status_GcodeArcRadiusError, "Invalid gcode ID:34", "Arc radius value is invalid." }, + { Status_GcodeNoOffsetsInPlane, "Invalid gcode ID:35", "G2 and G3 arcs require at least one in-plane offset word." }, + { Status_GcodeUnusedWords, "Invalid gcode ID:36", "Unused value words found in block." }, + { Status_GcodeG43DynamicAxisError, "Invalid gcode ID:37", "G43.1 dynamic tool length offset is not assigned to configured tool length axis." }, + { Status_GcodeIllegalToolTableEntry, "Invalid gcode ID:38", "Tool number greater than max supported value or undefined tool selected." }, + { Status_GcodeValueOutOfRange, "Invalid gcode ID:39", "Value out of range." }, + { Status_GcodeToolChangePending, "Invalid gcode ID:40", "G-code command not allowed when tool change is pending." }, + { Status_GcodeSpindleNotRunning, "Invalid gcode ID:41", "Spindle not running when motion commanded in CSS or spindle sync mode." }, + { Status_GcodeIllegalPlane, "Invalid gcode ID:42", "Plane must be ZX for threading." }, + { Status_GcodeMaxFeedRateExceeded, "Invalid gcode ID:43", "Max. feed rate exceeded." }, + { Status_GcodeRPMOutOfRange, "Invalid gcode ID:44", "RPM out of range." }, + { Status_LimitsEngaged, "Limit switch engaged", "Only homing is allowed when a limit switch is engaged." }, + { Status_HomingRequired, "Homing required", "Home machine to continue." }, + { Status_GCodeToolError, "Invalid gcode ID:47", "ATC: current tool is not set. Set current tool with M61." }, + { Status_ValueWordConflict, "Invalid gcode ID:48", "Value word conflict." }, + { Status_SelfTestFailed, "Self test failed", "Power on self test failed. A hard reset is required." }, + { Status_EStop, "E-stop", "Emergency stop active." }, + { Status_MotorFault, "Motor fault", "Motor fault." }, + { Status_SettingValueOutOfRange, "Value out of range.", "Setting value is out of range." }, + { Status_SettingDisabled, "Setting disabled", "Setting is not available, possibly due to limited driver support." }, + { Status_SDMountError, "SD Card", "SD Card mount failed." }, + { Status_SDReadError, "SD Card", "SD Card file open/read failed." }, + { Status_SDFailedOpenDir, "SD Card", "SD Card directory listing failed." }, + { Status_SDDirNotFound, "SD Card", "SD Card directory not found." }, + { Status_SDFileEmpty, "SD Card", "SD Card file empty." }, + { Status_BTInitError, "Bluetooth", "Bluetooth initalisation failed." } +}; + +#endif diff --git a/gcode.c b/gcode.c new file mode 100644 index 0000000..323a67c --- /dev/null +++ b/gcode.c @@ -0,0 +1,2748 @@ +/* + gcode.c - rs274/ngc parser. + + Part of grblHAL + + Copyright (c) 2017-2021 Terje Io + Copyright (c) 2011-2016 Sungeun K. Jeon for Gnea Research LLC + Copyright (c) 2009-2011 Simen Svale Skogsrud + + 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 . +*/ + +#include +#include +#include + +#include "hal.h" +#include "motion_control.h" +#include "protocol.h" +#include "state_machine.h" + +// NOTE: Max line number is defined by the g-code standard to be 99999. It seems to be an +// arbitrary value, and some GUIs may require more. So we increased it based on a max safe +// value when converting a float (7.2 digit precision)s to an integer. +#define MAX_LINE_NUMBER 10000000 +#ifdef N_TOOLS +#define MAX_TOOL_NUMBER N_TOOLS // Limited by max unsigned 8-bit value +#else +#define MAX_TOOL_NUMBER 4294967294 // Limited by max unsigned 32-bit value - 1 +#endif + +#define MACH3_SCALING + +// Do not change, must be same as axis indices +#define I_VALUE X_AXIS +#define J_VALUE Y_AXIS +#define K_VALUE Z_AXIS + +// Define modal groups internal bitfield for checking multiple command violations and tracking the +// type of command that is called in the block. A modal group is a group of g-code commands that are +// mutually exclusive, or cannot exist on the same line, because they each toggle a state or execute +// a unique motion. These are defined in the NIST RS274-NGC v3 g-code standard, available online, +// and are similar/identical to other g-code interpreters by manufacturers (Haas,Fanuc,Mazak,etc). +typedef union { + uint32_t mask; + struct { + uint32_t G0 :1, // [G4,G10,G28,G28.1,G30,G30.1,G53,G92,G92.1] Non-modal + G1 :1, // [G0,G1,G2,G3,G33,G38.2,G38.3,G38.4,G38.5,G76,G80] Motion + G2 :1, // [G17,G18,G19] Plane selection + G3 :1, // [G90,G91] Distance mode + G4 :1, // [G91.1] Arc IJK distance mode + G5 :1, // [G93,G94,G95] Feed rate mode + G6 :1, // [G20,G21] Units + G7 :1, // [G40] Cutter radius compensation mode. G41/42 NOT SUPPORTED. + G8 :1, // [G43,G43.1,G49] Tool length offset + G10 :1, // [G98,G99] Return mode in canned cycles + G11 :1, // [G50,G51] Scaling + G12 :1, // [G54,G55,G56,G57,G58,G59] Coordinate system selection + G13 :1, // [G61] Control mode + G14 :1, // [G96,G97] Spindle Speed Mode + G15 :1, // [G7,G8] Lathe Diameter Mode + + M4 :1, // [M0,M1,M2,M30] Stopping + M6 :1, // [M6] Tool change + M7 :1, // [M3,M4,M5] Spindle turning + M8 :1, // [M7,M8,M9] Coolant control + M9 :1, // [M49,M50,M51,M53,M56] Override control + M10 :1; // User defined M commands + }; +} modal_groups_t; + +typedef enum { + AxisCommand_None = 0, + AxisCommand_NonModal, + AxisCommand_MotionMode, + AxisCommand_ToolLengthOffset, + AxisCommand_Scaling +} axis_command_t; + +typedef struct { + parameter_words_t parameter; + modal_groups_t modal_group; +} word_bit_t; + +typedef union { + uint_fast8_t mask; + struct { + uint_fast8_t i :1, + j :1, + k :1; + }; +} ijk_words_t; + +// Declare gc extern struct +parser_state_t gc_state, *saved_state = NULL; +#ifdef N_TOOLS +tool_data_t tool_table[N_TOOLS + 1]; +#else +tool_data_t tool_table; +#endif + +#define FAIL(status) return(status); + +static gc_thread_data thread; +static output_command_t *output_commands = NULL; // Linked list +static scale_factor_t scale_factor = { + .ijk[X_AXIS] = 1.0f, + .ijk[Y_AXIS] = 1.0f, + .ijk[Z_AXIS] = 1.0f +#ifdef A_AXIS + , .ijk[A_AXIS] = 1.0f +#endif +#ifdef B_AXIS + , .ijk[B_AXIS] = 1.0f +#endif +#ifdef C_AXIS + , .ijk[C_AXIS] = 1.0f +#endif +}; + +// Simple hypotenuse computation function. +inline static float hypot_f (float x, float y) +{ + return sqrtf(x*x + y*y); +} + +inline static bool motion_is_lasercut (motion_mode_t motion) +{ + return motion == MotionMode_Linear || motion == MotionMode_CwArc || motion == MotionMode_CcwArc || motion == MotionMode_CubicSpline; +} + +static void set_scaling (float factor) +{ + uint_fast8_t idx = N_AXIS; + axes_signals_t state = gc_get_g51_state(); + + do { + scale_factor.ijk[--idx] = factor; +#ifdef MACH3_SCALING + scale_factor.xyz[idx] = 0.0f; +#endif + } while(idx); + + gc_state.modal.scaling_active = factor != 1.0f; + + if(state.value != gc_get_g51_state().value) + sys.report.scaling = On; +} + +float *gc_get_scaling (void) +{ + return scale_factor.ijk; +} + +axes_signals_t gc_get_g51_state () +{ + uint_fast8_t idx = N_AXIS; + axes_signals_t scaled = {0}; + + do { + scaled.value <<= 1; + if(scale_factor.ijk[--idx] != 1.0f) + scaled.value |= 0x01; + + } while(idx); + + return scaled; +} + +float gc_get_offset (uint_fast8_t idx) +{ + return gc_state.modal.coord_system.xyz[idx] + gc_state.g92_coord_offset[idx] + gc_state.tool_length_offset[idx]; +} + +inline static float gc_get_block_offset (parser_block_t *gc_block, uint_fast8_t idx) +{ + return gc_block->modal.coord_system.xyz[idx] + gc_state.g92_coord_offset[idx] + gc_state.tool_length_offset[idx]; +} + +void gc_set_tool_offset (tool_offset_mode_t mode, uint_fast8_t idx, int32_t offset) +{ + bool tlo_changed = false; + + switch(mode) { + + case ToolLengthOffset_Cancel: + idx = N_AXIS; + do { + idx--; + tlo_changed |= gc_state.tool_length_offset[idx] != 0.0f; + gc_state.tool_length_offset[idx] = 0.0f; + gc_state.tool->offset[idx] = 0; + } while(idx); + break; + + case ToolLengthOffset_EnableDynamic: + { + float new_offset = offset / settings.axis[idx].steps_per_mm; + tlo_changed |= gc_state.tool_length_offset[idx] != new_offset; + gc_state.tool_length_offset[idx] = new_offset; + gc_state.tool->offset[idx] = offset; + } + break; + + default: + break; + } + + gc_state.modal.tool_offset_mode = mode; + + if(tlo_changed) { + sys.report.tool_offset = true; + system_flag_wco_change(); + } +} + +plane_t *gc_get_plane_data (plane_t *plane, plane_select_t select) +{ + switch (select) { + + case PlaneSelect_XY: + plane->axis_0 = X_AXIS; + plane->axis_1 = Y_AXIS; + plane->axis_linear = Z_AXIS; + break; + + case PlaneSelect_ZX: + plane->axis_0 = Z_AXIS; + plane->axis_1 = X_AXIS; + plane->axis_linear = Y_AXIS; + break; + + default: // case PlaneSelect_YZ: + plane->axis_0 = Y_AXIS; + plane->axis_1 = Z_AXIS; + plane->axis_linear = X_AXIS; + } + + return plane; +} + +void gc_init (void) +{ + +#if COMPATIBILITY_LEVEL > 1 + memset(&gc_state, 0, sizeof(parser_state_t)); + #ifdef N_TOOLS + gc_state.tool = &tool_table[0]; + #else + memset(&tool_table, 0, sizeof(tool_table)); + gc_state.tool = &tool_table; + #endif +#else + if(sys.cold_start) { + memset(&gc_state, 0, sizeof(parser_state_t)); + #ifdef N_TOOLS + gc_state.tool = &tool_table[0]; + #else + memset(&tool_table, 0, sizeof(tool_table)); + gc_state.tool = &tool_table; + #endif + } else { + memset(&gc_state, 0, offsetof(parser_state_t, g92_coord_offset)); + gc_state.tool_pending = gc_state.tool->tool; + if(hal.tool.select) + hal.tool.select(gc_state.tool, false); + // TODO: restore offsets, tool offset mode? + } +#endif + + // Clear any pending output commands + while(output_commands) { + output_command_t *next = output_commands->next; + free(output_commands); + output_commands = next; + } + + // Load default override status + gc_state.modal.override_ctrl = sys.override.control; + gc_state.spindle.css.max_rpm = settings.spindle.rpm_max; // default max speed for CSS mode + + set_scaling(1.0f); + + // Load default G54 coordinate system. + if (!settings_read_coord_data(gc_state.modal.coord_system.id, &gc_state.modal.coord_system.xyz)) + grbl.report.status_message(Status_SettingReadFail); + +#if COMPATIBILITY_LEVEL <= 1 + if (sys.cold_start && !settings_read_coord_data(CoordinateSystem_G92, &gc_state.g92_coord_offset)) + grbl.report.status_message(Status_SettingReadFail); +#endif + +// if(settings.flags.lathe_mode) +// gc_state.modal.plane_select = PlaneSelect_ZX; +} + + +// Set dynamic laser power mode to PPI (Pulses Per Inch) +// Returns true if driver uses hardware implementation. +// Driver support for pulsing the laser on signal is required for this to work. +bool gc_laser_ppi_enable (uint_fast16_t ppi, uint_fast16_t pulse_length) +{ + gc_state.is_laser_ppi_mode = ppi > 0 && pulse_length > 0; + + return grbl.on_laser_ppi_enable && grbl.on_laser_ppi_enable(ppi, pulse_length); +} + +// Add output command to linked list +static bool add_output_command (output_command_t *command) +{ + output_command_t *add_cmd; + + if((add_cmd = malloc(sizeof(output_command_t)))) { + + memcpy(add_cmd, command, sizeof(output_command_t)); + + if(output_commands == NULL) + output_commands = add_cmd; + else { + output_command_t *cmd = output_commands; + while(cmd->next) + cmd = cmd->next; + cmd->next = add_cmd; + } + } + + return add_cmd != NULL; +} + +static status_code_t init_sync_motion (plan_line_data_t *pl_data, float pitch) +{ + pl_data->condition.inverse_time = Off; + pl_data->feed_rate = gc_state.distance_per_rev = pitch; + pl_data->condition.is_rpm_pos_adjusted = Off; // Switch off CSS. + pl_data->overrides = sys.override.control; // Use current override flags and + pl_data->overrides.sync = On; // set to sync overrides on execution of motion. + + // Disable feed rate and spindle overrides for the duration of the cycle. + pl_data->overrides.spindle_rpm_disable = sys.override.control.spindle_rpm_disable = On; + pl_data->overrides.feed_rate_disable = sys.override.control.feed_rate_disable = On; + sys.override.spindle_rpm = DEFAULT_SPINDLE_RPM_OVERRIDE; + // TODO: need for gc_state.distance_per_rev to be reset on modal change? + float feed_rate = pl_data->feed_rate * hal.spindle.get_data(SpindleData_RPM)->rpm; + + if(feed_rate == 0.0f) + FAIL(Status_GcodeSpindleNotRunning); // [Spindle not running] + + if(feed_rate > settings.axis[Z_AXIS].max_rate * 0.9f) + FAIL(Status_GcodeMaxFeedRateExceeded); // [Feed rate too high] + + return Status_OK; +} + +// Executes one block (line) of 0-terminated G-Code. The block is assumed to contain only uppercase +// characters and signed floating point values (no whitespace). Comments and block delete +// characters have been removed. In this function, all units and positions are converted and +// exported to grbl's internal functions in terms of (mm, mm/min) and absolute machine +// coordinates, respectively. +status_code_t gc_execute_block(char *block, char *message) +{ + static const parameter_words_t axis_words_mask = { + .x = On, + .y = On, + .z = On +#ifdef A_AXIS + , .a = On +#endif +#ifdef A_AXIS + , .b = On +#endif +#ifdef A_AXIS + , .c = On +#endif + }; + + static const parameter_words_t pq_words = { + .p = On, + .q = On + }; + + static const parameter_words_t ij_words = { + .i = On, + .j = On + }; + + static const parameter_words_t positive_only_words = { + .d = On, + .f = On, + .h = On, + .n = On, + .t = On, + .s = On + }; + + static const modal_groups_t jog_groups = { + .G0 = On, + .G3 = On, + .G6 = On + }; + + static parser_block_t gc_block; + + // Determine if the line is a program start/end marker. + // Old comment from protocol.c: + // NOTE: This maybe installed to tell Grbl when a program is running vs manual input, + // where, during a program, the system auto-cycle start will continue to execute + // everything until the next '%' sign. This will help fix resuming issues with certain + // functions that empty the planner buffer to execute its task on-time. + if (block[0] == CMD_PROGRAM_DEMARCATION && block[1] == '\0') { + gc_state.file_run = !gc_state.file_run; + return Status_OK; + } + + /* ------------------------------------------------------------------------------------- + STEP 1: Initialize parser block struct and copy current g-code state modes. The parser + updates these modes and commands as the block line is parsed and will only be used and + executed after successful error-checking. The parser block struct also contains a block + values struct, word tracking variables, and a non-modal commands tracker for the new + block. This struct contains all of the necessary information to execute the block. */ + + memset(&gc_block, 0, sizeof(gc_block)); // Initialize the parser block struct. + memcpy(&gc_block.modal, &gc_state.modal, sizeof(gc_state.modal)); // Copy current modes + + bool set_tool = false; + axis_command_t axis_command = AxisCommand_None; + uint_fast8_t port_command = 0; + plane_t plane; + + // Initialize bitflag tracking variables for axis indices compatible operations. + axes_signals_t axis_words = {0}; // XYZ tracking + ijk_words_t ijk_words = {0}; // IJK tracking + + // Initialize command and value words and parser flags variables. + modal_groups_t command_words = {0}; // Bitfield for tracking G and M command words. Also used for modal group violations. + parameter_words_t value_words = {0}; // Bitfield for tracking value words. + gc_parser_flags_t gc_parser_flags = {0}; // Parser flags for handling special cases. + + // Determine if the line is a jogging motion or a normal g-code block. + if (block[0] == '$') { // NOTE: `$J=` already parsed when passed to this function. + // Set G1 and G94 enforced modes to ensure accurate error checks. + gc_parser_flags.jog_motion = On; + gc_block.modal.motion = MotionMode_Linear; + gc_block.modal.feed_mode = FeedMode_UnitsPerMin; + gc_block.modal.spindle_rpm_mode = SpindleSpeedMode_RPM; + gc_block.values.n = JOG_LINE_NUMBER; // Initialize default line number reported during jog. + } + + /* ------------------------------------------------------------------------------------- + STEP 2: Import all g-code words in the block. A g-code word is a letter followed by + a number, which can either be a 'G'/'M' command or sets/assigns a command value. Also, + perform initial error-checks for command word modal group violations, for any repeated + words, and for negative values set for the value words F, N, P, T, and S. */ + + uint_fast8_t char_counter = gc_parser_flags.jog_motion ? 3 /* Start parsing after `$J=` */ : 0; + char letter; + float value; + uint32_t int_value = 0; + uint_fast16_t mantissa = 0; + bool is_user_mcode = false; + word_bit_t word_bit = { .parameter = {0}, .modal_group = {0} }; // Bit-value for assigning tracking variables + + while ((letter = block[char_counter++]) != '\0') { // Loop until no more g-code words in block. + + // Import the next g-code word, expecting a letter followed by a value. Otherwise, error out. + if((letter < 'A') || (letter > 'Z')) + FAIL(Status_ExpectedCommandLetter); // [Expected word letter] + + if (!read_float(block, &char_counter, &value)) { + if(is_user_mcode) // Valueless parameters allowed for user defined M-codes. + value = NAN; // Parameter validation deferred to implementation. + else + FAIL(Status_BadNumberFormat); // [Expected word value] + } + + // Convert values to smaller uint8 significand and mantissa values for parsing this word. + // NOTE: Mantissa is multiplied by 100 to catch non-integer command values. This is more + // accurate than the NIST gcode requirement of x10 when used for commands, but not quite + // accurate enough for value words that require integers to within 0.0001. This should be + // a good enough comprimise and catch most all non-integer errors. To make it compliant, + // we would simply need to change the mantissa to int16, but this add compiled flash space. + // Maybe update this later. + if(isnan(value)) + mantissa = 0; + else { + int_value = (uint32_t)truncf(value); + mantissa = (uint_fast16_t)roundf(100.0f * (value - int_value)); + } + // NOTE: Rounding must be used to catch small floating point errors. + + // Check if the g-code word is supported or errors due to modal group violations or has + // been repeated in the g-code block. If ok, update the command or record its value. + switch(letter) { + + /* 'G' and 'M' Command Words: Parse commands and check for modal group violations. + NOTE: Modal group numbers are defined in Table 4 of NIST RS274-NGC v3, pg.20 */ + + case 'G': // Determine 'G' command and its modal group + + is_user_mcode = false; + word_bit.modal_group.mask = 0; + + switch(int_value) { + + case 7: case 8: + if(settings.mode == Mode_Lathe) { + word_bit.modal_group.G15 = On; + gc_block.modal.diameter_mode = int_value == 7; // TODO: find specs for implementation, only affects X calculation? reporting? current position? + } else + FAIL(Status_GcodeUnsupportedCommand); // [G7 & G8 not supported] + break; + + case 10: case 28: case 30: case 92: + // Check for G10/28/30/92 being called with G0/1/2/3/38 on same block. + // * G43.1 is also an axis command but is not explicitly defined this way. + if (mantissa == 0) { // Ignore G28.1, G30.1, and G92.1 + if (axis_command) + FAIL(Status_GcodeAxisCommandConflict); // [Axis word/command conflict] + axis_command = AxisCommand_NonModal; + } + // No break. Continues to next line. + + case 4: case 53: + word_bit.modal_group.G0 = On; + gc_block.non_modal_command = (non_modal_t)int_value; + if ((int_value == 28) || (int_value == 30)) { + if (!((mantissa == 0) || (mantissa == 10))) + FAIL(Status_GcodeUnsupportedCommand); + gc_block.non_modal_command += mantissa; + mantissa = 0; // Set to zero to indicate valid non-integer G command. + } else if (int_value == 92) { + if (!((mantissa == 0) || (mantissa == 10) || (mantissa == 20) || (mantissa == 30))) + FAIL(Status_GcodeUnsupportedCommand); + gc_block.non_modal_command += mantissa; + mantissa = 0; // Set to zero to indicate valid non-integer G command. + } + break; + + case 33: case 76: + if(!hal.spindle.get_data) + FAIL(Status_GcodeUnsupportedCommand); // [G33 or G76 not supported] + if (axis_command) + FAIL(Status_GcodeAxisCommandConflict); // [Axis word/command conflict] + axis_command = AxisCommand_MotionMode; + word_bit.modal_group.G1 = On; + gc_block.modal.motion = (motion_mode_t)int_value; + gc_block.modal.canned_cycle_active = false; + break; + + case 38: + if(!(hal.probe.get_state && ((mantissa == 20) || (mantissa == 30) || (mantissa == 40) || (mantissa == 50)))) + FAIL(Status_GcodeUnsupportedCommand); // [probing not supported by driver or unsupported G38.x command] + int_value += (mantissa / 10) + 100; + mantissa = 0; // Set to zero to indicate valid non-integer G command. + // No break. Continues to next line. + + case 0: case 1: case 2: case 3: case 5: + // Check for G0/1/2/3/38 being called with G10/28/30/92 on same block. + // * G43.1 is also an axis command but is not explicitly defined this way. + if (axis_command) + FAIL(Status_GcodeAxisCommandConflict); // [Axis word/command conflict] + axis_command = AxisCommand_MotionMode; + // No break. Continues to next line. + + case 80: + word_bit.modal_group.G1 = On; + gc_block.modal.motion = (motion_mode_t)int_value; + gc_block.modal.canned_cycle_active = false; + break; + + case 73: case 81: case 82: case 83: case 85: case 86: case 89: + if (axis_command) + FAIL(Status_GcodeAxisCommandConflict); // [Axis word/command conflict] + axis_command = AxisCommand_MotionMode; + word_bit.modal_group.G1 = On; + gc_block.modal.canned_cycle_active = true; + gc_block.modal.motion = (motion_mode_t)int_value; + gc_parser_flags.canned_cycle_change = gc_block.modal.motion != gc_state.modal.motion; + break; + + case 17: case 18: case 19: + word_bit.modal_group.G2 = On; + gc_block.modal.plane_select = (plane_select_t)(int_value - 17); + break; + + case 90: case 91: + if (mantissa == 0) { + word_bit.modal_group.G3 = On; + gc_block.modal.distance_incremental = int_value == 91; + } else { + word_bit.modal_group.G4 = On; + if ((mantissa != 10) || (int_value == 90)) + FAIL(Status_GcodeUnsupportedCommand); // [G90.1 not supported] + mantissa = 0; // Set to zero to indicate valid non-integer G command. + // Otherwise, arc IJK incremental mode is default. G91.1 does nothing. + } + break; + + case 93: case 94: + word_bit.modal_group.G5 = On; + gc_block.modal.feed_mode = (feed_mode_t)(94 - int_value); + break; + + case 95: + if(hal.spindle.get_data) { + word_bit.modal_group.G5 = On; + gc_block.modal.feed_mode = FeedMode_UnitsPerRev; + } else + FAIL(Status_GcodeUnsupportedCommand); // [G95 not supported] + break; + + case 20: case 21: + word_bit.modal_group.G6 = On; + gc_block.modal.units_imperial = int_value == 20; + break; + + case 40: + word_bit.modal_group.G7 = On; + // NOTE: Not required since cutter radius compensation is always disabled. Only here + // to support G40 commands that often appear in g-code program headers to setup defaults. + // gc_block.modal.cutter_comp = CUTTER_COMP_DISABLE; // G40 + break; + + case 43: case 49: + word_bit.modal_group.G8 = On; + // NOTE: The NIST g-code standard vaguely states that when a tool length offset is changed, + // there cannot be any axis motion or coordinate offsets updated. Meaning G43, G43.1, and G49 + // all are explicit axis commands, regardless if they require axis words or not. + + if (axis_command) + FAIL(Status_GcodeAxisCommandConflict); // [Axis word/command conflict] } + + axis_command = AxisCommand_ToolLengthOffset; + if (int_value == 49) // G49 + gc_block.modal.tool_offset_mode = ToolLengthOffset_Cancel; +#ifdef N_TOOLS + else if (mantissa == 0) // G43 + gc_block.modal.tool_offset_mode = ToolLengthOffset_Enable; + else if (mantissa == 20) // G43.2 + gc_block.modal.tool_offset_mode = ToolLengthOffset_ApplyAdditional; +#endif + else if (mantissa == 10) // G43.1 + gc_block.modal.tool_offset_mode = ToolLengthOffset_EnableDynamic; + else + FAIL(Status_GcodeUnsupportedCommand); // [Unsupported G43.x command] + mantissa = 0; // Set to zero to indicate valid non-integer G command. + break; + + case 54: case 55: case 56: case 57: case 58: case 59: + word_bit.modal_group.G12 = On; + gc_block.modal.coord_system.id = (coord_system_id_t)(int_value - 54); // Shift to array indexing. + if(int_value == 59 && mantissa > 0) { + if(N_WorkCoordinateSystems == 9 && (mantissa == 10 || mantissa == 20 || mantissa == 30)) { + gc_block.modal.coord_system.id += mantissa / 10; + mantissa = 0; + } else + FAIL(Status_GcodeUnsupportedCommand); // [Unsupported G59.x command] + } + break; + + case 61: + word_bit.modal_group.G13 = On; + if (mantissa != 0) // [G61.1 not supported] + FAIL(Status_GcodeUnsupportedCommand); + // gc_block.modal.control = CONTROL_MODE_EXACT_PATH; // G61 + break; + + case 96: case 97: + if(settings.mode == Mode_Lathe && hal.driver_cap.variable_spindle) { + word_bit.modal_group.G14 = On; + gc_block.modal.spindle_rpm_mode = (spindle_rpm_mode_t)((int_value - 96) ^ 1); + } else + FAIL(Status_GcodeUnsupportedCommand); + break; + + case 98: case 99: + word_bit.modal_group.G10 = On; + gc_block.modal.retract_mode = (cc_retract_mode_t)(int_value - 98); + break; + + case 50: case 51: + axis_command = AxisCommand_Scaling; + word_bit.modal_group.G11 = On; + gc_block.modal.scaling_active = int_value == 51; + break; + + default: FAIL(Status_GcodeUnsupportedCommand); // [Unsupported G command] + } // end G-value switch + + if (mantissa > 0) + FAIL(Status_GcodeCommandValueNotInteger); // [Unsupported or invalid Gxx.x command] + + // Check for more than one command per modal group violations in the current block + // NOTE: Variable 'word_bit' is always assigned, if the command is valid. + if (command_words.mask & word_bit.modal_group.mask) + FAIL(Status_GcodeModalGroupViolation); + + command_words.mask |= word_bit.modal_group.mask; + break; + + case 'M': // Determine 'M' command and its modal group + + if (mantissa > 0) + FAIL(Status_GcodeCommandValueNotInteger); // [No Mxx.x commands] + + is_user_mcode = false; + word_bit.modal_group.mask = 0; + + switch(int_value) { + + case 0: case 1: case 2: case 30: case 60: + word_bit.modal_group.M4 = On; + switch(int_value) { + + case 0: // M0 - program pause + gc_block.modal.program_flow = ProgramFlow_Paused; + break; + + case 1: // M1 - program pause + if(hal.signals_cap.stop_disable ? !hal.control.get_state().stop_disable : !sys.flags.optional_stop_disable) + gc_block.modal.program_flow = ProgramFlow_OptionalStop; + break; + + default: // M2, M30, M60 - program end and reset + gc_block.modal.program_flow = (program_flow_t)int_value; + } + break; + + case 3: case 4: case 5: + word_bit.modal_group.M7 = On; + gc_block.modal.spindle.on = !(int_value == 5); + gc_block.modal.spindle.ccw = int_value == 4; + sys.flags.delay_overrides = On; + break; + + case 6: + if(settings.tool_change.mode != ToolChange_Ignore) { + if(hal.stream.suspend_read || hal.tool.change) + word_bit.modal_group.M6 = On; + else + FAIL(Status_GcodeUnsupportedCommand); // [Unsupported M command] + } + break; + + case 7: case 8: case 9: + word_bit.modal_group.M8 = On; + sys.flags.delay_overrides = On; + gc_parser_flags.set_coolant = On; + switch(int_value) { + + case 7: + if(!hal.driver_cap.mist_control) + FAIL(Status_GcodeUnsupportedCommand); + gc_block.modal.coolant.mist = On; + break; + + case 8: + gc_block.modal.coolant.flood = On; + break; + + case 9: + gc_block.modal.coolant.value = 0; + break; + } + break; + + case 56: + if(!settings.parking.flags.enable_override_control) // TODO: check if enabled? + FAIL(Status_GcodeUnsupportedCommand); // [Unsupported M command] + // no break; + case 49: case 50: case 51: case 53: + word_bit.modal_group.M9 = On; + gc_block.override_command = (override_mode_t)int_value; + break; + + case 61: + set_tool = true; + word_bit.modal_group.M6 = On; //?? + break; + + case 62: + case 63: + case 64: + case 65: + if(hal.port.digital_out == NULL || hal.port.num_digital_out == 0) + FAIL(Status_GcodeUnsupportedCommand); // [Unsupported M command] + word_bit.modal_group.M10 = On; + port_command = int_value; + break; + + case 66: + if(hal.port.wait_on_input == NULL || (hal.port.num_digital_in == 0 && hal.port.num_analog_in == 0)) + FAIL(Status_GcodeUnsupportedCommand); // [Unsupported M command] + word_bit.modal_group.M10 = On; + port_command = int_value; + break; + + case 67: + case 68: + if(hal.port.analog_out == NULL || hal.port.num_analog_out == 0) + FAIL(Status_GcodeUnsupportedCommand); // [Unsupported M command] + word_bit.modal_group.M10 = On; + port_command = int_value; + break; +/* + case 70: + if(!saved_state) + saved_state = malloc(sizeof(parser_state_t)); + if(!saved_state) + FAIL(Status_GcodeUnsupportedCommand); // [Unsupported M command] + memcpy(saved_state, &gc_state, sizeof(parser_state_t)); + return Status_OK; + + case 71: // Invalidate saved state + if(saved_state) { + free(saved_state); + saved_state = NULL; + } + return Status_OK; // Should fail if no state is saved... + + case 72: + if(saved_state) { + // TODO: restore state, need to split out execution part of parser to separate functions first? + free(saved_state); + saved_state = NULL; + } + return Status_OK; +*/ + default: + if(hal.user_mcode.check && (gc_block.user_mcode = hal.user_mcode.check((user_mcode_t)int_value))) { + is_user_mcode = true; + word_bit.modal_group.M10 = On; + } else + FAIL(Status_GcodeUnsupportedCommand); // [Unsupported M command] + } // end M-value switch + + // Check for more than one command per modal group violations in the current block + // NOTE: Variable 'word_bit' is always assigned, if the command is valid. + if (command_words.mask & word_bit.modal_group.mask) + FAIL(Status_GcodeModalGroupViolation); + + command_words.mask |= word_bit.modal_group.mask; + break; + + // NOTE: All remaining letters assign values. + default: + + /* Non-Command Words: This initial parsing phase only checks for repeats of the remaining + legal g-code words and stores their value. Error-checking is performed later since some + words (I,J,K,L,P,R) have multiple connotations and/or depend on the issued commands. */ + + word_bit.parameter.mask = 0; + + switch(letter) { + + #ifdef A_AXIS + case 'A': + axis_words.a = On; + word_bit.parameter.a = On; + gc_block.values.xyz[A_AXIS] = value; + break; + #endif + + #ifdef B_AXIS + case 'B': + axis_words.b = On; + word_bit.parameter.b = On; + gc_block.values.xyz[B_AXIS] = value; + break; + #endif + + #ifdef C_AXIS + case 'C': + axis_words.c = On; + word_bit.parameter.c = On; + gc_block.values.xyz[C_AXIS] = value; + break; + #endif + + case 'D': + word_bit.parameter.d = On; + gc_block.values.d = value; + break; + + case 'E': + word_bit.parameter.e = On; + gc_block.values.e = value; + break; + + case 'F': + word_bit.parameter.f = On; + gc_block.values.f = value; + break; + + case 'H': + if (mantissa > 0) + FAIL(Status_GcodeCommandValueNotInteger); + word_bit.parameter.h = On; + gc_block.values.h = isnan(value) ? 0xFFFFFFFF : int_value; + break; + + case 'I': + ijk_words.i = On; + word_bit.parameter.i = On; + gc_block.values.ijk[I_VALUE] = value; + break; + + case 'J': + ijk_words.j = On; + word_bit.parameter.j = On; + gc_block.values.ijk[J_VALUE] = value; + break; + + case 'K': + ijk_words.k = On; + word_bit.parameter.k = On; + gc_block.values.ijk[K_VALUE] = value; + break; + + case 'L': + if (mantissa > 0) + FAIL(Status_GcodeCommandValueNotInteger); + word_bit.parameter.l = On; + gc_block.values.l = isnan(value) ? 0xFF : (uint8_t)int_value; + break; + + case 'N': + word_bit.parameter.n = On; + gc_block.values.n = (int32_t)truncf(value); + break; + + case 'P': // NOTE: For certain commands, P value must be an integer, but none of these commands are supported. + word_bit.parameter.p = On; + gc_block.values.p = value; + break; + + case 'Q': // may be used for user defined mcodes or G61,G76 + word_bit.parameter.q = On; + gc_block.values.q = value; + break; + + case 'R': + word_bit.parameter.r = On; + gc_block.values.r = value; + break; + + case 'S': + word_bit.parameter.s = On; + gc_block.values.s = value; + break; + + case 'T': + if (mantissa > 0) + FAIL(Status_GcodeCommandValueNotInteger); + if (int_value > MAX_TOOL_NUMBER) + FAIL(Status_GcodeIllegalToolTableEntry); + word_bit.parameter.t = On; + gc_block.values.t = isnan(value) ? 0xFFFFFFFF : int_value; + break; + + case 'X': + axis_words.x = On; + word_bit.parameter.x = On; + gc_block.values.xyz[X_AXIS] = value; + break; + + case 'Y': + axis_words.y = On; + word_bit.parameter.y = On; + gc_block.values.xyz[Y_AXIS] = value; + break; + + case 'Z': + axis_words.z = On; + word_bit.parameter.z = On; + gc_block.values.xyz[Z_AXIS] = value; + break; + + default: FAIL(Status_GcodeUnsupportedCommand); + + } // end parameter letter switch + + // NOTE: Variable 'word_bit' is always assigned, if the non-command letter is valid. + if (value_words.mask & word_bit.parameter.mask) + FAIL(Status_GcodeWordRepeated); // [Word repeated] + + // Check for invalid negative values for words F, H, N, P, T, and S. + // NOTE: Negative value check is done here simply for code-efficiency. + if ((word_bit.parameter.mask & positive_only_words.mask) && value < 0.0f) + FAIL(Status_NegativeValue); // [Word value cannot be negative] + + value_words.mask |= word_bit.parameter.mask; // Flag to indicate parameter assigned. + + } // end main letter switch + } + + // Parsing complete! + + + /* ------------------------------------------------------------------------------------- + STEP 3: Error-check all commands and values passed in this block. This step ensures all of + the commands are valid for execution and follows the NIST standard as closely as possible. + If an error is found, all commands and values in this block are dumped and will not update + the active system g-code modes. If the block is ok, the active system g-code modes will be + updated based on the commands of this block, and signal for it to be executed. + + Also, we have to pre-convert all of the values passed based on the modes set by the parsed + block. There are a number of error-checks that require target information that can only be + accurately calculated if we convert these values in conjunction with the error-checking. + This relegates the next execution step as only updating the system g-code modes and + performing the programmed actions in order. The execution step should not require any + conversion calculations and would only require minimal checks necessary to execute. + */ + + /* NOTE: At this point, the g-code block has been parsed and the block line can be freed. + NOTE: It's also possible, at some future point, to break up STEP 2, to allow piece-wise + parsing of the block on a per-word basis, rather than the entire block. This could remove + the need for maintaining a large string variable for the entire block and free up some memory. + To do this, this would simply need to retain all of the data in STEP 1, such as the new block + data struct, the modal group and value bitflag tracking variables, and axis array indices + compatible variables. This data contains all of the information necessary to error-check the + new g-code block when the EOL character is received. However, this would break Grbl's startup + lines in how it currently works and would require some refactoring to make it compatible. + */ + + /* + * Order of execution as per RS274-NGC_3 table 8: + * + * 1. comment (includes message) + * 2. set feed rate mode (G93, G94 - inverse time or per minute) + * 3. set feed rate (F) + * 4. set spindle speed (S) + * 5. select tool (T) + * 6. change tool (M6) + * 7. spindle on or off (M3, M4, M5) + * 8. coolant on or off (M7, M8, M9) + * 9. enable or disable overrides (M48, M49, M50, M51, M53) + * 10. dwell (G4) + * 11. set active plane (G17, G18, G19) + * 12. set length units (G20, G21) + * 13. cutter radius compensation on or off (G40, G41, G42) + * 14. cutter length compensation on or off (G43, G49) + * 15. coordinate system selection (G54, G55, G56, G57, G58, G59, G59.1, G59.2, G59.3) + * 16. set path control mode (G61, G61.1, G64) + * 17. set distance mode (G90, G91) + * 18. set retract mode (G98, G99) + * 19. home (G28, G30) or + * change coordinate system data (G10) or + * set axis offsets (G92, G92.1, G92.2, G94). + * 20. perform motion (G0 to G3, G33, G80 to G89) as modified (possibly) by G53 + * 21. stop and end (M0, M1, M2, M30, M60) + */ + + // [0. Non-specific/common error-checks and miscellaneous setup]: + + // Determine implicit axis command conditions. Axis words have been passed, but no explicit axis + // command has been sent. If so, set axis command to current motion mode. + if (axis_words.mask && !axis_command) + axis_command = AxisCommand_MotionMode; // Assign implicit motion-mode + + if(gc_state.tool_change && axis_command == AxisCommand_MotionMode && !gc_parser_flags.jog_motion) + FAIL(Status_GcodeToolChangePending); // [Motions (except jogging) not allowed when changing tool] + + // Check for valid line number N value. + // Line number value cannot be less than zero (done) or greater than max line number. + if (value_words.n && gc_block.values.n > MAX_LINE_NUMBER) + FAIL(Status_GcodeInvalidLineNumber); // [Exceeds max line number] + + // bit_false(value_words,bit(Word_N)); // NOTE: Single-meaning value word. Set at end of error-checking. + + // Track for unused words at the end of error-checking. + // NOTE: Single-meaning value words are removed all at once at the end of error-checking, because + // they are always used when present. This was done to save a few bytes of flash. For clarity, the + // single-meaning value words may be removed as they are used. Also, axis words are treated in the + // same way. If there is an explicit/implicit axis command, XYZ words are always used and are + // are removed at the end of error-checking. + + // [1. Comments ]: MSG's may be supported by driver layer. Comment handling performed by protocol. + + // [2. Set feed rate mode ]: G93 F word missing with G1,G2/3 active, implicitly or explicitly. Feed rate + // is not defined after switching between G93, G94 and G95. + // NOTE: For jogging, ignore prior feed rate mode. Enforce G94 and check for required F word. + if (gc_parser_flags.jog_motion) { + + if(!value_words.f) + FAIL(Status_GcodeUndefinedFeedRate); + + if (gc_block.modal.units_imperial) + gc_block.values.f *= MM_PER_INCH; + + } else if(gc_block.modal.motion == MotionMode_SpindleSynchronized) { + + if (!value_words.k) { + gc_block.values.k = gc_state.distance_per_rev; + } else { + value_words.k = Off; + gc_block.values.k = gc_block.modal.units_imperial ? gc_block.values.ijk[K_VALUE] *= MM_PER_INCH : gc_block.values.ijk[K_VALUE]; + } + + } else if (gc_block.modal.feed_mode == FeedMode_InverseTime) { // = G93 + // NOTE: G38 can also operate in inverse time, but is undefined as an error. Missing F word check added here. + if (axis_command == AxisCommand_MotionMode) { + if (!(gc_block.modal.motion == MotionMode_None || gc_block.modal.motion == MotionMode_Seek)) { + if (!value_words.f) + FAIL(Status_GcodeUndefinedFeedRate); // [F word missing] + } + } + // NOTE: It seems redundant to check for an F word to be passed after switching from G94 to G93. We would + // accomplish the exact same thing if the feed rate value is always reset to zero and undefined after each + // inverse time block, since the commands that use this value already perform undefined checks. This would + // also allow other commands, following this switch, to execute and not error out needlessly. This code is + // combined with the above feed rate mode and the below set feed rate error-checking. + + // [3. Set feed rate ]: F is negative (done.) + // - In inverse time mode: Always implicitly zero the feed rate value before and after block completion. + // NOTE: If in G93 mode or switched into it from G94, just keep F value as initialized zero or passed F word + // value in the block. If no F word is passed with a motion command that requires a feed rate, this will error + // out in the motion modes error-checking. However, if no F word is passed with NO motion command that requires + // a feed rate, we simply move on and the state feed rate value gets updated to zero and remains undefined. + + } else if (gc_block.modal.feed_mode == FeedMode_UnitsPerMin || gc_block.modal.feed_mode == FeedMode_UnitsPerRev) { + // if F word passed, ensure value is in mm/min or mm/rev depending on mode, otherwise push last state value. + if (!value_words.f) { + if(gc_block.modal.feed_mode == gc_state.modal.feed_mode) + gc_block.values.f = gc_state.feed_rate; // Push last state feed rate + } else if (gc_block.modal.units_imperial) + gc_block.values.f *= MM_PER_INCH; + } // else, switching to G94 from G93, so don't push last state feed rate. Its undefined or the passed F word value. + + // bit_false(value_words,bit(Word_F)); // NOTE: Single-meaning value word. Set at end of error-checking. + + // [4. Set spindle speed ]: S or D is negative (done.) + if (command_words.G14) { + if(gc_block.modal.spindle_rpm_mode == SpindleSpeedMode_CSS) { + if (!value_words.s) // TODO: add check for S0? + FAIL(Status_GcodeValueWordMissing); + // see below!! gc_block.values.s *= (gc_block.modal.units_imperial ? MM_PER_INCH * 12.0f : 1000.0f); // convert surface speed to mm/min + if (value_words.d) { + gc_state.spindle.css.max_rpm = min(gc_block.values.d, settings.spindle.rpm_max); + value_words.d = Off; + } else + gc_state.spindle.css.max_rpm = settings.spindle.rpm_max; + } else if(gc_state.modal.spindle_rpm_mode == SpindleSpeedMode_CSS) + gc_state.spindle.rpm = sys.spindle_rpm; // Is it correct to restore latest spindle RPM here? + gc_state.modal.spindle_rpm_mode = gc_block.modal.spindle_rpm_mode; + } + + if (!value_words.s) + gc_block.values.s = gc_state.modal.spindle_rpm_mode == SpindleSpeedMode_RPM ? gc_state.spindle.rpm : gc_state.spindle.css.surface_speed; + else if(gc_state.modal.spindle_rpm_mode == SpindleSpeedMode_CSS) + // Unsure what to do about S values when in SpindleSpeedMode_CSS - ignore? For now use it to (re)calculate surface speed. + // Reinsert commented out code above if this is removed!! + gc_block.values.s *= (gc_block.modal.units_imperial ? MM_PER_INCH * 12.0f : 1000.0f); // convert surface speed to mm/min + + // bit_false(value_words,bit(Word_S)); // NOTE: Single-meaning value word. Set at end of error-checking. + + // [5. Select tool ]: If not supported then only tracks value. T is negative (done.) Not an integer (done). + if(set_tool) { // M61 + if(!value_words.q) + FAIL(Status_GcodeValueWordMissing); + if (floorf(gc_block.values.q) - gc_block.values.q != 0.0f) + FAIL(Status_GcodeCommandValueNotInteger); + if ((uint32_t)gc_block.values.q > MAX_TOOL_NUMBER) + FAIL(Status_GcodeIllegalToolTableEntry); + + gc_block.values.t = (uint32_t)gc_block.values.q; + value_words.q = Off; + } else if (!value_words.t) + gc_block.values.t = gc_state.tool_pending; + + if(command_words.M10 && port_command) { + + switch(port_command) { + + case 62: + case 63: + case 64: + case 65: + if(!value_words.p) + FAIL(Status_GcodeValueWordMissing); + if(gc_block.values.p < 0.0f) + FAIL(Status_NegativeValue); + if((uint32_t)gc_block.values.p + 1 > hal.port.num_digital_out) + FAIL(Status_GcodeValueOutOfRange); + gc_block.output_command.is_digital = true; + gc_block.output_command.port = (uint8_t)gc_block.values.p; + gc_block.output_command.value = port_command == 62 || port_command == 64 ? 1.0f : 0.0f; + value_words.p = Off; + break; + + case 66: + if(!(value_words.l || value_words.q)) + FAIL(Status_GcodeValueWordMissing); + + if(value_words.p && value_words.e) + FAIL(Status_ValueWordConflict); + + if(gc_block.values.l >= (uint8_t)WaitMode_Max) + FAIL(Status_GcodeValueOutOfRange); + + if((wait_mode_t)gc_block.values.l != WaitMode_Immediate && gc_block.values.q == 0.0f) + FAIL(Status_GcodeValueOutOfRange); + + if(value_words.p) { + if(gc_block.values.p < 0.0f) + FAIL(Status_NegativeValue); + if((uint32_t)gc_block.values.p + 1 > hal.port.num_digital_in) + FAIL(Status_GcodeValueOutOfRange); + + gc_block.output_command.is_digital = true; + gc_block.output_command.port = (uint8_t)gc_block.values.p; + } + + if(value_words.e) { + if((uint32_t)gc_block.values.e + 1 > hal.port.num_analog_in) + FAIL(Status_GcodeValueOutOfRange); + if((wait_mode_t)gc_block.values.l != WaitMode_Immediate) + FAIL(Status_GcodeValueOutOfRange); + + gc_block.output_command.is_digital = false; + gc_block.output_command.port = (uint8_t)gc_block.values.e; + } + + value_words.e = value_words.l = value_words.p = value_words.q = Off; + break; + + case 67: + case 68: + if(!(value_words.e || value_words.q)) + FAIL(Status_GcodeValueWordMissing); + if((uint32_t)gc_block.values.e + 1 > hal.port.num_analog_out) + FAIL(Status_GcodeRPMOutOfRange); + gc_block.output_command.is_digital = false; + gc_block.output_command.port = (uint8_t)gc_block.values.e; + gc_block.output_command.value = gc_block.values.q; + value_words.e = value_words.q = Off; + break; + } + } + + // bit_false(value_words,bit(Word_T)); // NOTE: Single-meaning value word. Set at end of error-checking. + + // [6. Change tool ]: N/A + // [7. Spindle control ]: N/A + // [8. Coolant control ]: N/A + + // [9. Override control ]: + if (command_words.M9) { + + if(!value_words.p) + gc_block.values.p = 1.0f; + else { + if(gc_block.values.p < 0.0f) + FAIL(Status_NegativeValue); + value_words.p = Off; + } + switch(gc_block.override_command) { + + case Override_FeedSpeed: + gc_block.modal.override_ctrl.feed_rate_disable = gc_block.values.p == 0.0f; + gc_block.modal.override_ctrl.spindle_rpm_disable = gc_block.values.p == 0.0f; + break; + + case Override_FeedRate: + gc_block.modal.override_ctrl.feed_rate_disable = gc_block.values.p == 0.0f; + break; + + case Override_SpindleSpeed: + gc_block.modal.override_ctrl.spindle_rpm_disable = gc_block.values.p == 0.0f; + break; + + case Override_FeedHold: + gc_block.modal.override_ctrl.feed_hold_disable = gc_block.values.p == 0.0f; + break; + + case Override_Parking: + if(settings.parking.flags.enable_override_control) + gc_block.modal.override_ctrl.parking_disable = gc_block.values.p == 0.0f; + break; + + default: + break; + } + } + + // [9a. User defined M commands ]: + if (command_words.M10 && gc_block.user_mcode) { + if((int_value = (uint_fast16_t)hal.user_mcode.validate(&gc_block, &value_words))) + FAIL((status_code_t)int_value); + axis_words.mask = ijk_words.mask = 0; + } + + // [10. Dwell ]: P value missing. NOTE: See below. + if (gc_block.non_modal_command == NonModal_Dwell) { + if (!value_words.p) + FAIL(Status_GcodeValueWordMissing); // [P word missing] + if(gc_block.values.p < 0.0f) + FAIL(Status_NegativeValue); + value_words.p = Off; + } + + // [11. Set active plane ]: N/A + gc_get_plane_data(&plane, gc_block.modal.plane_select); + + // [12. Set length units ]: N/A + // Pre-convert XYZ coordinate values to millimeters, if applicable. + uint_fast8_t idx = N_AXIS; + if (gc_block.modal.units_imperial) do { // Axes indices are consistent, so loop may be used. + if (bit_istrue(axis_words.mask, bit(--idx))) + gc_block.values.xyz[idx] *= MM_PER_INCH; + } while(idx); + + if (command_words.G15) { + sys.report.xmode |= gc_state.modal.diameter_mode != gc_block.modal.diameter_mode; + gc_state.modal.diameter_mode = gc_block.modal.diameter_mode; + } + + if(gc_state.modal.diameter_mode && bit_istrue(axis_words.mask, bit(X_AXIS))) + gc_block.values.xyz[X_AXIS] /= 2.0f; + + // Scale axis words if commanded + if(axis_command == AxisCommand_Scaling) { + + if(gc_block.modal.scaling_active) { + + // TODO: precheck for 0.0f and fail if found? + + gc_block.modal.scaling_active = false; + +#ifdef MACH3_SCALING + // [G51 Errors]: No axis words. TODO: add support for P (scale all with same factor)? + if (!axis_words.mask) + FAIL(Status_GcodeNoAxisWords); // [No axis words] + + idx = N_AXIS; + do { + if(bit_istrue(axis_words.mask, bit(--idx))) { + sys.report.scaling = sys.report.scaling || scale_factor.ijk[idx] != gc_block.values.xyz[idx]; + scale_factor.ijk[idx] = gc_block.values.xyz[idx]; + bit_false(axis_words.mask, bit(idx)); + } + gc_block.modal.scaling_active = gc_block.modal.scaling_active || (scale_factor.xyz[idx] != 1.0f); + } while(idx); + + value_words.mask &= ~axis_words_mask.mask; // Remove axis words. +#else + if (!(value_words.p || ijk_words.mask)) + FAIL(Status_GcodeNoAxisWords); // [No axis words] + + idx = N_AXIS; + do { + if(bit_istrue(axis_words.mask, bit(--idx))) + scale_factor.xyz[idx] = gc_block.values.xyz[idx]; + else + scale_factor.xyz[idx] = gc_state.position[idx]; + } while(idx); + + value_words.mask &= ~axis_words_mask.mask; // Remove axis words. + + idx = 3; + do { + idx--; + if(value_words.p) { + sys.report.scaling = sys.report.scaling || scale_factor.ijk[idx] != gc_block.values.p; + scale_factor.ijk[idx] = gc_block.values.p; + } else if(bit_istrue(ijk_words.mask, bit(idx))) { + sys.report.scaling = sys.report.scaling || scale_factor.ijk[idx] != gc_block.values.ijk[idx]; + scale_factor.ijk[idx] = gc_block.values.ijk[idx]; + } + gc_block.modal.scaling_active = gc_block.modal.scaling_active || (scale_factor.ijk[idx] != 1.0f); + } while(idx); + + if(value_words.p) + value_words.p = Off; + else + value_words.i = value_words.j = value_words.k = Off; +#endif + sys.report.scaling = sys.report.scaling || gc_state.modal.scaling_active != gc_block.modal.scaling_active; + gc_state.modal.scaling_active = gc_block.modal.scaling_active; + + } else + set_scaling(1.0f); + } + + // Scale axis words if scaling active + if(gc_state.modal.scaling_active) { + idx = N_AXIS; + do { + if(bit_istrue(axis_words.mask, bit(--idx))) { + if(gc_block.modal.distance_incremental) + gc_block.values.xyz[idx] *= scale_factor.ijk[idx]; + else + gc_block.values.xyz[idx] = (gc_block.values.xyz[idx] - scale_factor.xyz[idx]) * scale_factor.ijk[idx] + scale_factor.xyz[idx]; + } + } while(idx); + } + + // [13. Cutter radius compensation ]: G41/42 NOT SUPPORTED. Error, if enabled while G53 is active. + // [G40 Errors]: G2/3 arc is programmed after a G40. The linear move after disabling is less than tool diameter. + // NOTE: Since cutter radius compensation is never enabled, these G40 errors don't apply. Grbl supports G40 + // only for the purpose to not error when G40 is sent with a g-code program header to setup the default modes. + + // [14. Tool length compensation ]: G43.1 and G49 are always supported, G43 and G43.2 if N_TOOLS defined. + // [G43.1 Errors]: Motion command in same line. + // [G43.2 Errors]: Motion command in same line. Tool number not in the tool table, + // NOTE: Although not explicitly stated so, G43.1 should be applied to only one valid + // axis that is configured (in config.h). There should be an error if the configured axis + // is absent or if any of the other axis words are present. + if (axis_command == AxisCommand_ToolLengthOffset) { // Indicates called in block. + +#ifdef TOOL_LENGTH_OFFSET_AXIS + if(gc_block.modal.tool_offset_mode == ToolLengthOffset_EnableDynamic) { + if (axis_words.mask ^ bit(TOOL_LENGTH_OFFSET_AXIS)) + FAIL(Status_GcodeG43DynamicAxisError); + } +#endif + + switch(gc_block.modal.tool_offset_mode) { + + case ToolLengthOffset_EnableDynamic: + if (!axis_words.mask) + FAIL(Status_GcodeG43DynamicAxisError); + break; +#ifdef N_TOOLS + case ToolLengthOffset_Enable: + if (value_words.h) { + if(gc_block.values.h > MAX_TOOL_NUMBER) + FAIL(Status_GcodeIllegalToolTableEntry); + value_words.h = Off; + if(gc_block.values.h == 0) + gc_block.values.h = gc_block.values.t; + } else + gc_block.values.h = gc_block.values.t; + break; + + case ToolLengthOffset_ApplyAdditional: + if (value_words.h) { + if(gc_block.values.h == 0 || gc_block.values.h > MAX_TOOL_NUMBER) + FAIL(Status_GcodeIllegalToolTableEntry); + value_words.h = Off; + } else + FAIL(Status_GcodeValueWordMissing); + break; +#endif + default: + break; + } + } + + // [15. Coordinate system selection ]: *N/A. Error, if cutter radius comp is active. + // TODO: A read of the coordinate data may require a buffer sync when the cycle + // is active. The read pauses the processor temporarily and may cause a rare crash. + // NOTE: If NVS buffering is active then non-volatile storage reads/writes are buffered and updates + // delayed until no cycle is active. + + if (command_words.G12) { // Check if called in block + if (gc_state.modal.coord_system.id != gc_block.modal.coord_system.id && !settings_read_coord_data(gc_block.modal.coord_system.id, &gc_block.modal.coord_system.xyz)) + FAIL(Status_SettingReadFail); + } + + // [16. Set path control mode ]: N/A. Only G61. G61.1 and G64 NOT SUPPORTED. + // [17. Set distance mode ]: N/A. Only G91.1. G90.1 NOT SUPPORTED. + // [18. Set retract mode ]: N/A. + + // [19. Remaining non-modal actions ]: Check go to predefined position, set G10, or set axis offsets. + // NOTE: We need to separate the non-modal commands that are axis word-using (G10/G28/G30/G92), as these + // commands all treat axis words differently. G10 as absolute offsets or computes current position as + // the axis value, G92 similarly to G10 L20, and G28/30 as an intermediate target position that observes + // all the current coordinate system and G92 offsets. + switch (gc_block.non_modal_command) { + + case NonModal_SetCoordinateData: + + // [G10 Errors]: L missing and is not 2 or 20. P word missing. (Negative P value done.) + // [G10 L2 Errors]: R word NOT SUPPORTED. P value not 0 to N_WorkCoordinateSystems (max 9). Axis words missing. + // [G10 L20 Errors]: P must be 0 to N_WorkCoordinateSystems (max 9). Axis words missing. + // [G10 L1, L10, L11 Errors]: P must be 0 to MAX_TOOL_NUMBER (max 9). Axis words or R word missing. + + if (!(axis_words.mask || (gc_block.values.l != 20 && value_words.r))) + FAIL(Status_GcodeNoAxisWords); // [No axis words (or R word for tool offsets)] + + if (!(value_words.p || value_words.l)) + FAIL(Status_GcodeValueWordMissing); // [P/L word missing] + + if(gc_block.values.p < 0.0f) + FAIL(Status_NegativeValue); + + uint8_t p_value; + + p_value = (uint8_t)truncf(gc_block.values.p); // Convert p value to int. + + switch(gc_block.values.l) { + + case 2: + if (value_words.r) + FAIL(Status_GcodeUnsupportedCommand); // [G10 L2 R not supported] + // no break + + case 20: + if (p_value > N_WorkCoordinateSystems) + FAIL(Status_GcodeUnsupportedCoordSys); // [Greater than N sys] + // Determine coordinate system to change and try to load from non-volatile storage. + gc_block.values.coord_data.id = p_value == 0 + ? gc_block.modal.coord_system.id // Index P0 as the active coordinate system + : (coord_system_id_t)(p_value - 1); // else adjust index to NVS coordinate data indexing. + + if (!settings_read_coord_data(gc_block.values.coord_data.id, &gc_block.values.coord_data.xyz)) + FAIL(Status_SettingReadFail); // [non-volatile storage read fail] + + // Pre-calculate the coordinate data changes. + idx = N_AXIS; + do { // Axes indices are consistent, so loop may be used. + // Update axes defined only in block. Always in machine coordinates. Can change non-active system. + if (bit_istrue(axis_words.mask, bit(--idx))) { + if (gc_block.values.l == 20) + // L20: Update coordinate system axis at current position (with modifiers) with programmed value + // WPos = MPos - WCS - G92 - TLO -> WCS = MPos - G92 - TLO - WPos + gc_block.values.coord_data.xyz[idx] = gc_state.position[idx] - gc_block.values.xyz[idx] - gc_state.g92_coord_offset[idx] - gc_state.tool_length_offset[idx]; + else // L2: Update coordinate system axis to programmed value. + gc_block.values.coord_data.xyz[idx] = gc_block.values.xyz[idx]; + } // else, keep current stored value. + } while(idx); + break; + +#ifdef N_TOOLS + case 1: case 10: case 11:; + if(p_value == 0 || p_value > MAX_TOOL_NUMBER) + FAIL(Status_GcodeIllegalToolTableEntry); // [Greater than MAX_TOOL_NUMBER] + + tool_table[p_value].tool = p_value; + + if(value_words.r) { + tool_table[p_value].radius = gc_block.values.r; + value_words.r = Off; + } + + float g59_3_offset[N_AXIS]; + if(gc_block.values.l == 11 && !settings_read_coord_data(CoordinateSystem_G59_3, &g59_3_offset)) + FAIL(Status_SettingReadFail); + + idx = N_AXIS; + do { + if (bit_istrue(axis_words.mask, bit(--idx))) { + if(gc_block.values.l == 1) + tool_table[p_value].offset[idx] = gc_block.values.xyz[idx]; + else if(gc_block.values.l == 10) + tool_table[p_value].offset[idx] = gc_state.position[idx] - gc_state.g92_coord_offset[idx] - gc_block.values.xyz[idx]; + else if(gc_block.values.l == 11) + tool_table[p_value].offset[idx] = g59_3_offset[idx] - gc_block.values.xyz[idx]; + if (gc_block.values.l != 1) + tool_table[p_value].offset[idx] -= gc_state.tool_length_offset[idx]; + } + // else, keep current stored value. + } while(idx); + + if(gc_block.values.l == 1) + settings_write_tool_data(&tool_table[p_value]); + + break; +#endif + default: + FAIL(Status_GcodeUnsupportedCommand); // [Unsupported L] + } + value_words.l = value_words.p = Off; + break; + + case NonModal_SetCoordinateOffset: + + // [G92 Errors]: No axis words. + if (!axis_words.mask) + FAIL(Status_GcodeNoAxisWords); // [No axis words] + + // Update axes defined only in block. Offsets current system to defined value. Does not update when + // active coordinate system is selected, but is still active unless G92.1 disables it. + idx = N_AXIS; + do { // Axes indices are consistent, so loop may be used. + if (bit_istrue(axis_words.mask, bit(--idx))) { + // WPos = MPos - WCS - G92 - TLO -> G92 = MPos - WCS - TLO - WPos + gc_block.values.xyz[idx] = gc_state.position[idx] - gc_block.modal.coord_system.xyz[idx] - gc_block.values.xyz[idx] - gc_state.tool_length_offset[idx]; + } else + gc_block.values.xyz[idx] = gc_state.g92_coord_offset[idx]; + } while(idx); + break; + + default: + + // At this point, the rest of the explicit axis commands treat the axis values as the traditional + // target position with the coordinate system offsets, G92 offsets, absolute override, and distance + // modes applied. This includes the motion mode commands. We can now pre-compute the target position. + // NOTE: Tool offsets may be appended to these conversions when/if this feature is added. + if (axis_words.mask && axis_command != AxisCommand_ToolLengthOffset) { // TLO block any axis command. + idx = N_AXIS; + do { // Axes indices are consistent, so loop may be used to save flash space. + if (bit_isfalse(axis_words.mask, bit(--idx))) + gc_block.values.xyz[idx] = gc_state.position[idx]; // No axis word in block. Keep same axis position. + else if (gc_block.non_modal_command != NonModal_AbsoluteOverride) { + // Update specified value according to distance mode or ignore if absolute override is active. + // NOTE: G53 is never active with G28/30 since they are in the same modal group. + // Apply coordinate offsets based on distance mode. + if (gc_block.modal.distance_incremental) + gc_block.values.xyz[idx] += gc_state.position[idx]; + else // Absolute mode + gc_block.values.xyz[idx] += gc_get_block_offset(&gc_block, idx); + } + } while(idx); + } + + // Check remaining non-modal commands for errors. + switch (gc_block.non_modal_command) { + + case NonModal_GoHome_0: // G28 + case NonModal_GoHome_1: // G30 + // [G28/30 Errors]: Cutter compensation is enabled. + // Retreive G28/30 go-home position data (in machine coordinates) from non-volatile storage + + if (!settings_read_coord_data(gc_block.non_modal_command == NonModal_GoHome_0 ? CoordinateSystem_G28 : CoordinateSystem_G30, &gc_block.values.coord_data.xyz)) + FAIL(Status_SettingReadFail); + + if (axis_words.mask) { + // Move only the axes specified in secondary move. + idx = N_AXIS; + do { + if (bit_isfalse(axis_words.mask, bit(--idx))) + gc_block.values.coord_data.xyz[idx] = gc_state.position[idx]; + } while(idx); + } else + axis_command = AxisCommand_None; // Set to none if no intermediate motion. + break; + + case NonModal_SetHome_0: // G28.1 + case NonModal_SetHome_1: // G30.1 + // [G28.1/30.1 Errors]: Cutter compensation is enabled. + // NOTE: If axis words are passed here, they are interpreted as an implicit motion mode. + break; + + case NonModal_ResetCoordinateOffset: + // NOTE: If axis words are passed here, they are interpreted as an implicit motion mode. + break; + + case NonModal_AbsoluteOverride: + // [G53 Errors]: G0 and G1 are not active. Cutter compensation is enabled. + // NOTE: All explicit axis word commands are in this modal group. So no implicit check necessary. + if (!(gc_block.modal.motion == MotionMode_Seek || gc_block.modal.motion == MotionMode_Linear)) + FAIL(Status_GcodeG53InvalidMotionMode); // [G53 G0/1 not active] + break; + + default: + break; + } + } // end gc_block.non_modal_command + + // [20. Motion modes ]: + if (gc_block.modal.motion == MotionMode_None) { + + // [G80 Errors]: Axis word are programmed while G80 is active. + // NOTE: Even non-modal commands or TLO that use axis words will throw this strict error. + if (axis_words.mask) // [No axis words allowed] + FAIL(Status_GcodeAxisWordsExist); + + gc_block.modal.retract_mode = CCRetractMode_Previous; + + // Check remaining motion modes, if axis word are implicit (exist and not used by G10/28/30/92), or + // was explicitly commanded in the g-code block. + } else if (axis_command == AxisCommand_MotionMode) { + + gc_parser_flags.motion_mode_changed = gc_block.modal.motion != gc_state.modal.motion; + + if (gc_block.modal.motion == MotionMode_Seek) { + // [G0 Errors]: Axis letter not configured or without real value (done.) + // Axis words are optional. If missing, set axis command flag to ignore execution. + if (!axis_words.mask) + axis_command = AxisCommand_None; + + // All remaining motion modes (all but G0 and G80), require a valid feed rate value. In units per mm mode, + // the value must be positive. In inverse time mode, a positive value must be passed with each block. + } else { + + if(!gc_block.modal.canned_cycle_active) + gc_block.modal.retract_mode = CCRetractMode_Previous; + + // Initial(?) check for spindle running for moves in G96 mode + if(gc_block.modal.spindle_rpm_mode == SpindleSpeedMode_CSS && (!gc_block.modal.spindle.on || gc_block.values.s == 0.0f)) + FAIL(Status_GcodeSpindleNotRunning); + + // Check if feed rate is defined for the motion modes that require it. + if (gc_block.modal.motion == MotionMode_SpindleSynchronized) { + + if(gc_block.values.k == 0.0f) + FAIL(Status_GcodeValueOutOfRange); // [No distance (pitch) given] + + // Ensure spindle speed is at 100% - any override will be disabled on execute. + gc_parser_flags.spindle_force_sync = On; + + } else if (gc_block.modal.motion == MotionMode_Threading) { + + // Fail if cutter radius comp is active + + if(gc_block.modal.plane_select != PlaneSelect_ZX) + FAIL(Status_GcodeIllegalPlane); // [Plane not ZX] + + if(axis_words.mask & ~(bit(X_AXIS)|bit(Z_AXIS))) + FAIL(Status_GcodeUnusedWords); // [Only X and Z axis words allowed] + + if(value_words.r && gc_block.values.r < 1.0f) + FAIL(Status_GcodeValueOutOfRange); + + if(!axis_words.z || !(value_words.i || value_words.j || value_words.k || value_words.p)) + FAIL(Status_GcodeValueWordMissing); + + if(gc_block.values.p < 0.0f || gc_block.values.ijk[J_VALUE] < 0.0f || gc_block.values.ijk[K_VALUE] < 0.0f) + FAIL(Status_NegativeValue); + + if(gc_block.values.ijk[I_VALUE] == 0.0f || + gc_block.values.ijk[J_VALUE] == 0.0f || + gc_block.values.ijk[K_VALUE] <= gc_block.values.ijk[J_VALUE] || + (value_words.l && (gc_taper_type)gc_block.values.l > Taper_Both)) + FAIL(Status_GcodeValueOutOfRange); + + if(gc_state.spindle.rpm < settings.spindle.rpm_min || gc_state.spindle.rpm > settings.spindle.rpm_max) + FAIL(Status_GcodeRPMOutOfRange); + + if(gc_block.modal.motion != gc_state.modal.motion) { + memset(&thread, 0, sizeof(gc_thread_data)); + thread.depth_degression = 1.0f; + } + + thread.pitch = gc_block.values.p; + thread.z_final = gc_block.values.xyz[Z_AXIS]; + thread.cut_direction = gc_block.values.ijk[I_VALUE] < 0.0f ? -1.0f : 1.0f; + thread.peak = fabsf(gc_block.values.ijk[I_VALUE]); + thread.initial_depth = gc_block.values.ijk[J_VALUE]; + thread.depth = gc_block.values.ijk[K_VALUE]; + + if(gc_block.modal.units_imperial) { + thread.peak *= MM_PER_INCH; + thread.initial_depth *= MM_PER_INCH; + thread.depth *= MM_PER_INCH; + } + + if(gc_block.modal.diameter_mode) { + thread.peak /= 2.0f; + thread.initial_depth /= 2.0f; + thread.depth /= 2.0f; + } + + //scaling? + + if(axis_words.x) { + thread.main_taper_height = gc_block.values.xyz[X_AXIS] - gc_get_block_offset(&gc_block, X_AXIS); + gc_block.values.p = fabsf(thread.z_final - gc_state.position[Z_AXIS]); + thread.pitch = thread.pitch * hypot_f(thread.main_taper_height, gc_block.values.p) / gc_block.values.p; + } + + if(value_words.h) + thread.spring_passes = (uint_fast16_t)gc_block.values.h; + + if(value_words.l) + thread.end_taper_type = (gc_taper_type)gc_block.values.l; + + if(value_words.e) + thread.end_taper_length = gc_block.values.e; + + if(thread.end_taper_length <= 0.0f || thread.end_taper_type == Taper_None) { + thread.end_taper_length = 0.0f; + thread.end_taper_type = Taper_None; + // TODO: fail? + } + + if(thread.end_taper_type != Taper_None && thread.end_taper_length > abs(thread.z_final - gc_state.position[Z_AXIS]) / 2.0f) + FAIL(Status_GcodeValueOutOfRange); + + if(value_words.r) + thread.depth_degression = gc_block.values.r; + + if(value_words.q) + thread.infeed_angle = gc_block.values.q; + + // Ensure spindle speed is at 100% - any override will be disabled on execute. + gc_parser_flags.spindle_force_sync = On; + + value_words.e = value_words.h = value_words.i = value_words.j = value_words.k = value_words.l = value_words.p = value_words.q = value_words.r = Off; + + } else if (gc_block.values.f == 0.0f) + FAIL(Status_GcodeUndefinedFeedRate); // [Feed rate undefined] + + if (gc_block.modal.canned_cycle_active) { + + if(gc_parser_flags.canned_cycle_change) { + + if(gc_state.modal.feed_mode == FeedMode_InverseTime) + FAIL(Status_InvalidStatement); + + if(!value_words.r) + FAIL(Status_GcodeValueWordMissing); + + if(!(axis_words.mask & bit(plane.axis_linear))) + FAIL(Status_GcodeValueWordMissing); + + gc_state.canned.dwell = 0.0f; + gc_state.canned.xyz[plane.axis_0] = 0.0f; + gc_state.canned.xyz[plane.axis_1] = 0.0f; + gc_state.canned.rapid_retract = On; + gc_state.canned.spindle_off = Off; + gc_state.canned.prev_position = gc_state.position[plane.axis_linear]; + } + + if(!value_words.l) + gc_block.values.l = 1; + else if(gc_block.values.l <= 0) + FAIL(Status_NonPositiveValue); // [L <= 0] + + if (value_words.r) { + gc_state.canned.retract_position = gc_block.values.r; + if(gc_state.modal.distance_incremental) + gc_state.canned.retract_position += gc_state.position[plane.axis_linear]; + gc_state.canned.retract_position = gc_block.modal.coord_system.xyz[plane.axis_linear] + gc_state.canned.retract_position; + } + + idx = N_AXIS; + do { + if(bit_istrue(axis_words.mask, bit(--idx))) { + gc_state.canned.xyz[idx] = gc_block.values.xyz[idx]; + if(idx != plane.axis_linear) + gc_state.canned.xyz[idx] -= gc_state.position[idx]; + } + } while(idx); + + if(gc_state.canned.retract_position < gc_state.canned.xyz[plane.axis_linear]) + FAIL(Status_GcodeAxisCommandConflict); + + value_words.r = value_words.l = Off; // Remove single-meaning value words. + + switch (gc_block.modal.motion) { + + case MotionMode_CannedCycle86: + case MotionMode_CannedCycle89: + gc_state.canned.spindle_off = gc_block.modal.motion == MotionMode_CannedCycle86; + gc_state.canned.rapid_retract = gc_block.modal.motion == MotionMode_CannedCycle86; + // no break + + case MotionMode_CannedCycle82: + if(value_words.p) { + if(gc_block.values.p < 0.0f) + FAIL(Status_NegativeValue); + gc_state.canned.dwell = gc_block.values.p; + value_words.p = Off; // Remove single-meaning value word. + } else if(gc_parser_flags.canned_cycle_change) + FAIL(Status_GcodeValueWordMissing); + // no break + + case MotionMode_CannedCycle85: + case MotionMode_CannedCycle81: + gc_state.canned.delta = - gc_state.canned.xyz[plane.axis_linear] + gc_state.canned.retract_position; + if(gc_block.modal.motion == MotionMode_CannedCycle85) + gc_state.canned.rapid_retract = Off; + break; + + case MotionMode_DrillChipBreak: + case MotionMode_CannedCycle83: + if(value_words.q) { + if(gc_block.values.q <= 0.0f) + FAIL(Status_NegativeValue); // [Q <= 0] + gc_state.canned.delta = gc_block.values.q; + value_words.q = Off; // Remove single-meaning value word. + } else if(gc_parser_flags.canned_cycle_change) + FAIL(Status_GcodeValueWordMissing); + gc_state.canned.dwell = 0.25f; + break; + + default: + break; + + } // end switch gc_state.canned.motion + + } else switch (gc_block.modal.motion) { + + case MotionMode_Linear: + // [G1 Errors]: Feed rate undefined. Axis letter not configured or without real value. + // Axis words are optional. If missing, set axis command flag to ignore execution. + if (!axis_words.mask) + axis_command = AxisCommand_None; + break; + + case MotionMode_CwArc: + gc_parser_flags.arc_is_clockwise = On; + // No break intentional. + + case MotionMode_CcwArc: + // [G2/3 Errors All-Modes]: Feed rate undefined. + // [G2/3 Radius-Mode Errors]: No axis words in selected plane. Target point is same as current. + // [G2/3 Offset-Mode Errors]: No axis words and/or offsets in selected plane. The radius to the current + // point and the radius to the target point differs more than 0.002mm (EMC def. 0.5mm OR 0.005mm and 0.1% radius). + // [G2/3 Full-Circle-Mode Errors]: NOT SUPPORTED. Axis words exist. No offsets programmed. P must be an integer. + // NOTE: Both radius and offsets are required for arc tracing and are pre-computed with the error-checking. + + if (!axis_words.mask) + FAIL(Status_GcodeNoAxisWords); // [No axis words] + + if (!(axis_words.mask & (bit(plane.axis_0)|bit(plane.axis_1)))) + FAIL(Status_GcodeNoAxisWordsInPlane); // [No axis words in plane] + + // Calculate the change in position along each selected axis + float x, y; + x = gc_block.values.xyz[plane.axis_0] - gc_state.position[plane.axis_0]; // Delta x between current position and target + y = gc_block.values.xyz[plane.axis_1] - gc_state.position[plane.axis_1]; // Delta y between current position and target + + if (value_words.r) { // Arc Radius Mode + + value_words.r = Off; + + if (isequal_position_vector(gc_state.position, gc_block.values.xyz)) + FAIL(Status_GcodeInvalidTarget); // [Invalid target] + + // Convert radius value to proper units. + if (gc_block.modal.units_imperial) + gc_block.values.r *= MM_PER_INCH; + + if(gc_state.modal.scaling_active) + gc_block.values.r *= (scale_factor.ijk[plane.axis_0] > scale_factor.ijk[plane.axis_1] + ? scale_factor.ijk[plane.axis_0] + : scale_factor.ijk[plane.axis_1]); + + /* We need to calculate the center of the circle that has the designated radius and passes + through both the current position and the target position. This method calculates the following + set of equations where [x,y] is the vector from current to target position, d == magnitude of + that vector, h == hypotenuse of the triangle formed by the radius of the circle, the distance to + the center of the travel vector. A vector perpendicular to the travel vector [-y,x] is scaled to the + length of h [-y/d*h, x/d*h] and added to the center of the travel vector [x/2,y/2] to form the new point + [i,j] at [x/2-y/d*h, y/2+x/d*h] which will be the center of our arc. + + d^2 == x^2 + y^2 + h^2 == r^2 - (d/2)^2 + i == x/2 - y/d*h + j == y/2 + x/d*h + + O <- [i,j] + - | + r - | + - | + - | h + - | + [0,0] -> C -----------------+--------------- T <- [x,y] + | <------ d/2 ---->| + + C - Current position + T - Target position + O - center of circle that pass through both C and T + d - distance from C to T + r - designated radius + h - distance from center of CT to O + + Expanding the equations: + + d -> sqrt(x^2 + y^2) + h -> sqrt(4 * r^2 - x^2 - y^2)/2 + i -> (x - (y * sqrt(4 * r^2 - x^2 - y^2)) / sqrt(x^2 + y^2)) / 2 + j -> (y + (x * sqrt(4 * r^2 - x^2 - y^2)) / sqrt(x^2 + y^2)) / 2 + + Which can be written: + + i -> (x - (y * sqrt(4 * r^2 - x^2 - y^2))/sqrt(x^2 + y^2))/2 + j -> (y + (x * sqrt(4 * r^2 - x^2 - y^2))/sqrt(x^2 + y^2))/2 + + Which we for size and speed reasons optimize to: + + h_x2_div_d = sqrt(4 * r^2 - x^2 - y^2)/sqrt(x^2 + y^2) + i = (x - (y * h_x2_div_d))/2 + j = (y + (x * h_x2_div_d))/2 + */ + + // First, use h_x2_div_d to compute 4*h^2 to check if it is negative or r is smaller + // than d. If so, the sqrt of a negative number is complex and error out. + float h_x2_div_d = 4.0f * gc_block.values.r * gc_block.values.r - x * x - y * y; + + if (h_x2_div_d < 0.0f) + FAIL(Status_GcodeArcRadiusError); // [Arc radius error] + + // Finish computing h_x2_div_d. + h_x2_div_d = -sqrtf(h_x2_div_d) / hypot_f(x, y); // == -(h * 2 / d) + + // Invert the sign of h_x2_div_d if the circle is counter clockwise (see sketch below) + if (gc_block.modal.motion == MotionMode_CcwArc) + h_x2_div_d = -h_x2_div_d; + + /* The counter clockwise circle lies to the left of the target direction. When offset is positive, + the left hand circle will be generated - when it is negative the right hand circle is generated. + + T <-- Target position + + ^ + Clockwise circles with this center | Clockwise circles with this center will have + will have > 180 deg of angular travel | < 180 deg of angular travel, which is a good thing! + \ | / + center of arc when h_x2_div_d is positive -> x <----- | -----> x <- center of arc when h_x2_div_d is negative + | + | + + C <-- Current position + */ + // Negative R is g-code-alese for "I want a circle with more than 180 degrees of travel" (go figure!), + // even though it is advised against ever generating such circles in a single line of g-code. By + // inverting the sign of h_x2_div_d the center of the circles is placed on the opposite side of the line of + // travel and thus we get the unadvisably long arcs as prescribed. + if (gc_block.values.r < 0.0f) { + h_x2_div_d = -h_x2_div_d; + gc_block.values.r = -gc_block.values.r; // Finished with r. Set to positive for mc_arc + } + // Complete the operation by calculating the actual center of the arc + gc_block.values.ijk[plane.axis_0] = 0.5f * (x - (y * h_x2_div_d)); + gc_block.values.ijk[plane.axis_1] = 0.5f * (y + (x * h_x2_div_d)); + + } else { // Arc Center Format Offset Mode + + if (!(ijk_words.mask & (bit(plane.axis_0)|bit(plane.axis_1)))) + FAIL(Status_GcodeNoOffsetsInPlane);// [No offsets in plane] + + value_words.i = value_words.j = value_words.k = Off; + + // Convert IJK values to proper units. + if (gc_block.modal.units_imperial) { + idx = 3; + do { // Axes indices are consistent, so loop may be used to save flash space. + if (ijk_words.mask & bit(--idx)) + gc_block.values.ijk[idx] *= MM_PER_INCH; + } while(idx); + } + + // Scale values if scaling active - NOTE: only incremental mode is supported + if(gc_state.modal.scaling_active) { + idx = 3; + do { + if (ijk_words.mask & bit(--idx)) + gc_block.values.ijk[idx] *= scale_factor.ijk[idx]; + } while(idx); + } + + // Arc radius from center to target + x -= gc_block.values.ijk[plane.axis_0]; // Delta x between circle center and target + y -= gc_block.values.ijk[plane.axis_1]; // Delta y between circle center and target + float target_r = hypot_f(x, y); + + // Compute arc radius for mc_arc. Defined from current location to center. + gc_block.values.r = hypot_f(gc_block.values.ijk[plane.axis_0], gc_block.values.ijk[plane.axis_1]); + + // Compute difference between current location and target radii for final error-checks. + float delta_r = fabsf(target_r - gc_block.values.r); + if (delta_r > 0.005f) { + if (delta_r > 0.5f) + FAIL(Status_GcodeInvalidTarget); // [Arc definition error] > 0.5mm + if (delta_r > (0.001f * gc_block.values.r)) + FAIL(Status_GcodeInvalidTarget); // [Arc definition error] > 0.005mm AND 0.1% radius + } + } + break; + + case MotionMode_CubicSpline: + // [G5 Errors]: Feed rate undefined. + // [G5 Plane Errors]: The active plane is not G17. + // [G5 Offset Errors]: P and Q are not both specified. + // [G5 Offset Errors]: Just one of I or J are specified. + // [G5 Offset Errors]: I or J are unspecified in the first of a series of G5 commands. + // [G5 Axisword Errors]: An axis other than X or Y is specified. + if(gc_block.modal.plane_select != PlaneSelect_XY) + FAIL(Status_GcodeIllegalPlane); // [The active plane is not G17] + + if (axis_words.mask & ~(bit(X_AXIS)|bit(Y_AXIS))) + FAIL(Status_GcodeAxisCommandConflict); // [An axis other than X or Y is specified] + + if((value_words.mask & pq_words.mask) != pq_words.mask) + FAIL(Status_GcodeValueWordMissing); // [P and Q are not both specified] + + if(gc_parser_flags.motion_mode_changed && (value_words.mask & ij_words.mask) != ij_words.mask) + FAIL(Status_GcodeValueWordMissing); // [I or J are unspecified in the first of a series of G5 commands] + + if(!(value_words.i || value_words.j)) { + gc_block.values.ijk[I_VALUE] = - gc_block.values.p; + gc_block.values.ijk[J_VALUE] = - gc_block.values.q; + } else { + // Convert I and J values to proper units. + if (gc_block.modal.units_imperial) { + gc_block.values.ijk[I_VALUE] *= MM_PER_INCH; + gc_block.values.ijk[J_VALUE] *= MM_PER_INCH; + } + // Scale values if scaling active + if(gc_state.modal.scaling_active) { + gc_block.values.ijk[I_VALUE] *= scale_factor.ijk[X_AXIS]; + gc_block.values.ijk[J_VALUE] *= scale_factor.ijk[Y_AXIS]; + } + } + // Convert P and Q values to proper units. + if (gc_block.modal.units_imperial) { + gc_block.values.p *= MM_PER_INCH; + gc_block.values.q *= MM_PER_INCH; + } + // Scale values if scaling active + if(gc_state.modal.scaling_active) { + gc_block.values.p *= scale_factor.ijk[X_AXIS]; + gc_block.values.q *= scale_factor.ijk[Y_AXIS]; + } + gc_state.modal.spline_pq[X_AXIS] = gc_block.values.p; + gc_state.modal.spline_pq[Y_AXIS] = gc_block.values.q; + value_words.p = value_words.q = value_words.i = value_words.j = Off; + break; + + case MotionMode_ProbeTowardNoError: + case MotionMode_ProbeAwayNoError: + gc_parser_flags.probe_is_no_error = On; + // No break intentional. + + case MotionMode_ProbeToward: + case MotionMode_ProbeAway: + if(gc_block.modal.motion == MotionMode_ProbeAway || gc_block.modal.motion == MotionMode_ProbeAwayNoError) + gc_parser_flags.probe_is_away = On; + // [G38 Errors]: Target is same current. No axis words. Cutter compensation is enabled. Feed rate + // is undefined. Probe is triggered. NOTE: Probe check moved to probe cycle. Instead of returning + // an error, it issues an alarm to prevent further motion to the probe. It's also done there to + // allow the planner buffer to empty and move off the probe trigger before another probing cycle. + if (!axis_words.mask) + FAIL(Status_GcodeNoAxisWords); // [No axis words] + if (isequal_position_vector(gc_state.position, gc_block.values.xyz)) + FAIL(Status_GcodeInvalidTarget); // [Invalid target] + break; + + default: + break; + + } // end switch gc_block.modal.motion + } + } + + // [21. Program flow ]: No error checks required. + + // [0. Non-specific error-checks]: Complete unused value words check, i.e. IJK used when in arc + // radius mode, or axis words that aren't used in the block. + if (gc_parser_flags.jog_motion) // Jogging only uses the F feed rate and XYZ value words. N is valid, but S and T are invalid. + value_words.n = value_words.f = Off; + else + value_words.n = value_words.f = value_words.s = value_words.t = Off; + + if (axis_command) + value_words.mask &= ~axis_words_mask.mask; // Remove axis words. + + if (value_words.mask) + FAIL(Status_GcodeUnusedWords); // [Unused words] + + /* ------------------------------------------------------------------------------------- + STEP 4: EXECUTE!! + Assumes that all error-checking has been completed and no failure modes exist. We just + need to update the state and execute the block according to the order-of-execution. + */ + + // Initialize planner data struct for motion blocks. + plan_line_data_t plan_data; + memset(&plan_data, 0, sizeof(plan_line_data_t)); // Zero plan_data struct + + // Intercept jog commands and complete error checking for valid jog commands and execute. + // NOTE: G-code parser state is not updated, except the position to ensure sequential jog + // targets are computed correctly. The final parser position after a jog is updated in + // protocol_execute_realtime() when jogging completes or is canceled. + if (gc_parser_flags.jog_motion) { + + // Only distance and unit modal commands and G53 absolute override command are allowed. + // NOTE: Feed rate word and axis word checks have already been performed in STEP 3. + if (command_words.mask & ~jog_groups.mask) + FAIL(Status_InvalidJogCommand); + + if (!(gc_block.non_modal_command == NonModal_AbsoluteOverride || gc_block.non_modal_command == NonModal_NoAction)) + FAIL(Status_InvalidJogCommand); + + // Initialize planner data to current spindle and coolant modal state. + memcpy(&plan_data.spindle, &gc_state.spindle, sizeof(spindle_t)); + plan_data.condition.spindle = gc_state.modal.spindle; + plan_data.condition.coolant = gc_state.modal.coolant; + plan_data.condition.is_rpm_rate_adjusted = gc_state.is_rpm_rate_adjusted; + + if ((status_code_t)(int_value = (uint_fast16_t)mc_jog_execute(&plan_data, &gc_block)) == Status_OK) + memcpy(gc_state.position, gc_block.values.xyz, sizeof(gc_state.position)); + + return (status_code_t)int_value; + } + + // If in laser mode, setup laser power based on current and past parser conditions. + if(settings.mode == Mode_Laser) { + + if(!motion_is_lasercut(gc_block.modal.motion)) + gc_parser_flags.laser_disable = On; + + // Any motion mode with axis words is allowed to be passed from a spindle speed update. + // NOTE: G1 and G0 without axis words sets axis_command to none. G28/30 are intentionally omitted. + // TODO: Check sync conditions for M3 enabled motions that don't enter the planner. (zero length). + if(axis_words.mask && (axis_command == AxisCommand_MotionMode)) + gc_parser_flags.laser_is_motion = On; + else if(gc_state.modal.spindle.on && !gc_state.modal.spindle.ccw) { + // M3 constant power laser requires planner syncs to update the laser when changing between + // a G1/2/3 motion mode state and vice versa when there is no motion in the line. + if(motion_is_lasercut(gc_state.modal.motion)) { + if(gc_parser_flags.laser_disable) + gc_parser_flags.spindle_force_sync = On; // Change from G1/2/3 motion mode. + } else if(!gc_parser_flags.laser_disable) // When changing to a G1 motion mode without axis words from a non-G1/2/3 motion mode. + gc_parser_flags.spindle_force_sync = On; + } + + gc_state.is_rpm_rate_adjusted = gc_state.modal.spindle.ccw && !gc_parser_flags.laser_disable && hal.driver_cap.variable_spindle; + } + + // [0. Non-specific/common error-checks and miscellaneous setup]: + // NOTE: If no line number is present, the value is zero. + gc_state.line_number = gc_block.values.n; + plan_data.line_number = gc_state.line_number; // Record data for planner use. + + bool check_mode = state_get() == STATE_CHECK_MODE; + + // [1. Comments feedback ]: Extracted in protocol.c if HAL entry point provided + if(message && !check_mode && (plan_data.message = malloc(strlen(message) + 1))) + strcpy(plan_data.message, message); + + // [2. Set feed rate mode ]: + gc_state.modal.feed_mode = gc_block.modal.feed_mode; + if (gc_state.modal.feed_mode == FeedMode_InverseTime) + plan_data.condition.inverse_time = On; // Set condition flag for planner use. + + // [3. Set feed rate ]: + gc_state.feed_rate = gc_block.values.f; // Always copy this value. See feed rate error-checking. + plan_data.feed_rate = gc_state.feed_rate; // Record data for planner use. + + // [4. Set spindle speed ]: + if(gc_state.modal.spindle_rpm_mode == SpindleSpeedMode_CSS) { + gc_state.spindle.css.surface_speed = gc_block.values.s; + if((plan_data.condition.is_rpm_pos_adjusted = gc_block.modal.motion != MotionMode_None && gc_block.modal.motion != MotionMode_Seek)) { + gc_state.spindle.css.active = true; + gc_state.spindle.css.axis = plane.axis_1; + gc_state.spindle.css.tool_offset = gc_get_offset(gc_state.spindle.css.axis); + float pos = gc_state.position[gc_state.spindle.css.axis] - gc_state.spindle.css.tool_offset; + gc_block.values.s = pos <= 0.0f ? gc_state.spindle.css.max_rpm : min(gc_state.spindle.css.max_rpm, gc_state.spindle.css.surface_speed / (pos * (float)(2.0f * M_PI))); + gc_parser_flags.spindle_force_sync = On; + } else { + if(gc_state.spindle.css.active) { + gc_state.spindle.css.active = false; + protocol_buffer_synchronize(); // Empty planner buffer to ensure we get RPM at end of last CSS motion + } + gc_block.values.s = sys.spindle_rpm; // Keep current RPM + } + } + + if ((gc_state.spindle.rpm != gc_block.values.s) || gc_parser_flags.spindle_force_sync) { + if (gc_state.modal.spindle.on && !gc_parser_flags.laser_is_motion) + spindle_sync(gc_state.modal.spindle, gc_parser_flags.laser_disable ? 0.0f : gc_block.values.s); + gc_state.spindle.rpm = gc_block.values.s; // Update spindle speed state. + } + + // NOTE: Pass zero spindle speed for all restricted laser motions. + if (!gc_parser_flags.laser_disable) + memcpy(&plan_data.spindle, &gc_state.spindle, sizeof(spindle_t)); // Record data for planner use. + // else { plan_data.spindle.speed = 0.0; } // Initialized as zero already. + + // [5. Select tool ]: Only tracks tool value if ATC or manual tool change is not possible. + if(gc_state.tool_pending != gc_block.values.t && !check_mode) { + + gc_state.tool_pending = gc_block.values.t; + + // If M6 not available or M61 commanded set new tool immediately + if(set_tool || settings.tool_change.mode == ToolChange_Ignore || !(hal.stream.suspend_read || hal.tool.change)) { +#ifdef N_TOOLS + gc_state.tool = &tool_table[gc_state.tool_pending]; +#else + gc_state.tool->tool = gc_state.tool_pending; +#endif + sys.report.tool = On; + } + + // Prepare tool carousel when available + if(hal.tool.select) { +#ifdef N_TOOLS + hal.tool.select(&tool_table[gc_state.tool_pending], !set_tool); +#else + hal.tool.select(gc_state.tool, !set_tool); +#endif + } else + sys.report.tool = On; + } + + // [5a. HAL pin I/O ]: M62 - M68. (Modal group M10) + + if(port_command) { + + switch(port_command) { + + case 62: + case 63: + add_output_command(&gc_block.output_command); + break; + + case 64: + case 65: + hal.port.digital_out(gc_block.output_command.port, gc_block.output_command.value != 0.0f); + break; + + case 66: + hal.port.wait_on_input(gc_block.output_command.is_digital, gc_block.output_command.port, (wait_mode_t)gc_block.values.l, gc_block.values.q); + break; + + case 67: + add_output_command(&gc_block.output_command); + break; + + case 68: + hal.port.analog_out(gc_block.output_command.port, gc_block.output_command.value); + break; + } + } + + // [6. Change tool ]: Delegated to (possible) driver implementation + if (command_words.M6 && !set_tool && !check_mode) { + + protocol_buffer_synchronize(); + + if(plan_data.message) { + report_message(plan_data.message, Message_Plain); + free(plan_data.message); + plan_data.message = NULL; + } + +#ifdef N_TOOLS + gc_state.tool = &tool_table[gc_state.tool_pending]; +#else + gc_state.tool->tool = gc_state.tool_pending; +#endif + if(hal.tool.change) { // ATC + if((int_value = (uint_fast16_t)hal.tool.change(&gc_state)) != Status_OK) + FAIL((status_code_t)int_value); + sys.report.tool = On; + } else { // Manual + gc_state.tool_change = true; + system_set_exec_state_flag(EXEC_TOOL_CHANGE); // Set up program pause for manual tool change + protocol_execute_realtime(); // Execute... + } + } + + // [7. Spindle control ]: + if (gc_state.modal.spindle.value != gc_block.modal.spindle.value) { + // Update spindle control and apply spindle speed when enabling it in this block. + // NOTE: All spindle state changes are synced, even in laser mode. Also, plan_data, + // rather than gc_state, is used to manage laser state for non-laser motions. + if(spindle_sync(gc_block.modal.spindle, plan_data.spindle.rpm)) + gc_state.modal.spindle = gc_block.modal.spindle; + } + +// TODO: Recheck spindle running in CCS mode (is_rpm_pos_adjusted = On)? + + plan_data.condition.spindle = gc_state.modal.spindle; // Set condition flag for planner use. + plan_data.condition.is_rpm_rate_adjusted = gc_state.is_rpm_rate_adjusted; + plan_data.condition.is_laser_ppi_mode = gc_state.is_rpm_rate_adjusted && gc_state.is_laser_ppi_mode; + + // [8. Coolant control ]: + if (gc_parser_flags.set_coolant && gc_state.modal.coolant.value != gc_block.modal.coolant.value) { + // NOTE: Coolant M-codes are modal. Only one command per line is allowed. But, multiple states + // can exist at the same time, while coolant disable clears all states. + if(coolant_sync(gc_block.modal.coolant)) + gc_state.modal.coolant = gc_block.modal.coolant; + } + + plan_data.condition.coolant = gc_state.modal.coolant; // Set condition flag for planner use. + + sys.flags.delay_overrides = Off; + + // [9. Override control ]: + if (gc_state.modal.override_ctrl.value != gc_block.modal.override_ctrl.value) { + gc_state.modal.override_ctrl = gc_block.modal.override_ctrl; + + if(gc_state.modal.override_ctrl.feed_rate_disable) + plan_feed_override(0, 0); + + if(gc_state.modal.override_ctrl.spindle_rpm_disable) + spindle_set_override(DEFAULT_SPINDLE_RPM_OVERRIDE); + + mc_override_ctrl_update(gc_state.modal.override_ctrl); // NOTE: must be called last! + } + + // [9a. User defined M commands ]: + if(gc_block.user_mcode && !check_mode) { + + if(gc_block.user_mcode_sync) + protocol_buffer_synchronize(); // Ensure user defined mcode is executed when specified in program. + + hal.user_mcode.execute(state_get(), &gc_block); + } + + // [10. Dwell ]: + if (gc_block.non_modal_command == NonModal_Dwell) + mc_dwell(gc_block.values.p); + + // [11. Set active plane ]: + gc_state.modal.plane_select = gc_block.modal.plane_select; + + // [12. Set length units ]: + gc_state.modal.units_imperial = gc_block.modal.units_imperial; + + // [13. Cutter radius compensation ]: G41/42 NOT SUPPORTED + // gc_state.modal.cutter_comp = gc_block.modal.cutter_comp; // NOTE: Not needed since always disabled. + + // [14. Tool length compensation ]: G43, G43.1 and G49 supported. G43 supported when N_TOOLS defined. + // NOTE: If G43 were supported, its operation wouldn't be any different from G43.1 in terms + // of execution. The error-checking step would simply load the offset value into the correct + // axis of the block XYZ value array. + if (axis_command == AxisCommand_ToolLengthOffset) { // Indicates a change. + + bool tlo_changed = false; + + idx = N_AXIS; + gc_state.modal.tool_offset_mode = gc_block.modal.tool_offset_mode; + + do { + + idx--; + + switch(gc_state.modal.tool_offset_mode) { + + case ToolLengthOffset_Cancel: // G49 + tlo_changed |= gc_state.tool_length_offset[idx] != 0.0f; + gc_state.tool_length_offset[idx] = 0.0f; + break; +#ifdef N_TOOLS + case ToolLengthOffset_Enable: // G43 + if (gc_state.tool_length_offset[idx] != tool_table[gc_block.values.h].offset[idx]) { + tlo_changed = true; + gc_state.tool_length_offset[idx] = tool_table[gc_block.values.h].offset[idx]; + } + break; + + case ToolLengthOffset_ApplyAdditional: // G43.2 + tlo_changed |= tool_table[gc_block.values.h].offset[idx] != 0.0f; + gc_state.tool_length_offset[idx] += tool_table[gc_block.values.h].offset[idx]; + break; +#endif + case ToolLengthOffset_EnableDynamic: // G43.1 + if (bit_istrue(axis_words.mask, bit(idx)) && gc_state.tool_length_offset[idx] != gc_block.values.xyz[idx]) { + tlo_changed = true; + gc_state.tool_length_offset[idx] = gc_block.values.xyz[idx]; + } + break; + + default: + break; + } + } while(idx); + + if(tlo_changed) { + sys.report.tool_offset = true; + system_flag_wco_change(); + } + } + + // [15. Coordinate system selection ]: + if (gc_state.modal.coord_system.id != gc_block.modal.coord_system.id) { + memcpy(&gc_state.modal.coord_system, &gc_block.modal.coord_system, sizeof(gc_state.modal.coord_system)); + sys.report.gwco = On; + system_flag_wco_change(); + } + + // [16. Set path control mode ]: G61.1/G64 NOT SUPPORTED + // gc_state.modal.control = gc_block.modal.control; // NOTE: Always default. + + // [17. Set distance mode ]: + gc_state.modal.distance_incremental = gc_block.modal.distance_incremental; + + // [18. Set retract mode ]: + gc_state.modal.retract_mode = gc_block.modal.retract_mode; + + // [19. Go to predefined position, Set G10, or Set axis offsets ]: + switch(gc_block.non_modal_command) { + + case NonModal_SetCoordinateData: + settings_write_coord_data(gc_block.values.coord_data.id, &gc_block.values.coord_data.xyz); + // Update system coordinate system if currently active. + if (gc_state.modal.coord_system.id == gc_block.values.coord_data.id) { + memcpy(gc_state.modal.coord_system.xyz, gc_block.values.coord_data.xyz, sizeof(gc_state.modal.coord_system.xyz)); + system_flag_wco_change(); + } + break; + + case NonModal_GoHome_0: + case NonModal_GoHome_1: + // Move to intermediate position before going home. Obeys current coordinate system and offsets + // and absolute and incremental modes. + plan_data.condition.rapid_motion = On; // Set rapid motion condition flag. + if (axis_command) + mc_line(gc_block.values.xyz, &plan_data); + mc_line(gc_block.values.coord_data.xyz, &plan_data); + memcpy(gc_state.position, gc_block.values.coord_data.xyz, sizeof(gc_state.position)); + set_scaling(1.0f); + break; + + case NonModal_SetHome_0: + settings_write_coord_data(CoordinateSystem_G28, &gc_state.position); + break; + + case NonModal_SetHome_1: + settings_write_coord_data(CoordinateSystem_G30, &gc_state.position); + break; + + case NonModal_SetCoordinateOffset: // G92 + memcpy(gc_state.g92_coord_offset, gc_block.values.xyz, sizeof(gc_state.g92_coord_offset)); +#if COMPATIBILITY_LEVEL <= 1 + settings_write_coord_data(CoordinateSystem_G92, &gc_state.g92_coord_offset); // Save G92 offsets to non-volatile storage +#endif + system_flag_wco_change(); + break; + + case NonModal_ResetCoordinateOffset: // G92.1 + clear_vector(gc_state.g92_coord_offset); // Disable G92 offsets by zeroing offset vector. + settings_write_coord_data(CoordinateSystem_G92, &gc_state.g92_coord_offset); // Save G92 offsets to non-volatile storage + system_flag_wco_change(); + break; + + case NonModal_ClearCoordinateOffset: // G92.2 + clear_vector(gc_state.g92_coord_offset); // Disable G92 offsets by zeroing offset vector. + system_flag_wco_change(); + break; + + case NonModal_RestoreCoordinateOffset: // G92.3 + settings_read_coord_data(CoordinateSystem_G92, &gc_state.g92_coord_offset); // Restore G92 offsets from non-volatile storage + system_flag_wco_change(); + break; + + default: + break; + } + + // [20. Motion modes ]: + // NOTE: Commands G10,G28,G30,G92 lock out and prevent axis words from use in motion modes. + // Enter motion modes only if there are axis words or a motion mode command word in the block. + gc_state.modal.motion = gc_block.modal.motion; + gc_state.modal.canned_cycle_active = gc_block.modal.canned_cycle_active; + + if (gc_state.modal.motion != MotionMode_None && axis_command == AxisCommand_MotionMode) { + + plan_data.output_commands = output_commands; + output_commands = NULL; + + pos_update_t gc_update_pos = GCUpdatePos_Target; + + switch(gc_state.modal.motion) { + + case MotionMode_Linear: + if(gc_state.modal.feed_mode == FeedMode_UnitsPerRev) { + plan_data.condition.spindle.synchronized = On; + //?? gc_state.distance_per_rev = plan_data.feed_rate; + // check initial feed rate - fail if zero? + } + mc_line(gc_block.values.xyz, &plan_data); + break; + + case MotionMode_Seek: + plan_data.condition.rapid_motion = On; // Set rapid motion condition flag. + mc_line(gc_block.values.xyz, &plan_data); + break; + + case MotionMode_CwArc: + case MotionMode_CcwArc: + // fail if spindle synchronized motion? + mc_arc(gc_block.values.xyz, &plan_data, gc_state.position, gc_block.values.ijk, gc_block.values.r, + plane, gc_parser_flags.arc_is_clockwise); + break; + + case MotionMode_CubicSpline: + mc_cubic_b_spline(gc_block.values.xyz, &plan_data, gc_state.position, gc_block.values.ijk, gc_state.modal.spline_pq); + break; + + case MotionMode_SpindleSynchronized: + { + protocol_buffer_synchronize(); // Wait until any previous moves are finished. + + gc_override_flags_t overrides = sys.override.control; // Save current override disable status. + + status_code_t status = init_sync_motion(&plan_data, gc_block.values.k); + if(status != Status_OK) + FAIL(status); + + plan_data.condition.spindle.synchronized = On; + + mc_line(gc_block.values.xyz, &plan_data); + + protocol_buffer_synchronize(); // Wait until synchronized move is finished, + sys.override.control = overrides; // then restore previous override disable status. + } + break; + + case MotionMode_Threading: + { + protocol_buffer_synchronize(); // Wait until any previous moves are finished. + + gc_override_flags_t overrides = sys.override.control; // Save current override disable status. + + status_code_t status = init_sync_motion(&plan_data, thread.pitch); + if(status != Status_OK) + FAIL(status); + + mc_thread(&plan_data, gc_state.position, &thread, overrides.feed_hold_disable); + + sys.override.control = overrides; // then restore previous override disable status. + } + break; + + case MotionMode_DrillChipBreak: + case MotionMode_CannedCycle81: + case MotionMode_CannedCycle82: + case MotionMode_CannedCycle83:; + plan_data.spindle.rpm = gc_block.values.s; + gc_state.canned.retract_mode = gc_state.modal.retract_mode; + mc_canned_drill(gc_state.modal.motion, gc_block.values.xyz, &plan_data, gc_state.position, plane, gc_block.values.l, &gc_state.canned); + break; + + case MotionMode_ProbeToward: + case MotionMode_ProbeTowardNoError: + case MotionMode_ProbeAway: + case MotionMode_ProbeAwayNoError: + // NOTE: gc_block.values.xyz is returned from mc_probe_cycle with the updated position value. So + // upon a successful probing cycle, the machine position and the returned value should be the same. + plan_data.condition.no_feed_override = !settings.probe.allow_feed_override; + gc_update_pos = (pos_update_t)mc_probe_cycle(gc_block.values.xyz, &plan_data, gc_parser_flags); + break; + + default: + break; + } + + // Do not update position on cancel (already done in protocol_exec_rt_system) + if(sys.cancel) + gc_update_pos = GCUpdatePos_None; + + // Clean out any remaining output commands (may linger on error) + while(plan_data.output_commands) { + output_command_t *next = plan_data.output_commands; + free(plan_data.output_commands); + plan_data.output_commands = next; + } + + // As far as the parser is concerned, the position is now == target. In reality the + // motion control system might still be processing the action and the real tool position + // in any intermediate location. + if (gc_update_pos == GCUpdatePos_Target) + memcpy(gc_state.position, gc_block.values.xyz, sizeof(gc_state.position)); // gc_state.position[] = gc_block.values.xyz[] + else if (gc_update_pos == GCUpdatePos_System) + gc_sync_position(); // gc_state.position[] = sys.position + // == GCUpdatePos_None + } + + if(plan_data.message) { + report_message(plan_data.message, Message_Plain); + free(plan_data.message); + } + + // [21. Program flow ]: + // M0,M1,M2,M30,M60: Perform non-running program flow actions. During a program pause, the buffer may + // refill and can only be resumed by the cycle start run-time command. + gc_state.modal.program_flow = gc_block.modal.program_flow; + + if (gc_state.modal.program_flow) { + + protocol_buffer_synchronize(); // Sync and finish all remaining buffered motions before moving on. + + if (gc_state.modal.program_flow == ProgramFlow_Paused || gc_block.modal.program_flow == ProgramFlow_OptionalStop || gc_block.modal.program_flow == ProgramFlow_CompletedM60) { + if (!check_mode) { + if(gc_block.modal.program_flow == ProgramFlow_CompletedM60 && hal.pallet_shuttle) + hal.pallet_shuttle(); + system_set_exec_state_flag(EXEC_FEED_HOLD); // Use feed hold for program pause. + protocol_execute_realtime(); // Execute suspend. + } + } else { // == ProgramFlow_Completed + // Upon program complete, only a subset of g-codes reset to certain defaults, according to + // LinuxCNC's program end descriptions and testing. Only modal groups [G-code 1,2,3,5,7,12] + // and [M-code 7,8,9] reset to [G1,G17,G90,G94,G40,G54,M5,M9,M48]. The remaining modal groups + // [G-code 4,6,8,10,13,14,15] and [M-code 4,5,6] and the modal words [F,S,T,H] do not reset. + + if(!check_mode && gc_block.modal.program_flow == ProgramFlow_CompletedM30 && hal.pallet_shuttle) + hal.pallet_shuttle(); + + gc_state.file_run = false; + gc_state.modal.motion = MotionMode_Linear; + gc_block.modal.canned_cycle_active = false; + gc_state.modal.plane_select = PlaneSelect_XY; +// gc_state.modal.plane_select = settings.flags.lathe_mode ? PlaneSelect_ZX : PlaneSelect_XY; + gc_state.modal.spindle_rpm_mode = SpindleSpeedMode_RPM; // NOTE: not compliant with linuxcnc (?) + gc_state.modal.distance_incremental = false; + gc_state.modal.feed_mode = FeedMode_UnitsPerMin; +// TODO: check gc_state.distance_per_rev = 0.0f; + // gc_state.modal.cutter_comp = CUTTER_COMP_DISABLE; // Not supported. + gc_state.modal.coord_system.id = CoordinateSystem_G54; + gc_state.modal.spindle = (spindle_state_t){0}; + gc_state.modal.coolant = (coolant_state_t){0}; + gc_state.modal.override_ctrl.feed_rate_disable = Off; + gc_state.modal.override_ctrl.spindle_rpm_disable = Off; + if(settings.parking.flags.enabled) + gc_state.modal.override_ctrl.parking_disable = settings.parking.flags.enable_override_control && + settings.parking.flags.deactivate_upon_init; + sys.override.control = gc_state.modal.override_ctrl; + + if(settings.flags.restore_overrides) { + sys.override.feed_rate = DEFAULT_FEED_OVERRIDE; + sys.override.rapid_rate = DEFAULT_RAPID_OVERRIDE; + sys.override.spindle_rpm = DEFAULT_SPINDLE_RPM_OVERRIDE; + } + + // Execute coordinate change and spindle/coolant stop. + if (!check_mode) { + + float g92_offset_stored[N_AXIS]; + if(settings_read_coord_data(CoordinateSystem_G92, &g92_offset_stored) && !isequal_position_vector(g92_offset_stored, gc_state.g92_coord_offset)) + settings_write_coord_data(CoordinateSystem_G92, &gc_state.g92_coord_offset); // Save G92 offsets to non-volatile storage + + if (!(settings_read_coord_data(gc_state.modal.coord_system.id, &gc_state.modal.coord_system.xyz))) + FAIL(Status_SettingReadFail); + system_flag_wco_change(); // Set to refresh immediately just in case something altered. + hal.spindle.set_state(gc_state.modal.spindle, 0.0f); + hal.coolant.set_state(gc_state.modal.coolant); + sys.report.spindle = On; // Set to report change immediately + sys.report.coolant = On; // ... + + if(grbl.on_program_completed) + grbl.on_program_completed(gc_state.modal.program_flow); + } + + // Clear any pending output commands + while(output_commands) { + output_command_t *next = output_commands->next; + free(output_commands); + output_commands = next; + } + + grbl.report.feedback_message(Message_ProgramEnd); + } + gc_state.modal.program_flow = ProgramFlow_Running; // Reset program flow. + } + + // TODO: % to denote start of program. + + return Status_OK; +} diff --git a/gcode.h b/gcode.h new file mode 100644 index 0000000..d689e53 --- /dev/null +++ b/gcode.h @@ -0,0 +1,480 @@ +/* + gcode.h - rs274/ngc parser. + + Part of grblHAL + + Copyright (c) 2017-2020 Terje Io + Copyright (c) 2011-2016 Sungeun K. Jeon for Gnea Research LLC + Copyright (c) 2009-2011 Simen Svale Skogsrud + + 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 . +*/ + +#ifndef _GCODE_H_ +#define _GCODE_H_ + +#include "nuts_bolts.h" +#include "coolant_control.h" +#include "spindle_control.h" +#include "errors.h" + +// Define command actions for within execution-type modal groups (motion, stopping, non-modal). Used +// internally by the parser to know which command to execute. +// NOTE: Some values are assigned specific values to make g-code state reporting and parsing +// compile a litte smaller. Although not +// ideal, just be careful with values that state 'do not alter' and check both report.c and gcode.c +// to see how they are used, if you need to alter them. + +// Modal Group G0: Non-modal actions +typedef enum { + NonModal_NoAction = 0, // (Default: Must be zero) + NonModal_Dwell = 4, // G4 (Do not alter value) + NonModal_SetCoordinateData = 10, // G10 (Do not alter value) + NonModal_GoHome_0 = 28, // G28 (Do not alter value) + NonModal_SetHome_0 = 38, // G28.1 (Do not alter value) + NonModal_GoHome_1 = 30, // G30 (Do not alter value) + NonModal_SetHome_1 = 40, // G30.1 (Do not alter value) + NonModal_AbsoluteOverride = 53, // G53 (Do not alter value) + NonModal_SetCoordinateOffset = 92, // G92 (Do not alter value) + NonModal_ResetCoordinateOffset = 102, // G92.1 (Do not alter value) + NonModal_ClearCoordinateOffset = 112, // G92.2 (Do not alter value) + NonModal_RestoreCoordinateOffset = 122 // G92.3 (Do not alter value) +} non_modal_t; + +// Modal Group G1: Motion modes +typedef enum { + MotionMode_Seek = 0, // G0 (Default: Must be zero) + MotionMode_Linear = 1, // G1 (Do not alter value) + MotionMode_CwArc = 2, // G2 (Do not alter value) + MotionMode_CcwArc = 3, // G3 (Do not alter value) + MotionMode_CubicSpline = 5, // G5 (Do not alter value) + MotionMode_SpindleSynchronized = 33, // G33 (Do not alter value) + MotionMode_DrillChipBreak = 73, // G73 (Do not alter value) + MotionMode_Threading = 76, // G76 (Do not alter value) + MotionMode_CannedCycle81 = 81, // G81 (Do not alter value) + MotionMode_CannedCycle82 = 82, // G82 (Do not alter value) + MotionMode_CannedCycle83 = 83, // G83 (Do not alter value) + MotionMode_CannedCycle85 = 85, // G85 (Do not alter value) + MotionMode_CannedCycle86 = 86, // G86 (Do not alter value) + MotionMode_CannedCycle89 = 89, // G89 (Do not alter value) + MotionMode_ProbeToward = 140, // G38.2 (Do not alter value) + MotionMode_ProbeTowardNoError = 141, // G38.3 (Do not alter value) + MotionMode_ProbeAway = 142, // G38.4 (Do not alter value) + MotionMode_ProbeAwayNoError = 143, // G38.5 (Do not alter value) + MotionMode_None = 80 // G80 (Do not alter value) +} motion_mode_t; + +// Modal Group G2: Plane select +typedef enum { + PlaneSelect_XY = 0, // G17 (Default: Must be zero) + PlaneSelect_ZX = 1, // G18 (Do not alter value) + PlaneSelect_YZ = 2 // G19 (Do not alter value) +} plane_select_t; + +// Modal Group G4: Arc IJK distance mode +//#define DISTANCE_ARC_MODE_INCREMENTAL 0 // G91.1 (Default: Must be zero) + +// Modal Group M4: Program flow +typedef enum { + ProgramFlow_Running = 0, // (Default: Must be zero) + ProgramFlow_Paused = 3, // M0 + ProgramFlow_OptionalStop = 1, // M1 + ProgramFlow_CompletedM2 = 2, // M2 (Do not alter value) + ProgramFlow_CompletedM30 = 30, // M30 (Do not alter value) + ProgramFlow_CompletedM60 = 60 // M60 (Do not alter value) +} program_flow_t; + +// Modal Group G5: Feed rate mode +typedef enum { + FeedMode_UnitsPerMin = 0, // G94 (Default: Must be zero) + FeedMode_InverseTime = 1, // G93 (Do not alter value) + FeedMode_UnitsPerRev = 2 // G95 (Do not alter value) +} feed_mode_t; + +// Modal Group G10: Canned cycle return mode +typedef enum { + CCRetractMode_Previous = 0, // G98 (Default: Must be zero) + CCRetractMode_RPos = 1 // G99 (Do not alter value) +} cc_retract_mode_t; + +// Modal Group G7: Cutter radius compensation mode +//#define CUTTER_COMP_DISABLE 0 // G40 (Default: Must be zero) + +// Modal Group G13: Control mode +//#define CONTROL_MODE_EXACT_PATH 0 // G61 (Default: Must be zero) + +// Modal Group G8: Tool length offset +typedef enum { + ToolLengthOffset_Cancel = 0, // G49 (Default: Must be zero) + ToolLengthOffset_Enable = 1, // G43 + ToolLengthOffset_EnableDynamic = 2, // G43.1 + ToolLengthOffset_ApplyAdditional = 3 // G43.2 +} tool_offset_mode_t; + +// Modal Group M9: Override control +typedef enum { + Override_Parking = 56, // M56 + Override_FeedHold = 53, // M53 + Override_FeedSpeed = 49, // M49 + Override_FeedRate = 50, // M50 + Override_SpindleSpeed = 51 // M51 +} override_mode_t; + +// Modal Group G12: Active work coordinate system +// N/A: Stores coordinate system value (54-59) to change to. + +// Modal Group G14: Spindle Speed Mode +typedef enum { + SpindleSpeedMode_RPM = 0, // G96 (Default: Must be zero) + SpindleSpeedMode_CSS = 1 // G97 (Do not alter value) +} spindle_rpm_mode_t; + +typedef struct output_command { + bool is_digital; + bool is_executed; + uint8_t port; + int32_t value; + struct output_command *next; +} output_command_t; + +typedef enum { + WaitMode_Immediate = 0, + WaitMode_Rise, + WaitMode_Fall, + WaitMode_High, + WaitMode_Low, + WaitMode_Max // Used for validation only +} wait_mode_t; + +// Modal Group M10: User defined M commands +// NOTE: Not used by core, may be used by driver code +typedef enum { + UserMCode_Ignore = 0, + UserMCode_Generic0 = 100, + UserMCode_Generic1 = 101, + UserMCode_Generic2 = 102, + UserMCode_Generic3 = 103, + UserMCode_Generic4 = 104, + LaserPPI_Enable = 112, + LaserPPI_Rate = 113, + LaserPPI_PulseLength = 114, + Laser_Coolant = 115, + Trinamic_DebugReport = 122, + Trinamic_StepperCurrent = 906, + Trinamic_ModeToggle = 569, + Trinamic_ReportPrewarnFlags = 911, + Trinamic_ClearPrewarnFlags = 912, + Trinamic_HybridThreshold = 913, + Trinamic_HomingSensitivity = 914 +} user_mcode_t; + +// Define g-code parser position updating flags +typedef enum { + GCUpdatePos_Target = 0, + GCUpdatePos_System, + GCUpdatePos_None +} pos_update_t; + +// Define probe cycle exit states and assign proper position updating. +typedef enum { + GCProbe_Found = GCUpdatePos_System, + GCProbe_Abort = GCUpdatePos_None, + GCProbe_FailInit = GCUpdatePos_None, + GCProbe_FailEnd = GCUpdatePos_Target, + #ifdef SET_CHECK_MODE_PROBE_TO_START + GCProbe_CheckMode = GCUpdatePos_None + #else + GCProbe_CheckMode = GCUpdatePos_Target + #endif +} gc_probe_t; + +// Define parser words bitfield +typedef union { + uint32_t mask; + uint32_t value; + struct { + uint32_t e :1, + f :1, + h :1, + i :1, + j :1, + k :1, + l :1, + n :1, + p :1, + r :1, + s :1, + t :1, + x :1, + y :1, + z :1, + q :1, +#if N_AXIS > 3 + a :1, + b :1, + c :1, +#endif + d :1; + }; +} parameter_words_t; + +// Define gcode parser flags for handling special cases. + +typedef union { + uint16_t value; + struct { + uint16_t jog_motion :1, + canned_cycle_change :1, // Use motion_mode_changed? + arc_is_clockwise :1, + probe_is_away :1, + probe_is_no_error :1, + spindle_force_sync :1, + laser_disable :1, + laser_is_motion :1, + set_coolant :1, + motion_mode_changed :1, + reserved :6; + }; +} gc_parser_flags_t; + +typedef union { + uint8_t value; + struct { + uint8_t feed_rate_disable :1, + feed_hold_disable :1, + spindle_rpm_disable :1, + parking_disable :1, + reserved :3, + sync :1; + }; +} gc_override_flags_t; + +typedef union { + float values[N_AXIS]; + struct { + float x; + float y; + float z; +#ifdef A_AXIS + float a; +#endif +#ifdef B_AXIS + float b; +#endif +#ifdef C_AXIS + float c; +#endif + }; +} coord_data_t; + +typedef enum { + CoordinateSystem_G54 = 0, + CoordinateSystem_G55, + CoordinateSystem_G56, + CoordinateSystem_G57, + CoordinateSystem_G58, + CoordinateSystem_G59, +#if COMPATIBILITY_LEVEL <= 1 + CoordinateSystem_G59_1, + CoordinateSystem_G59_2, + CoordinateSystem_G59_3, +#endif + N_WorkCoordinateSystems, + CoordinateSystem_G28 = N_WorkCoordinateSystems, + CoordinateSystem_G30, + CoordinateSystem_G92, + N_CoordinateSystems +} coord_system_id_t; + +typedef struct { + float xyz[N_AXIS]; + coord_system_id_t id; +} coord_system_t; + +typedef struct { + uint8_t axis_0; + uint8_t axis_1; + uint8_t axis_linear; +} plane_t; + +// NOTE: When this struct is zeroed, the above defines set the defaults for the system. +typedef struct { + motion_mode_t motion; // {G0,G1,G2,G3,G38.2,G80} + feed_mode_t feed_mode; // {G93,G94} + bool units_imperial; // {G20,G21} + bool distance_incremental; // {G90,G91} + bool diameter_mode; // {G7,G8} Lathe diameter mode. + // uint8_t distance_arc; // {G91.1} NOTE: Don't track. Only default supported. + plane_select_t plane_select; // {G17,G18,G19} + // uint8_t cutter_comp; // {G40} NOTE: Don't track. Only default supported. + tool_offset_mode_t tool_offset_mode; // {G43,G43.1,G49} + coord_system_t coord_system; // {G54,G55,G56,G57,G58,G59,G59.1,G59.2,G59.3} + // uint8_t control; // {G61} NOTE: Don't track. Only default supported. + program_flow_t program_flow; // {M0,M1,M2,M30,M60} + coolant_state_t coolant; // {M7,M8,M9} + spindle_state_t spindle; // {M3,M4,M5} + gc_override_flags_t override_ctrl; // {M48,M49,M50,M51,M53,M56} + spindle_rpm_mode_t spindle_rpm_mode; // {G96,G97} + cc_retract_mode_t retract_mode; // {G98,G99} + bool scaling_active; // {G50,G51} + bool canned_cycle_active; + float spline_pq[2]; // {G5} +} gc_modal_t; + +typedef struct { + float d; // Max spindle RPM in Constant Surface Speed Mode (G96) + float e; // Thread taper length (G76) + float f; // Feed + float ijk[3]; // I,J,K Axis arc offsets + float k; // G33 distance per revolution + float p; // G10 or dwell parameters + float q; // User defined M-code parameter (G82 peck drilling, not supported) + float r; // Arc radius or retract position + float s; // Spindle speed + float xyz[N_AXIS]; // X,Y,Z Translational axes + coord_system_t coord_data; // Coordinate data + int32_t n; // Line number + uint32_t h; // Tool number + uint32_t t; // Tool selection + uint8_t l; // G10 or canned cycles parameters +} gc_values_t; + +typedef struct { + float xyz[3]; + float delta; + float dwell; + float prev_position; + float retract_position; // Canned cycle retract position + bool rapid_retract; + bool spindle_off; + cc_retract_mode_t retract_mode; + bool change; +} gc_canned_t; + +typedef enum { + Taper_None = 0, + Taper_Entry, + Taper_Exit, + Taper_Both +} gc_taper_type; + +typedef struct { + float pitch; + float z_final; + float peak; + float initial_depth; + float depth; + float depth_degression; + float main_taper_height; + float end_taper_length; + float infeed_angle; + float cut_direction; + uint_fast16_t spring_passes; + gc_taper_type end_taper_type; +} gc_thread_data; + +typedef struct { + float offset[N_AXIS]; + float radius; + uint32_t tool; +} tool_data_t; + +// Data used for Constant Surface Speed Mode calculations +typedef struct { + float surface_speed; // Surface speed in millimeters/min + float target_rpm; // Target RPM at end of movement + float max_rpm; // Maximum spindle RPM + float tool_offset; // Tool offset + uint_fast8_t axis; // Linear (tool) axis + bool active; +} css_data_t; + +typedef struct { + float rpm; // RPM + css_data_t css; // Data used for Constant Surface Speed Mode calculations +} spindle_t; + +typedef struct { + gc_modal_t modal; + gc_canned_t canned; + spindle_t spindle; // RPM + float feed_rate; // Millimeters/min + float distance_per_rev; // Millimeters/rev + float position[N_AXIS]; // Where the interpreter considers the tool to be at this point in the code + int32_t line_number; // Last line number sent + uint32_t tool_pending; // Tool to be selected on next M6 + bool file_run; // Tracks % command + bool is_laser_ppi_mode; + bool is_rpm_rate_adjusted; + bool tool_change; + status_code_t last_error; // last return value from parser + // The following variables are not cleared upon warm restart when COMPATIBILITY_LEVEL <= 1 + float g92_coord_offset[N_AXIS]; // Retains the G92 coordinate offset (work coordinates) relative to + // machine zero in mm. Persistent and loaded from non-volatile storage + // on boot when COMPATIBILITY_LEVEL <= 1 + float tool_length_offset[N_AXIS]; // Tracks tool length offset when enabled + tool_data_t *tool; // Tracks tool number and tool offset +} parser_state_t; + +typedef struct { + float xyz[N_AXIS]; // Center point + float ijk[N_AXIS]; // Scaling factors +} scale_factor_t; + +extern parser_state_t gc_state; +#ifdef N_TOOLS +extern tool_data_t tool_table[N_TOOLS + 1]; +#else +extern tool_data_t tool_table; +#endif + +typedef struct { + non_modal_t non_modal_command; + override_mode_t override_command; // TODO: add to non_modal above? + user_mcode_t user_mcode; + bool user_mcode_sync; + gc_modal_t modal; + gc_values_t values; + output_command_t output_command; +} parser_block_t; + +// Initialize the parser +void gc_init (void); + +// Execute one block of rs275/ngc/g-code +status_code_t gc_execute_block (char *block, char *message); + +// Sets g-code parser position in mm. Input in steps. Called by the system abort and hard +// limit pull-off routines. +#define gc_sync_position() system_convert_array_steps_to_mpos (gc_state.position, sys.position) + +// Sets g-code parser and planner position in mm. +#define sync_position() plan_sync_position(); system_convert_array_steps_to_mpos (gc_state.position, sys.position) + +// Set dynamic laser power mode to PPI (Pulses Per Inch) +// Driver support for pulsing the laser on signal is required for this to work. +// Returns true if driver uses hardware implementation. +bool gc_laser_ppi_enable (uint_fast16_t ppi, uint_fast16_t pulse_length); + +// Gets axes scaling state. +axes_signals_t gc_get_g51_state (void); +float *gc_get_scaling (void); + +// Get current axis offset. +float gc_get_offset (uint_fast8_t idx); + +void gc_set_tool_offset (tool_offset_mode_t mode, uint_fast8_t idx, int32_t offset); +plane_t *gc_get_plane_data (plane_t *plane, plane_select_t select); + +#endif diff --git a/grbl.h b/grbl.h new file mode 100644 index 0000000..b89c972 --- /dev/null +++ b/grbl.h @@ -0,0 +1,276 @@ +/* + grbl.h - main Grbl include file for compile time configuration + + Part of grblHAL + + Copyright (c) 2017-2021 Terje Io + Copyright (c) 2015-2016 Sungeun K. Jeon for Gnea Research LLC + + Grbl is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + Grbl is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with Grbl. If not, see . +*/ + +#ifndef _GRBL_H_ +#define _GRBL_H_ + +#include +#include + +#include "config.h" + +// Grbl versioning system +#if COMPATIBILITY_LEVEL == 0 +#define GRBL_VERSION "1.1f" +#else +#define GRBL_VERSION "1.1f" +#endif +#define GRBL_VERSION_BUILD "20210222" + +// The following symbols are set here if not already set by the compiler or in config.h +// Do NOT change here! + +#ifdef GRBL_ESP32 +#include "esp_attr.h" +#define ISR_CODE IRAM_ATTR +#else +// Used to decorate code run in interrupt context. +// Do not remove or change unless you know what you are doing. +#define ISR_CODE +#endif + +#ifdef ARDUINO +#include +#endif + +#ifndef PROGMEM +#define PROGMEM +#endif + +#ifndef N_AXIS +#define N_AXIS 3 // Number of axes +#endif + +#ifndef COMPATIBILITY_LEVEL +#define COMPATIBILITY_LEVEL 0 +#endif + +#if (defined(COREXY) || defined(WALL_PLOTTER) || defined(MASLOW_ROUTER)) && !defined(KINEMATICS_API) +#define KINEMATICS_API +#endif + +#ifndef CHECK_MODE_DELAY +#define CHECK_MODE_DELAY 0 // ms +#endif + +#ifndef SAFETY_DOOR_SPINDLE_DELAY +#define SAFETY_DOOR_SPINDLE_DELAY 4.0f // Float (seconds) +#endif + +#ifndef SAFETY_DOOR_COOLANT_DELAY +#define SAFETY_DOOR_COOLANT_DELAY 1.0f // Float (seconds) +#endif + +#ifndef SPINDLE_RPM_CONTROLLED +#define SPINDLE_PWM_DIRECT +#endif + +#ifndef SLEEP_DURATION +#define SLEEP_DURATION 5.0f // Number of minutes before sleep mode is entered. +#endif + +#ifndef BUFFER_NVSDATA_DISABLE +#define BUFFER_NVSDATA +#endif + +// The following symbols are default values that are unlikely to be changed +// Do not change unless you know what you are doing! + +// Define realtime command special characters. These characters are 'picked-off' directly from the +// serial read data stream and are not passed to the grbl line execution parser. Select characters +// that do not and must not exist in the streamed g-code program. ASCII control characters may be +// used, if they are available per user setup. Also, extended ASCII codes (>127), which are never in +// g-code programs, maybe selected for interface programs. +// NOTE: If changed, manually update help message in report.c. + +#define CMD_EXIT 0x03 // ctrl-C (ETX) +#define CMD_REBOOT 0x14 // ctrl-T (DC4) - only acted upon if preceeded by 0x1B (ESC) +#define CMD_RESET 0x18 // ctrl-X (CAN) +#define CMD_STOP 0x19 // ctrl-Y (EM) +#define CMD_STATUS_REPORT_LEGACY '?' +#define CMD_CYCLE_START_LEGACY '~' +#define CMD_FEED_HOLD_LEGACY '!' +#define CMD_PROGRAM_DEMARCATION '%' + +// NOTE: All override realtime commands must be in the extended ASCII character set, starting +// at character value 128 (0x80) and up to 255 (0xFF). If the normal set of realtime commands, +// such as status reports, feed hold, reset, and cycle start, are moved to the extended set +// space, protocol.c's protocol_process_realtime() will need to be modified to accomodate the change. +#define CMD_STATUS_REPORT 0x80 // TODO: use 0x05 ctrl-E ENQ instead? +#define CMD_CYCLE_START 0x81 // TODO: use 0x06 ctrl-F ACK instead? or SYN/DC2/DC3? +#define CMD_FEED_HOLD 0x82 // TODO: use 0x15 ctrl-U NAK instead? +#define CMD_GCODE_REPORT 0x83 +#define CMD_SAFETY_DOOR 0x84 +#define CMD_JOG_CANCEL 0x85 +//#define CMD_DEBUG_REPORT 0x86 // Only when DEBUG enabled, sends debug report in '{}' braces. +#define CMD_STATUS_REPORT_ALL 0x87 +#define CMD_OPTIONAL_STOP_TOGGLE 0x88 +#define CMD_OVERRIDE_FEED_RESET 0x90 // Restores feed override value to 100%. +#define CMD_OVERRIDE_FEED_COARSE_PLUS 0x91 +#define CMD_OVERRIDE_FEED_COARSE_MINUS 0x92 +#define CMD_OVERRIDE_FEED_FINE_PLUS 0x93 +#define CMD_OVERRIDE_FEED_FINE_MINUS 0x94 +#define CMD_OVERRIDE_RAPID_RESET 0x95 // Restores rapid override value to 100%. +#define CMD_OVERRIDE_RAPID_MEDIUM 0x96 +#define CMD_OVERRIDE_RAPID_LOW 0x97 +// #define CMD_OVERRIDE_RAPID_EXTRA_LOW 0x98 // *NOT SUPPORTED* +#define CMD_OVERRIDE_SPINDLE_RESET 0x99 // Restores spindle override value to 100%. +#define CMD_OVERRIDE_SPINDLE_COARSE_PLUS 0x9A +#define CMD_OVERRIDE_SPINDLE_COARSE_MINUS 0x9B +#define CMD_OVERRIDE_SPINDLE_FINE_PLUS 0x9C +#define CMD_OVERRIDE_SPINDLE_FINE_MINUS 0x9D +#define CMD_OVERRIDE_SPINDLE_STOP 0x9E +#define CMD_OVERRIDE_COOLANT_FLOOD_TOGGLE 0xA0 +#define CMD_OVERRIDE_COOLANT_MIST_TOGGLE 0xA1 +#define CMD_PID_REPORT 0xA2 +#define CMD_TOOL_ACK 0xA3 +#define CMD_PROBE_CONNECTED_TOGGLE 0xA4 + +// System motion line numbers must be zero. +#define JOG_LINE_NUMBER 0 + +// Number of blocks Grbl executes upon startup. These blocks are stored in non-volatile storage, where the size +// and addresses are defined in settings.h. With the current settings, up to 2 startup blocks may +// be stored and executed in order. These startup blocks would typically be used to set the g-code +// parser state depending on user preferences. +#define N_STARTUP_LINE 2 // Integer (1-2) + +// Number of decimal places (scale) output by Grbl for certain value types. These settings +// are determined by realistic and commonly observed values in CNC machines. For example, position +// values cannot be less than 0.001mm or 0.0001in, because machines can not be physically more +// precise this. So, there is likely no need to change these, but you can if you need to here. +// NOTE: Must be an integer value from 0 to ~4. More than 4 may exhibit round-off errors. +#define N_DECIMAL_COORDVALUE_INCH 4 // Coordinate or position value in inches +#define N_DECIMAL_COORDVALUE_MM 3 // Coordinate or position value in mm +#define N_DECIMAL_RATEVALUE_INCH 1 // Rate or velocity value in in/min +#define N_DECIMAL_RATEVALUE_MM 0 // Rate or velocity value in mm/min +#define N_DECIMAL_SETTINGVALUE 3 // Floating point setting values +#define N_DECIMAL_RPMVALUE 0 // RPM value in rotations per min +#define N_DECIMAL_PIDVALUE 3 // PID value + +// --------------------------------------------------------------------------------------- +// ADVANCED CONFIGURATION OPTIONS: + +// Enables code for debugging purposes. Not for general use and always in constant flux. +// #define DEBUG // Uncomment to enable. Default disabled. + +// Configure rapid, feed, and spindle override settings. These values define the max and min +// allowable override values and the coarse and fine increments per command received. Please +// note the allowable values in the descriptions following each define. +#define DEFAULT_FEED_OVERRIDE 100 // 100%. Don't change this value. +#define MAX_FEED_RATE_OVERRIDE 200 // Percent of programmed feed rate (100-255). Usually 120% or 200% +#define MIN_FEED_RATE_OVERRIDE 10 // Percent of programmed feed rate (1-100). Usually 50% or 1% +#define FEED_OVERRIDE_COARSE_INCREMENT 10 // (1-99). Usually 10%. +#define FEED_OVERRIDE_FINE_INCREMENT 1 // (1-99). Usually 1%. + +#define DEFAULT_RAPID_OVERRIDE 100 // 100%. Don't change this value. +#define RAPID_OVERRIDE_MEDIUM 50 // Percent of rapid (1-99). Usually 50%. +#define RAPID_OVERRIDE_LOW 25 // Percent of rapid (1-99). Usually 25%. +// #define RAPID_OVERRIDE_EXTRA_LOW 5 // *NOT SUPPORTED* Percent of rapid (1-99). Usually 5%. + +// #define ENABLE_SPINDLE_LINEARIZATION // Uncomment to enable spindle RPM linearization. Requires compatible driver if enabled. +#define SPINDLE_NPWM_PIECES 4 // Maximum number of pieces for spindle RPM linearization, do not change unless more are needed. +#define DEFAULT_SPINDLE_RPM_OVERRIDE 100 // 100%. Don't change this value. +#define MAX_SPINDLE_RPM_OVERRIDE 200 // Percent of programmed spindle speed (100-255). Usually 200%. +#define MIN_SPINDLE_RPM_OVERRIDE 10 // Percent of programmed spindle speed (1-100). Usually 10%. +#define SPINDLE_OVERRIDE_COARSE_INCREMENT 10 // (1-99). Usually 10%. +#define SPINDLE_OVERRIDE_FINE_INCREMENT 1 // (1-99). Usually 1%. + +// Some status report data isn't necessary for realtime, only intermittently, because the values don't +// change often. The following macros configures how many times a status report needs to be called before +// the associated data is refreshed and included in the status report. However, if one of these value +// changes, Grbl will automatically include this data in the next status report, regardless of what the +// count is at the time. This helps reduce the communication overhead involved with high frequency reporting +// and agressive streaming. There is also a busy and an idle refresh count, which sets up Grbl to send +// refreshes more often when its not doing anything important. With a good GUI, this data doesn't need +// to be refreshed very often, on the order of a several seconds. +// NOTE: WCO refresh must be 2 or greater. OVERRIDE refresh must be 1 or greater. +//#define REPORT_OVERRIDE_REFRESH_BUSY_COUNT 20 // (1-255) +//#define REPORT_OVERRIDE_REFRESH_IDLE_COUNT 10 // (1-255) Must be less than or equal to the busy count +//#define REPORT_WCO_REFRESH_BUSY_COUNT 30 // (2-255) +//#define REPORT_WCO_REFRESH_IDLE_COUNT 10 // (2-255) Must be less than or equal to the busy count + +// The temporal resolution of the acceleration management subsystem. A higher number gives smoother +// acceleration, particularly noticeable on machines that run at very high feedrates, but may negatively +// impact performance. The correct value for this parameter is machine dependent, so it's advised to +// set this only as high as needed. Approximate successful values can widely range from 50 to 200 or more. +// NOTE: Changing this value also changes the execution time of a segment in the step segment buffer. +// When increasing this value, this stores less overall time in the segment buffer and vice versa. Make +// certain the step segment buffer is increased/decreased to account for these changes. +//#define ACCELERATION_TICKS_PER_SECOND 100 + +// Adaptive Multi-Axis Step Smoothing (AMASS) is an advanced feature that does what its name implies, +// smoothing the stepping of multi-axis motions. This feature smooths motion particularly at low step +// frequencies below 10kHz, where the aliasing between axes of multi-axis motions can cause audible +// noise and shake your machine. At even lower step frequencies, AMASS adapts and provides even better +// step smoothing. See stepper.c for more details on the AMASS system works. +#define ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING // Default enabled. Comment to disable. + +// Define Adaptive Multi-Axis Step-Smoothing(AMASS) levels and cutoff frequencies. The highest level +// frequency bin starts at 0Hz and ends at its cutoff frequency. The next lower level frequency bin +// starts at the next higher cutoff frequency, and so on. The cutoff frequencies for each level must +// be considered carefully against how much it over-drives the stepper ISR, the accuracy of the 16-bit +// timer, and the CPU overhead. Level 0 (no AMASS, normal operation) frequency bin starts at the +// Level 1 cutoff frequency and up to as fast as the CPU allows (over 30kHz in limited testing). +// NOTE: AMASS cutoff frequency multiplied by ISR overdrive factor must not exceed maximum step frequency. +// NOTE: Current settings are set to overdrive the ISR to no more than 16kHz, balancing CPU overhead +// and timer accuracy. Do not alter these settings unless you know what you are doing. +#ifdef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING + #ifndef MAX_AMASS_LEVEL + #define MAX_AMASS_LEVEL 3 + #endif + #if MAX_AMASS_LEVEL <= 0 + error "AMASS must have 1 or more levels to operate correctly." + #endif +#endif + +// Sets the maximum step rate allowed to be written as a Grbl setting. This option enables an error +// check in the settings module to prevent settings values that will exceed this limitation. The maximum +// step rate is strictly limited by the CPU speed and will change if something other than an AVR running +// at 16MHz is used. +// NOTE: For now disabled, will enable if flash space permits. +// #define MAX_STEP_RATE_HZ 30000 // Hz + +// With this enabled, Grbl sends back an echo of the line it has received, which has been pre-parsed (spaces +// removed, capitalized letters, no comments) and is to be immediately executed by Grbl. Echoes will not be +// sent upon a line buffer overflow, but should for all normal lines sent to Grbl. For example, if a user +// sendss the line 'g1 x1.032 y2.45 (test comment)', Grbl will echo back in the form '[echo: G1X1.032Y2.45]'. +// NOTE: Only use this for debugging purposes!! When echoing, this takes up valuable resources and can effect +// performance. If absolutely needed for normal operation, the serial write buffer should be greatly increased +// to help minimize transmission waiting within the serial write protocol. +// #define REPORT_ECHO_LINE_RECEIVED // Default disabled. Uncomment to enable. + +// Sets which axis the tool length offset is applied. Assumes the spindle is always parallel with +// the selected axis with the tool oriented toward the negative direction. In other words, a positive +// tool length offset value is subtracted from the current location. +#if COMPATIBILITY_LEVEL > 2 && defined(TOOL_LENGTH_OFFSET_AXIS) == 0 +#define TOOL_LENGTH_OFFSET_AXIS Z_AXIS // Default z-axis. Valid values are X_AXIS, Y_AXIS, or Z_AXIS. +#endif + +// Max length of gcode lines (blocks) stored in non-volatile storage +#if N_AXIS == 6 && COMPATIBILITY_LEVEL <= 1 +#define MAX_STORED_LINE_LENGTH 60 // do not change! +#else +#define MAX_STORED_LINE_LENGTH 70 // do not set > 70 unless less than 5 axes are enabled or COMPATIBILITY_LEVEL > 1 +#endif + +#endif diff --git a/grbllib.c b/grbllib.c new file mode 100644 index 0000000..6863b01 --- /dev/null +++ b/grbllib.c @@ -0,0 +1,277 @@ +/* + grbllib.c - An embedded CNC Controller with rs274/ngc (g-code) support + + Part of grblHAL + + Copyright (c) 2017-2021 Terje Io + Copyright (c) 2011-2015 Sungeun K. Jeon + Copyright (c) 2009-2011 Simen Svale Skogsrud + + 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 . +*/ + +#include +#include +#include + +#include "hal.h" +#include "nuts_bolts.h" +#include "tool_change.h" +#include "override.h" +#include "protocol.h" +#include "limits.h" +#include "report.h" +#include "state_machine.h" +#include "nvs_buffer.h" +#ifdef ENABLE_BACKLASH_COMPENSATION +#include "motion_control.h" +#endif +#ifdef KINEMATICS_API +#include "kinematics.h" +#endif + +#ifdef COREXY +#include "corexy.h" +#endif + +#ifdef WALL_PLOTTER +#include "wall_plotter.h" +#endif + +// Declare system global variable structure +system_t sys = {0}; +grbl_t grbl; +grbl_hal_t hal; + +// called from stream drivers while tx is blocking, return false to terminate + +static bool stream_tx_blocking (void) +{ + // TODO: Restructure st_prep_buffer() calls to be executed here during a long print. + + grbl.on_execute_realtime(state_get()); + + return !(sys.rt_exec_state & EXEC_RESET); +} + +#ifdef KINEMATICS_API + +kinematics_t kinematics; + +// called from mc_line() to segment lines if not overridden, default implementation for pass-through +static bool kinematics_segment_line (float *target, plan_line_data_t *pl_data, bool init) +{ + static uint_fast8_t iterations; + + if(init) + iterations = 2; + else + iterations--; + + return iterations != 0; +} + +#endif + +#ifdef DEBUGOUT +static void debug_out (bool on) +{ + // NOOP +} +#endif + +void dummy_bool_handler (bool arg) +{ + // NOOP +} + +// main entry point + +int grbl_enter (void) +{ +#ifdef N_TOOLS + assert(NVS_ADDR_GLOBAL + sizeof(settings_t) + NVS_CRC_BYTES < NVS_ADDR_TOOL_TABLE); +#else + assert(NVS_ADDR_GLOBAL + sizeof(settings_t) + NVS_CRC_BYTES < NVS_ADDR_PARAMETERS); +#endif + assert(NVS_ADDR_PARAMETERS + N_CoordinateSystems * (sizeof(coord_data_t) + NVS_CRC_BYTES) < NVS_ADDR_STARTUP_BLOCK); + assert(NVS_ADDR_STARTUP_BLOCK + N_STARTUP_LINE * (sizeof(stored_line_t) + NVS_CRC_BYTES) < NVS_ADDR_BUILD_INFO); + + bool looping = true, driver_ok; + + // Clear all and set some core function pointers + memset(&grbl, 0, sizeof(grbl_t)); + grbl.on_execute_realtime = protocol_execute_noop; + grbl.protocol_enqueue_gcode = protocol_enqueue_gcode; + grbl.on_report_options = dummy_bool_handler; + + // Clear all and set some HAL function pointers + memset(&hal, 0, sizeof(grbl_hal_t)); + hal.version = HAL_VERSION; // Update when signatures and/or contract is changed - driver_init() should fail + hal.driver_reset = dummy_handler; + hal.irq_enable = dummy_handler; + hal.irq_disable = dummy_handler; + hal.nvs.size = GRBL_NVS_SIZE; + hal.stream.enqueue_realtime_command = protocol_enqueue_realtime_command; + hal.limits.interrupt_callback = limit_interrupt_handler; + hal.control.interrupt_callback = control_interrupt_handler; + hal.stepper.interrupt_callback = stepper_driver_interrupt_handler; + hal.stream_blocking_callback = stream_tx_blocking; + hal.signals_cap.reset = hal.signals_cap.feed_hold = hal.signals_cap.cycle_start = On; + + sys.cold_start = true; + +#ifdef BUFFER_NVSDATA + nvs_buffer_alloc(); // Allocate memory block for NVS buffer +#endif + + report_init_fns(); + +#ifdef KINEMATICS_API + memset(&kinematics, 0, sizeof(kinematics_t)); + + kinematics.segment_line = kinematics_segment_line; // default to no segmentation +#endif + +#ifdef DEBUGOUT + hal.debug_out = debug_out; // must be overridden by driver to have any effect +#endif + driver_ok = driver_init(); + +#if COMPATIBILITY_LEVEL > 0 + hal.stream.suspend_read = NULL; +#endif + +#ifndef ENABLE_SAFETY_DOOR_INPUT_PIN + hal.signals_cap.safety_door_ajar = Off; +#else + driver_ok &= hal.signals_cap.safety_door_ajar; +#endif + + #ifdef BUFFER_NVSDATA + nvs_buffer_init(); + #endif + settings_init(); // Load Grbl settings from non-volatile storage + + memset(sys.position, 0, sizeof(sys.position)); // Clear machine position. + +// check and configure driver + +#ifdef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING + driver_ok = driver_ok && hal.driver_cap.amass_level >= MAX_AMASS_LEVEL; + hal.driver_cap.amass_level = MAX_AMASS_LEVEL; +#else + hal.driver_cap.amass_level = 0; +#endif + +#ifdef DEFAULT_STEP_PULSE_DELAY + driver_ok = driver_ok & hal.driver_cap.step_pulse_delay; +#endif +/* +#if AXIS_N_SETTINGS > 4 + driver_ok = driver_ok & hal.driver_cap.axes >= AXIS_N_SETTINGS; +#endif +*/ + sys.mpg_mode = false; + + driver_ok = driver_ok && hal.driver_setup(&settings); + +#ifdef ENABLE_SPINDLE_LINEARIZATION + driver_ok = driver_ok && hal.driver_cap.spindle_pwm_linearization; +#endif + +#ifdef SPINDLE_PWM_DIRECT + driver_ok = driver_ok && hal.spindle.get_pwm != NULL && hal.spindle.update_pwm != NULL; +#endif + + if(!driver_ok) { + hal.stream.write("grblHAL: incompatible driver" ASCII_EOL); + while(true); + } + + if(hal.get_position) + hal.get_position(&sys.position); // TODO: restore on abort when returns true? + +#ifdef COREXY + corexy_init(); +#endif + +#ifdef WALL_PLOTTER + wall_plotter_init(); +#endif + + sys.driver_started = true; + + // "Wire" homing switches to limit switches if not provided by the driver. + if(hal.homing.get_state == NULL) + hal.homing.get_state = hal.limits.get_state; + + // Grbl initialization loop upon power-up or a system abort. For the latter, all processes + // will return to this loop to be cleanly re-initialized. + while(looping) { + + // Reset report entry points + report_init_fns(); + + if(!sys.position_lost || settings.homing.flags.keep_on_reset) + memset(&sys, 0, offsetof(system_t, homed)); // Clear system variables except alarm & homed status. + else + memset(&sys, 0, offsetof(system_t, alarm)); // Clear system variables except state & alarm. + + sys.override.feed_rate = DEFAULT_FEED_OVERRIDE; // Set to 100% + sys.override.rapid_rate = DEFAULT_RAPID_OVERRIDE; // Set to 100% + sys.override.spindle_rpm = DEFAULT_SPINDLE_RPM_OVERRIDE; // Set to 100% + + if(settings.parking.flags.enabled) + sys.override.control.parking_disable = settings.parking.flags.deactivate_upon_init; + + flush_override_buffers(); + + // Reset Grbl primary systems. + hal.stream.reset_read_buffer(); // Clear input stream buffer + gc_init(); // Set g-code parser to default state + hal.limits.enable(settings.limits.flags.hard_enabled, false); + plan_reset(); // Clear block buffer and planner variables + st_reset(); // Clear stepper subsystem variables. + limits_set_homing_axes(); // Set axes to be homed from settings. +#ifdef ENABLE_BACKLASH_COMPENSATION + mc_backlash_init(); // Init backlash configuration. +#endif + // Sync cleared gcode and planner positions to current system position. + sync_position(); + + if(hal.stepper.disable_motors) + hal.stepper.disable_motors((axes_signals_t){0}, SquaringMode_Both); + + if(!hal.driver_cap.atc) + tc_init(); + + // Print welcome message. Indicates an initialization has occured at power-up or with a reset. + report_init_message(); + + if(state_get() == STATE_ESTOP) + state_set(STATE_ALARM); + + if(hal.driver_cap.mpg_mode) + hal.stream.enqueue_realtime_command(sys.mpg_mode ? CMD_STATUS_REPORT_ALL : CMD_STATUS_REPORT); + + // Start Grbl main loop. Processes program inputs and executes them. + if(!(looping = protocol_main_loop())) + looping = hal.driver_release == NULL || hal.driver_release(); + + sys.cold_start = false; + } + + return 0; +} diff --git a/grbllib.h b/grbllib.h new file mode 100644 index 0000000..e03e52d --- /dev/null +++ b/grbllib.h @@ -0,0 +1,39 @@ +/* + grbllib.h - An embedded CNC Controller with rs274/ngc (g-code) support + + Part of grblHAL + + Copyright (c) 2011-2015 Sungeun K. Jeon + Copyright (c) 2009-2011 Simen Svale Skogsrud + + 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 . +*/ + +#ifndef _GRBLLIB_H_ +#define _GRBLLIB_H_ + +#include + +#ifdef __cplusplus +extern "C" +{ +#endif + +int grbl_enter (void); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/hal.h b/hal.h new file mode 100644 index 0000000..2a09aa0 --- /dev/null +++ b/hal.h @@ -0,0 +1,375 @@ +/* + hal.h - HAL (Hardware Abstraction Layer) entry points structures and capabilities type + + Part of grblHAL + + Copyright (c) 2016-2021 Terje Io + + Grbl is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + Grbl is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with Grbl. If not, see . +*/ + +#ifndef _HAL_H_ +#define _HAL_H_ + +#include "grbl.h" +#include "gcode.h" +#include "system.h" +#include "coolant_control.h" +#include "spindle_control.h" +#include "stepper.h" +#include "nvs.h" +#include "stream.h" +#include "probe.h" +#include "plugins.h" +#include "settings.h" +#include "report.h" + +#define HAL_VERSION 8 + +// driver capabilities, to be set by driver in driver_init(), flags may be cleared after to switch off option +typedef union { + uint32_t value; + struct { + uint32_t mist_control :1, + variable_spindle :1, + spindle_dir :1, + software_debounce :1, + step_pulse_delay :1, + limits_pull_up :1, + control_pull_up :1, + probe_pull_up :1, + amass_level :2, // 0...3 + spindle_at_speed :1, + laser_ppi_mode :1, + spindle_sync :1, + sd_card :1, + bluetooth :1, + ethernet :1, + wifi :1, + spindle_pwm_invert :1, + spindle_pid :1, + mpg_mode :1, + spindle_pwm_linearization :1, + atc :1, + no_gcode_message_handling :1, + dual_spindle :1, + odometers :1, + unassigned :7; + }; +} driver_cap_t; + +typedef void (*driver_reset_ptr)(void); + +// I/O stream + +typedef void (*stream_write_ptr)(const char *s); +typedef bool (*enqueue_realtime_command_ptr)(char data); + +typedef struct { + stream_type_t type; + uint16_t (*get_rx_buffer_available)(void); +// bool (*stream_write)(char c); + stream_write_ptr write; // write string to current I/O stream only. + stream_write_ptr write_all; // write string to all active output streams. + int16_t (*read)(void); + void (*reset_read_buffer)(void); + void (*cancel_read_buffer)(void); + bool (*suspend_read)(bool await); + enqueue_realtime_command_ptr enqueue_realtime_command; // NOTE: set by grbl at startup. +} io_stream_t; + +typedef struct { + uint8_t num_digital_in; + uint8_t num_digital_out; + uint8_t num_analog_in; + uint8_t num_analog_out; + void (*digital_out)(uint8_t port, bool on); + bool (*analog_out)(uint8_t port, float value); + int32_t (*wait_on_input)(bool digital, uint8_t port, wait_mode_t wait_mode, float timeout); +} io_port_t; + +// Spindle + +typedef void (*spindle_set_state_ptr)(spindle_state_t state, float rpm); +typedef spindle_state_t (*spindle_get_state_ptr)(void); +#ifdef SPINDLE_PWM_DIRECT +typedef uint_fast16_t (*spindle_get_pwm_ptr)(float rpm); +typedef void (*spindle_update_pwm_ptr)(uint_fast16_t pwm); +#else +typedef void (*spindle_update_rpm_ptr)(float rpm); +#endif +typedef spindle_data_t *(*spindle_get_data_ptr)(spindle_data_request_t request); +typedef void (*spindle_reset_data_ptr)(void); +typedef void (*spindle_pulse_on_ptr)(uint_fast16_t pulse_length); + +typedef struct { + spindle_set_state_ptr set_state; + spindle_get_state_ptr get_state; +#ifdef SPINDLE_PWM_DIRECT + spindle_get_pwm_ptr get_pwm; + spindle_update_pwm_ptr update_pwm; +#else + spindle_update_rpm_ptr update_rpm; +#endif + // Optional entry points: + spindle_get_data_ptr get_data; + spindle_reset_data_ptr reset_data; + spindle_pulse_on_ptr pulse_on; +} spindle_ptrs_t; + +// Coolant + +typedef void (*coolant_set_state_ptr)(coolant_state_t mode); +typedef coolant_state_t (*coolant_get_state_ptr)(void); + +typedef struct { + coolant_set_state_ptr set_state; + coolant_get_state_ptr get_state; +} coolant_ptrs_t; + +// Limits + +typedef void (*limits_enable_ptr)(bool on, bool homing); +typedef limit_signals_t (*limits_get_state_ptr)(void); +typedef void (*limit_interrupt_callback_ptr)(limit_signals_t state); + +typedef struct { + limits_enable_ptr enable; + limits_get_state_ptr get_state; + limit_interrupt_callback_ptr interrupt_callback; // set up by core before driver_init() is called. +} limits_ptrs_t; + +// Homing + +typedef struct { + limits_get_state_ptr get_state; +} homing_ptrs_t; + +// Control signals + +typedef control_signals_t (*control_signals_get_state_ptr)(void); +typedef void (*control_signals_callback_ptr)(control_signals_t signals); + +typedef struct { + control_signals_get_state_ptr get_state; + control_signals_callback_ptr interrupt_callback; // set up by core before driver_init() is called. +} control_signals_ptrs_t; + +// Steppers + +typedef void (*stepper_wake_up_ptr)(void); +typedef void (*stepper_go_idle_ptr)(bool clear_signals); +typedef void (*stepper_enable_ptr)(axes_signals_t enable); +typedef void (*stepper_disable_motors_ptr)(axes_signals_t axes, squaring_mode_t mode); +typedef void (*stepper_cycles_per_tick_ptr)(uint32_t cycles_per_tick); +typedef void (*stepper_pulse_start_ptr)(stepper_t *stepper); +typedef void (*stepper_output_step_ptr)(axes_signals_t step_outbits, axes_signals_t dir_outbits); +typedef axes_signals_t (*stepper_get_auto_squared_ptr)(void); +typedef void (*stepper_interrupt_callback_ptr)(void); + +typedef struct { + stepper_wake_up_ptr wake_up; + stepper_go_idle_ptr go_idle; + stepper_enable_ptr enable; + stepper_disable_motors_ptr disable_motors; + stepper_cycles_per_tick_ptr cycles_per_tick; + stepper_pulse_start_ptr pulse_start; + stepper_interrupt_callback_ptr interrupt_callback; // set up by core before driver_init() is called. + // Optional entry points: + stepper_get_auto_squared_ptr get_auto_squared; + stepper_output_step_ptr output_step; +} stepper_ptrs_t; + +// Probe (optional) + +typedef probe_state_t (*probe_get_state_ptr)(void); +typedef void (*probe_configure_ptr)(bool is_probe_away, bool probing); +typedef void (*probe_connected_toggle_ptr)(void); + +typedef struct { + probe_configure_ptr configure; + probe_get_state_ptr get_state; + probe_connected_toggle_ptr connected_toggle; +} probe_ptrs_t; + +typedef void (*tool_select_ptr)(tool_data_t *tool, bool next); +typedef status_code_t (*tool_change_ptr)(parser_state_t *gc_state); + +typedef struct { + tool_select_ptr select; + tool_change_ptr change; +} tool_ptrs_t; + +// User M-codes (optional) + +typedef user_mcode_t (*user_mcode_check_ptr)(user_mcode_t mcode); +typedef status_code_t (*user_mcode_validate_ptr)(parser_block_t *gc_block, parameter_words_t *parameter_words); +typedef void (*user_mcode_execute_ptr)(sys_state_t state, parser_block_t *gc_block); + +typedef struct { + user_mcode_check_ptr check; + user_mcode_validate_ptr validate; + user_mcode_execute_ptr execute; +} user_mcode_ptrs_t; + +// Encoder (optional) + +typedef uint8_t (*encoder_get_n_encoders_ptr)(void); +typedef void (*encoder_on_event_ptr)(encoder_t *encoder, int32_t position); +typedef void (*encoder_reset_ptr)(uint_fast8_t id); + +typedef struct { + encoder_get_n_encoders_ptr get_n_encoders; + encoder_on_event_ptr on_event; + encoder_reset_ptr reset; +} encoder_ptrs_t; + +// + +// main HAL structure + +typedef struct { + uint32_t version; + char *info; + char *driver_version; + char *driver_options; + char *board; + uint32_t f_step_timer; + uint32_t rx_buffer_size; + uint32_t max_step_rate; + uint8_t driver_axis_settings; + + bool (*driver_setup)(settings_t *settings); + void (*delay_ms)(uint32_t ms, void (*callback)(void)); + void (*set_bits_atomic)(volatile uint_fast16_t *value, uint_fast16_t bits); + uint_fast16_t (*clear_bits_atomic)(volatile uint_fast16_t *value, uint_fast16_t bits); + uint_fast16_t (*set_value_atomic)(volatile uint_fast16_t *value, uint_fast16_t bits); + void (*irq_enable)(void); + void (*irq_disable)(void); + + limits_ptrs_t limits; + homing_ptrs_t homing; + coolant_ptrs_t coolant; + spindle_ptrs_t spindle; + stepper_ptrs_t stepper; + control_signals_ptrs_t control; + io_stream_t stream; + settings_changed_ptr settings_changed; + + // + // optional entry points, may be unassigned (null) + // + bool (*driver_release)(void); + bool (*get_position)(int32_t (*position)[N_AXIS]); + uint32_t (*get_elapsed_ticks)(void); + void (*pallet_shuttle)(void); + void (*reboot)(void); +#ifdef DEBUGOUT + void (*debug_out)(bool on); +#endif + + probe_ptrs_t probe; + user_mcode_ptrs_t user_mcode; + driver_reset_ptr driver_reset; + tool_ptrs_t tool; + encoder_ptrs_t encoder; + nvs_io_t nvs; + io_port_t port; + + bool (*stream_blocking_callback)(void); // set up by core before driver_init() is called. + + // driver capabilities flags + driver_cap_t driver_cap; + control_signals_t signals_cap; + +} grbl_hal_t; + +// TODO: move the following structs to grbl.h? + +/* TODO: add to grbl pointers so that a different formatting (xml, json etc) of reports may be implemented by driver? +typedef struct { + status_code_t (*report_status_message)(status_code_t status_code); + alarm_code_t (*report_alarm_message)(alarm_code_t alarm_code); + message_code_t (*report_feedback_message)(message_code_t message_code); + void (*report_init_message)(void); + void (*report_grbl_help)(void); + void (*report_echo_line_received)(char *line); + void (*report_realtime_status)(void); + void (*report_probe_parameters)(void); + void (*report_ngc_parameters)(void); + void (*report_gcode_modes)(void); + void (*report_startup_line)(uint8_t n, char *line); + void (*report_execute_startup_message)(char *line, status_code_t status_code); +} grbl_report_t; +*/ + +// Report entry points set by core at reset. + +typedef status_code_t (*status_message_ptr)(status_code_t status_code); +typedef message_code_t (*feedback_message_ptr)(message_code_t message_code); + +typedef struct { + setting_output_ptr setting; + status_message_ptr status_message; + feedback_message_ptr feedback_message; +} report_t; + +// Core event handler and other entry points. +// Most of the event handlers defaults to NULL, a few is set up to call a dummy handler for simpler code. + +typedef void (*on_state_change_ptr)(sys_state_t state); +typedef void (*on_probe_completed_ptr)(void); +typedef void (*on_program_completed_ptr)(program_flow_t program_flow); +typedef void (*on_execute_realtime_ptr)(sys_state_t state); +typedef void (*on_unknown_accessory_override_ptr)(uint8_t cmd); +typedef void (*on_report_options_ptr)(bool newopt); +typedef void (*on_report_command_help_ptr)(void); +typedef void (*on_global_settings_restore_ptr)(void); +typedef setting_details_t *(*on_get_settings_ptr)(void); // NOTE: this must match the signature of the same definition in + // the setting_details_t structure in settings.h! +typedef void (*on_realtime_report_ptr)(stream_write_ptr stream_write, report_tracking_flags_t report); +typedef void (*on_unknown_feedback_message_ptr)(stream_write_ptr stream_write); +typedef bool (*on_laser_ppi_enable_ptr)(uint_fast16_t ppi, uint_fast16_t pulse_length); +typedef status_code_t (*on_unknown_sys_command_ptr)(sys_state_t state, char *line); // return Status_Unhandled. +typedef status_code_t (*on_user_command_ptr)(char *line); +typedef sys_commands_t *(*on_get_commands_ptr)(void); + +typedef struct { + // report entry points set by core at reset. + report_t report; + // grbl core events - may be subscribed to by drivers or by the core. + on_state_change_ptr on_state_change; + on_probe_completed_ptr on_probe_completed; + on_program_completed_ptr on_program_completed; + on_execute_realtime_ptr on_execute_realtime; + on_unknown_accessory_override_ptr on_unknown_accessory_override; + on_report_options_ptr on_report_options; + on_report_command_help_ptr on_report_command_help; + on_global_settings_restore_ptr on_global_settings_restore; + on_get_settings_ptr on_get_settings; + on_realtime_report_ptr on_realtime_report; + on_unknown_feedback_message_ptr on_unknown_feedback_message; + on_unknown_sys_command_ptr on_unknown_sys_command; // return Status_Unhandled if not handled. + on_get_commands_ptr on_get_commands; + on_user_command_ptr on_user_command; + on_laser_ppi_enable_ptr on_laser_ppi_enable; + // core entry points - set up by core before driver_init() is called. + bool (*protocol_enqueue_gcode)(char *data); +} grbl_t; + +extern grbl_t grbl; +extern grbl_hal_t hal; +extern bool driver_init (void); + +#endif diff --git a/kinematics.h b/kinematics.h new file mode 100644 index 0000000..16d08e7 --- /dev/null +++ b/kinematics.h @@ -0,0 +1,36 @@ +/* + kinematics.h - kinematics interface (API) + + Part of grblHAL + + Copyright (c) 2019 Terje Io + + Grbl is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + Grbl is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with Grbl. If not, see . +*/ + +#ifndef _KINEMATICS_H_ +#define _KINEMATICS_H_ + +typedef struct { + void (*convert_array_steps_to_mpos)(float *position, int32_t *steps); + void (*plan_target_to_steps) (int32_t *target_steps, float *target); + bool (*segment_line) (float *target, plan_line_data_t *pl_data, bool init); + 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); +} kinematics_t; + +extern kinematics_t kinematics; + +#endif diff --git a/limits.c b/limits.c new file mode 100644 index 0000000..340be0f --- /dev/null +++ b/limits.c @@ -0,0 +1,541 @@ +/* + limits.c - code pertaining to limit-switches and performing the homing cycle + + Part of grblHAL + + Copyright (c) 2017-2021 Terje Io + Copyright (c) 2012-2016 Sungeun K. Jeon for Gnea Research LLC + Copyright (c) 2009-2011 Simen Svale Skogsrud + + 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 . +*/ + +#include +#include +#include + +#include "hal.h" +#include "nuts_bolts.h" +#include "protocol.h" +#include "motion_control.h" +#include "limits.h" +#include "tool_change.h" +#include "state_machine.h" +#ifdef KINEMATICS_API +#include "kinematics.h" +#endif + +#include "defaults.h" + +// Homing axis search distance multiplier. Computed by this value times the cycle travel. +#ifndef HOMING_AXIS_SEARCH_SCALAR + #define HOMING_AXIS_SEARCH_SCALAR 1.5f // Must be > 1 to ensure limit switch will be engaged. +#endif +#ifndef HOMING_AXIS_LOCATE_SCALAR + #define HOMING_AXIS_LOCATE_SCALAR 5.0f // Must be > 1 to ensure limit switch is cleared. +#endif + +// This is the Limit Pin Change Interrupt, which handles the hard limit feature. A bouncing +// limit switch can cause a lot of problems, like false readings and multiple interrupt calls. +// If a switch is triggered at all, something bad has happened and treat it as such, regardless +// if a limit switch is being disengaged. It's impossible to reliably tell the state of a +// bouncing pin because the microcontroller does not retain any state information when +// detecting a pin change. If we poll the pins in the ISR, you can miss the correct reading if the +// switch is bouncing. +// NOTE: Do not attach an e-stop to the limit pins, because this interrupt is disabled during +// homing cycles and will not respond correctly. Upon user request or need, there may be a +// special pinout for an e-stop, but it is generally recommended to just directly connect +// your e-stop switch to the microcontroller reset pin, since it is the most correct way to do this. + +// Merge (bitwise or) all limit switch inputs. +ISR_CODE axes_signals_t limit_signals_merge (limit_signals_t signals) +{ + axes_signals_t state; + + state.mask = signals.min.mask | signals.min2.mask | signals.max.mask | signals.max2.mask; + + return state; +} + +// Merge (bitwise or) home switch inputs (typically aquired from limits.min and limits.min2). +ISR_CODE static axes_signals_t homing_signals_select (limit_signals_t signals, axes_signals_t auto_square, squaring_mode_t mode) +{ + axes_signals_t state; + + switch(mode) { + + case SquaringMode_A: + signals.min.mask &= ~auto_square.mask; + break; + + case SquaringMode_B: + signals.min2.mask &= ~auto_square.mask; + break; + + default: + break; + } + + state.mask = signals.min.mask | signals.min2.mask; + + return state; +} + +ISR_CODE void limit_interrupt_handler (limit_signals_t state) // DEFAULT: Limit pin change interrupt process. +{ + // Ignore limit switches if already in an alarm state or in-process of executing an alarm. + // When in the alarm state, Grbl should have been reset or will force a reset, so any pending + // moves in the planner and stream input buffers are all cleared and newly sent blocks will be + // locked out until a homing cycle or a kill lock command. Allows the user to disable the hard + // limit setting if their limits are constantly triggering after a reset and move their axes. + + memcpy(&sys.last_event.limits, &state, sizeof(limit_signals_t)); + + if (!(state_get() & (STATE_ALARM|STATE_ESTOP)) && !sys.rt_exec_alarm) { + + #ifdef HARD_LIMIT_FORCE_STATE_CHECK + // Check limit pin state. + if (limit_signals_merge(state).value) { + mc_reset(); // Initiate system kill. + system_set_exec_alarm(Alarm_HardLimit); // Indicate hard limit critical event + } + #else + mc_reset(); // Initiate system kill. + system_set_exec_alarm(Alarm_HardLimit); // Indicate hard limit critical event + #endif + } +} + +#ifndef KINEMATICS_API +// Set machine positions for homed limit switches. Don't update non-homed axes. +// NOTE: settings.max_travel[] is stored as a negative value. +void limits_set_machine_positions (axes_signals_t cycle, bool add_pulloff) +{ + uint_fast8_t idx = N_AXIS; + float pulloff = add_pulloff ? settings.homing.pulloff : 0.0f; + + if(settings.homing.flags.force_set_origin) { + do { + if (cycle.mask & bit(--idx)) { + sys.position[idx] = 0; + sys.home_position[idx] = 0.0f; + } + } while(idx); + } else do { + if (cycle.mask & bit(--idx)) { + sys.home_position[idx] = bit_istrue(settings.homing.dir_mask.value, bit(idx)) + ? settings.axis[idx].max_travel + pulloff + : - pulloff; + sys.position[idx] = sys.home_position[idx] * settings.axis[idx].steps_per_mm; + } + } while(idx); +} +#endif + +// Pulls off axes from asserted homing switches before homing starts. +// For now only for auto squared axes. +static bool limits_pull_off (axes_signals_t axis, float distance) +{ + uint_fast8_t n_axis = 0, idx = N_AXIS; + coord_data_t target = {0}; + plan_line_data_t plan_data = { + .condition.system_motion = On, + .condition.no_feed_override = On, + .line_number = HOMING_CYCLE_LINE_NUMBER + }; + + system_convert_array_steps_to_mpos(target.values, sys.position); + + do { + idx--; + if(bit_istrue(axis.mask, bit(idx))) { + n_axis++; + if (bit_istrue(settings.homing.dir_mask.value, bit(idx))) + target.values[idx] += distance; + else + target.values[idx] -= distance; + } + } while(idx); + + plan_data.feed_rate = settings.homing.seek_rate * sqrtf(n_axis); // Adjust so individual axes all move at pull-off rate. + plan_data.condition.spindle = gc_state.modal.spindle; + plan_data.condition.coolant = gc_state.modal.coolant; + memcpy(&plan_data.spindle, &gc_state.spindle, sizeof(spindle_t)); + + plan_buffer_line(target.values, &plan_data); // Bypass mc_line(). Directly plan homing motion. + + sys.step_control.flags = 0; // Clear existing flags and + sys.step_control.execute_sys_motion = On; // set to execute homing motion. + sys.homing_axis_lock.mask = axis.mask; + + st_prep_buffer(); // Prep and fill segment buffer from newly planned block. + st_wake_up(); // Initiate motion. + + while(true) { + + st_prep_buffer(); // Check and prep segment buffer. + + // Exit routines: No time to run protocol_execute_realtime() in this loop. + if (sys.rt_exec_state & (EXEC_SAFETY_DOOR | EXEC_RESET | EXEC_CYCLE_COMPLETE)) { + + uint_fast16_t rt_exec = sys.rt_exec_state; + + // Homing failure condition: Reset issued during cycle. + if (rt_exec & EXEC_RESET) + system_set_exec_alarm(Alarm_HomingFailReset); + + // Homing failure condition: Safety door was opened. + if (rt_exec & EXEC_SAFETY_DOOR) + system_set_exec_alarm(Alarm_HomingFailDoor); + + // Homing failure condition: Homing switch(es) still engaged after pull-off motion + if (homing_signals_select(hal.homing.get_state(), (axes_signals_t){0}, SquaringMode_Both).mask & axis.mask) + system_set_exec_alarm(Alarm_FailPulloff); + + if (sys.rt_exec_alarm) { + mc_reset(); // Stop motors, if they are running. + protocol_execute_realtime(); + return false; + } else { + // Pull-off motion complete. Disable CYCLE_STOP from executing. + system_clear_exec_state_flag(EXEC_CYCLE_COMPLETE); + break; + } + } + + grbl.on_execute_realtime(STATE_HOMING); + } + + st_reset(); // Immediately force kill steppers and reset step segment buffer. + + sys.step_control.flags = 0; // Return step control to normal operation. + + return true; // Note: failure is returned above if move fails. +} + + +// Homes the specified cycle axes, sets the machine position, and performs a pull-off motion after +// completing. Homing is a special motion case, which involves rapid uncontrolled stops to locate +// the trigger point of the limit switches. The rapid stops are handled by a system level axis lock +// mask, which prevents the stepper algorithm from executing step pulses. Homing motions typically +// circumvent the processes for executing motions in normal operation. +// NOTE: Only the abort realtime command can interrupt this process. +static bool limits_homing_cycle (axes_signals_t cycle, axes_signals_t auto_square) +{ + if (ABORTED) // Block if system reset has been issued. + return false; + + int32_t initial_trigger_position = 0, autosquare_fail_distance = 0; + uint_fast8_t n_cycle = (2 * settings.homing.locate_cycles + 1); + uint_fast8_t step_pin[N_AXIS], n_active_axis, dual_motor_axis = 0; + float target[N_AXIS]; + float max_travel = 0.0f, homing_rate = settings.homing.seek_rate; + bool approach = true, autosquare_check = false; + axes_signals_t axislock, homing_state; + limit_signals_t limits_state; + squaring_mode_t squaring_mode = SquaringMode_Both; + plan_line_data_t plan_data; + + // Initialize plan data struct for homing motion. + + memset(&plan_data, 0, sizeof(plan_line_data_t)); + plan_data.condition.system_motion = On; + plan_data.condition.no_feed_override = On; + plan_data.line_number = HOMING_CYCLE_LINE_NUMBER; + memcpy(&plan_data.spindle, &gc_state.spindle, sizeof(spindle_t)); + plan_data.condition.spindle = gc_state.modal.spindle; + plan_data.condition.coolant = gc_state.modal.coolant; + + uint_fast8_t idx = N_AXIS; + do { + idx--; + // Initialize step pin masks +#ifdef KINEMATICS_API + step_pin[idx] = kinematics.limits_get_axis_mask(idx); +#else + step_pin[idx] = bit(idx); +#endif + // Set target based on max_travel setting. Ensure homing switches engaged with search scalar. + // NOTE: settings.max_travel[] is stored as a negative value. + if (bit_istrue(cycle.mask, bit(idx))) { + max_travel = max(max_travel,(-HOMING_AXIS_SEARCH_SCALAR) * settings.axis[idx].max_travel); + if(bit_istrue(auto_square.mask, bit(idx))) + dual_motor_axis = idx; + } + } while(idx); + + if(auto_square.mask) { + float fail_distance = (-settings.homing.dual_axis.fail_length_percent / 100.0f) * settings.axis[dual_motor_axis].max_travel; + fail_distance = min(fail_distance, settings.homing.dual_axis.fail_distance_min); + fail_distance = max(fail_distance, settings.homing.dual_axis.fail_distance_max); + autosquare_fail_distance = truncf(fail_distance * settings.axis[dual_motor_axis].steps_per_mm); + } + + // Set search mode with approach at seek rate to quickly engage the specified cycle.mask limit switches. + do { + + // Initialize and declare variables needed for homing routine. + system_convert_array_steps_to_mpos(target, sys.position); + axislock = (axes_signals_t){0}; + n_active_axis = 0; + + idx = N_AXIS; + do { + // Set target location for active axes and setup computation for homing rate. + if (bit_istrue(cycle.mask, bit(--idx))) { + n_active_axis++; + +#ifdef KINEMATICS_API + kinematics.limits_set_target_pos(idx); +#else + sys.position[idx] = 0; +#endif + // Set target direction based on cycle mask and homing cycle approach state. + if (bit_istrue(settings.homing.dir_mask.value, bit(idx))) + target[idx] = approach ? - max_travel : max_travel; + else + target[idx] = approach ? max_travel : - max_travel; + + // Apply axislock to the step port pins active in this cycle. + axislock.mask |= step_pin[idx]; + } + } while(idx); + + homing_rate *= sqrtf(n_active_axis); // [sqrt(N_AXIS)] Adjust so individual axes all move at homing rate. + sys.homing_axis_lock.mask = axislock.mask; + + // Perform homing cycle. Planner buffer should be empty, as required to initiate the homing cycle. + plan_data.feed_rate = homing_rate; // Set current homing rate. + plan_buffer_line(target, &plan_data); // Bypass mc_line(). Directly plan homing motion. + + sys.step_control.flags = 0; + sys.step_control.execute_sys_motion = On; // Set to execute homing motion and clear existing flags. + st_prep_buffer(); // Prep and fill segment buffer from newly planned block. + st_wake_up(); // Initiate motion + + do { + + if (approach) { + + // Check homing switches state. Lock out cycle axes when they change. + homing_state = homing_signals_select(limits_state = hal.homing.get_state(), auto_square, squaring_mode); + + // Auto squaring check + if((homing_state.mask & auto_square.mask) && squaring_mode == SquaringMode_Both) { + if((autosquare_check = (limits_state.min.mask & auto_square.mask) != (limits_state.min2.mask & auto_square.mask))) { + initial_trigger_position = sys.position[dual_motor_axis]; + homing_state.mask &= ~auto_square.mask; + squaring_mode = (limits_state.min.mask & auto_square.mask) ? SquaringMode_A : SquaringMode_B; + hal.stepper.disable_motors(auto_square, squaring_mode); + } + } + + idx = N_AXIS; + do { + idx--; + if ((axislock.mask & step_pin[idx]) && (homing_state.mask & bit(idx))) { +#ifdef KINEMATICS_API + axislock.mask &= ~kinematics.limits_get_axis_mask(idx); +#else + axislock.mask &= ~bit(idx); +#endif + if(idx == dual_motor_axis) + autosquare_check = false; + } + } while(idx); + + sys.homing_axis_lock.mask = axislock.mask; + + if (autosquare_check && abs(initial_trigger_position - sys.position[dual_motor_axis]) > autosquare_fail_distance) { + system_set_exec_alarm(Alarm_HomingFailAutoSquaringApproach); + mc_reset(); + protocol_execute_realtime(); + return false; + } + } + + st_prep_buffer(); // Check and prep segment buffer. NOTE: Should take no longer than 200us. + + // Exit routines: No time to run protocol_execute_realtime() in this loop. + if (sys.rt_exec_state & (EXEC_SAFETY_DOOR | EXEC_RESET | EXEC_CYCLE_COMPLETE)) { + + uint_fast16_t rt_exec = sys.rt_exec_state; + + // Homing failure condition: Reset issued during cycle. + if (rt_exec & EXEC_RESET) + system_set_exec_alarm(Alarm_HomingFailReset); + + // Homing failure condition: Safety door was opened. + if (rt_exec & EXEC_SAFETY_DOOR) + system_set_exec_alarm(Alarm_HomingFailDoor); + + // Homing failure condition: Homing switch(es) still engaged after pull-off motion + if (!approach && (homing_signals_select(hal.homing.get_state(), (axes_signals_t){0}, SquaringMode_Both).mask & cycle.mask)) + system_set_exec_alarm(Alarm_FailPulloff); + + // Homing failure condition: Limit switch not found during approach. + if (approach && (rt_exec & EXEC_CYCLE_COMPLETE)) + system_set_exec_alarm(Alarm_HomingFailApproach); + + if (sys.rt_exec_alarm) { + mc_reset(); // Stop motors, if they are running. + protocol_execute_realtime(); + return false; + } else { + // Pull-off motion complete. Disable CYCLE_STOP from executing. + system_clear_exec_state_flag(EXEC_CYCLE_COMPLETE); + break; + } + } + + grbl.on_execute_realtime(STATE_HOMING); + + } while (axislock.mask & AXES_BITMASK); + + st_reset(); // Immediately force kill steppers and reset step segment buffer. + hal.delay_ms(settings.homing.debounce_delay, NULL); // Delay to allow transient dynamics to dissipate. + + // Reverse direction and reset homing rate for locate cycle(s). + approach = !approach; + + // After first cycle, homing enters locating phase. Shorten search to pull-off distance. + if (approach) { + // Only one initial pass for auto squared axis when both motors are active + //if(mode == SquaringMode_Both && auto_square.mask) + // cycle.mask &= ~auto_square.mask; + max_travel = settings.homing.pulloff * HOMING_AXIS_LOCATE_SCALAR; + homing_rate = settings.homing.feed_rate; + } else { + max_travel = settings.homing.pulloff; + homing_rate = settings.homing.seek_rate; + } + + if(auto_square.mask) { + autosquare_check = false; + squaring_mode = SquaringMode_Both; + hal.stepper.disable_motors((axes_signals_t){0}, SquaringMode_Both); + } + + } while (cycle.mask && n_cycle-- > 0); + + // Pull off B motor to compensate for switch inaccuracy when configured. + if(auto_square.mask && settings.axis[dual_motor_axis].dual_axis_offset != 0.0f) { + hal.stepper.disable_motors(auto_square, SquaringMode_A); + if(!limits_pull_off(auto_square, settings.axis[dual_motor_axis].dual_axis_offset)) + return false; + hal.stepper.disable_motors((axes_signals_t){0}, SquaringMode_Both); + } + + // The active cycle axes should now be homed and machine limits have been located. By + // default, Grbl defines machine space as all negative, as do most CNCs. Since limit switches + // can be on either side of an axes, check and set axes machine zero appropriately. Also, + // set up pull-off maneuver from axes limit switches that have been homed. This provides + // some initial clearance off the switches and should also help prevent them from falsely + // triggering when hard limits are enabled or when more than one axes shares a limit pin. +#ifdef KINEMATICS_API + kinematics.limits_set_machine_positions(cycle); +#else + limits_set_machine_positions(cycle, true); +#endif + +#ifdef ENABLE_BACKLASH_COMPENSATION + mc_backlash_init(); +#endif + sys.step_control.flags = 0; // Return step control to normal operation. + sys.homed.mask |= cycle.mask; + + return true; +} + +// Perform homing cycle(s) according to configuration. +// NOTE: only one auto squared axis can be homed at a time. +bool limits_go_home (axes_signals_t cycle) +{ + bool homed = false; + + axes_signals_t auto_square = {0}, auto_squared = {0}; + + if(hal.stepper.get_auto_squared) + auto_squared = hal.stepper.get_auto_squared(); + + auto_squared.mask &= cycle.mask; + + if(auto_squared.mask) { + + if(!hal.stepper.disable_motors) + return false; // Bad driver! + + auto_square.x = On; + while(!(auto_squared.mask & auto_square.mask)) + auto_square.mask <<= 1; + + if(auto_squared.mask != auto_square.mask) + return false; // Attempt at squaring more than one auto squared axis at the same time. + + if((auto_squared.mask & homing_signals_select(hal.homing.get_state(), (axes_signals_t){0}, SquaringMode_Both).mask) && !limits_pull_off(auto_square, settings.homing.pulloff * HOMING_AXIS_LOCATE_SCALAR)) + return false; // Auto squaring with limit switch asserted is not allowed. + } + + tc_clear_tlo_reference(cycle); + + homed = limits_homing_cycle(cycle, auto_square); + + return homed; +} + +// Performs a soft limit check. Called from mc_line() only. Assumes the machine has been homed, +// the workspace volume is in all negative space, and the system is in normal operation. +// NOTE: Also used by jogging to block travel outside soft-limit volume. +void limits_soft_check (float *target) +{ + if (!system_check_travel_limits(target)) { + sys.flags.soft_limit = On; + // Force feed hold if cycle is active. All buffered blocks are guaranteed to be within + // workspace volume so just come to a controlled stop so position is not lost. When complete + // enter alarm mode. + if (state_get() == STATE_CYCLE) { + system_set_exec_state_flag(EXEC_FEED_HOLD); + do { + if(!protocol_execute_realtime()) + return; // aborted! + } while (state_get() != STATE_IDLE); + } + mc_reset(); // Issue system reset and ensure spindle and coolant are shutdown. + system_set_exec_alarm(Alarm_SoftLimit); // Indicate soft limit critical event + protocol_execute_realtime(); // Execute to enter critical event loop and system abort + } +} + +// Set axes to be homed from settings. +void limits_set_homing_axes (void) +{ + uint_fast8_t idx = N_AXIS; + + sys.homing.mask = 0; + + do { + sys.homing.mask |= settings.homing.cycle[--idx].mask; + } while(idx); + + sys.homed.mask &= sys.homing.mask; +} + +// Check if homing is required. +bool limits_homing_required (void) +{ + return settings.homing.flags.enabled && settings.homing.flags.init_lock && + (sys.cold_start || !settings.homing.flags.override_locks) && + sys.homing.mask && (sys.homing.mask & sys.homed.mask) != sys.homing.mask; +} + diff --git a/limits.h b/limits.h new file mode 100644 index 0000000..1d234e1 --- /dev/null +++ b/limits.h @@ -0,0 +1,46 @@ +/* + limits.h - code pertaining to limit-switches and performing the homing cycle + + Part of grblHAL + + Copyright (c) 2017-2018 Terje Io + Copyright (c) 2012-2015 Sungeun K. Jeon + Copyright (c) 2009-2011 Simen Svale Skogsrud + + 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 . +*/ + +#ifndef _LIMITS_H_ +#define _LIMITS_H_ + +#include "nuts_bolts.h" + +// Perform one portion of the homing cycle based on the input settings. +bool limits_go_home(axes_signals_t cycle); + +// Check for soft limit violations +void limits_soft_check(float *target); + +// Check if homing is required. +bool limits_homing_required (void); + +// Set axes to be homed from settings. +void limits_set_homing_axes (void); +void limits_set_machine_positions (axes_signals_t cycle, bool add_pulloff); + +void limit_interrupt_handler (limit_signals_t state); + +axes_signals_t limit_signals_merge (limit_signals_t signals); + +#endif diff --git a/maslow.c b/maslow.c new file mode 100644 index 0000000..975d0cf --- /dev/null +++ b/maslow.c @@ -0,0 +1,676 @@ +/* + 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 . + + The basis for this code has been pulled from MaslowDue created by Larry D O'Cull. + + + 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. + + + It has been adapted for grblHAL by Terje Io. +*/ + +#include "grbl.h" + +#ifdef MASLOW_ROUTER + +#include + +#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 + diff --git a/maslow.h b/maslow.h new file mode 100644 index 0000000..f4aefeb --- /dev/null +++ b/maslow.h @@ -0,0 +1,161 @@ +/* + maslow.h - Maslow router kinematics implementation + + Part of grblHAL + + Grbl is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + Grbl is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with Grbl. If not, see . + + The basis for this code has been pulled from MaslowDue created by Larry D O'Cull. + + + 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. + + + It has been adapted for grbl by Terje Io. + + *** TO BE COMPLETED *** + +*/ + +#include "grbl.h" + +#ifndef _MASLOW_H_ +#define _MASLOW_H_ + +#define FP_SCALING 1024.0f +#define SPROCKET_RADIUS_MM (10.1f) +#define MAX_SEG_LENGTH_MM 2.0f /* long lines must be segmented due to circular motion */ + + // PID position loop factors X: Kp = 25000 Ki = 15000 Kd = 22000 Imax = 5000 + // 14.000 fixed point arithmatic S13.10 +#ifdef DRIVER_TLE5206 + #define MASLOW_A_KP 10.0f + #define MASLOW_A_KI 21.0f + #define MASLOW_A_KD 18.0f + #define MASLOW_A_IMAX 5000 + + #define MASLOW_B_KP 10.0f + #define MASLOW_B_KI 21.0f + #define MASLOW_B_KD 18.0f + #define MASLOW_B_IMAX 5000 + + #define MASLOW_Z_KP 10.0f + #define MASLOW_Z_KI 21.0f + #define MASLOW_Z_KD 17.0f + #define MASLOW_Z_IMAX 5000 +#else + #define MASLOW_A_KP 22.0f + #define MASLOW_A_KI 17.0f + #define MASLOW_A_KD 20.0f + #define MASLOW_A_IMAX 5000 + + #define MASLOW_B_KP 22.0f + #define MASLOW_B_KI 17.0f + #define MASLOW_B_KD 20.0f + #define MASLOW_B_IMAX 5000 + + #define MASLOW_Z_KP 20.0f + #define MASLOW_Z_KI 17.0f + #define MASLOW_Z_KD 18.0f + #define MASLOW_Z_IMAX 5000 +#endif + +#define MASLOW_MACHINEWIDTH 2400.0f +#define MASLOW_MACHINEHEIGHT 1200.0f +#define MASLOW_DISTBETWEENMOTORS 3000.0f +#define MASLOW_MOTOROFFSETY 600.0f +#define MASLOW_CHAINLENGTH 3000.0f +#define MASLOW_CHAINOVERSPROCKET 0 +#define MASLOW_CHAINSAGCORRECTION 59.504839f +#define MASLOW_LEFTCHAINTOLERANCE 0.0f +#define MASLOW_RIGHTCHAINTOLERANCE 0.0f +#define MASLOW_ROTATIONDISKRADIUS 104.3f +#define MASLOW_SLEDHEIGHT 139.0f +#define MASLOW_SLEDWIDTH 310.0f +#define MASLOW_ACORRSCALING 1.003922f +#define MASLOW_BCORRSCALING 1.002611f + +typedef enum { + Maslow_ChainOverSprocket = 260, + Maslow_MachineWidth, + Maslow_MachineHeight, + Maslow_DistBetweenMotors, + Maslow_MotorOffsetY, + Maslow_AcorrScaling, + Maslow_BcorrScaling, + Maslow_SettingMax, +} maslow_setting_t; + +typedef enum { + AxisSetting_MaslowKP = 10, + AxisSetting_MaslowKI, + AxisSetting_MaslowKD, + AxisSetting_MaslowIMax, + AxisSetting_MaslowMaxSetting +} maslow_axis_setting_t; + +typedef struct { + float Kp; + float Ki; + float Kd; + float Imax; +} maslow_pid_coefficients_t; + +typedef struct { + maslow_pid_coefficients_t pid[N_AXIS]; + + uint32_t chainOverSprocket; + float machineWidth; /* Maslow specific settings */ + float machineHeight; + float distBetweenMotors; + float motorOffsetY; + float chainSagCorrection; + float leftChainTolerance; + float rightChainTolerance; + float rotationDiskRadius; + float chainLength; + float sledHeight; + float sledWidth; + + float XcorrScaling; + float YcorrScaling; +} maslow_settings_t; + +typedef struct { + float Error; + float Integral; + float iterm; + float DiffTerm; + float speed; +} maslow_debug_t; + +typedef struct { + maslow_settings_t settings; + void (*pid_settings_changed)(uint_fast8_t idx); + void (*move)(uint_fast8_t idx, int_fast16_t distance); + void (*reset_pid)(uint_fast8_t idx); + void (*pos_enable)(bool enable); + void (*tuning_enable)(bool enable); + int32_t (*set_step_size)(uint_fast8_t idx, int32_t step_size); + maslow_debug_t *(*get_debug_data)(uint_fast8_t idx); +} maslow_hal_t; + +extern maslow_hal_t maslow_hal; + +// Initialize HAL pointers for Maslow Router kinematics +bool maslow_init (void); +static status_code_t maslow_tuning (uint_fast16_t state, char *line); + +#endif diff --git a/motion_control.c b/motion_control.c new file mode 100644 index 0000000..7629bfe --- /dev/null +++ b/motion_control.c @@ -0,0 +1,989 @@ +/* + motion_control.c - high level interface for issuing motion commands + + Part of grblHAL + + Copyright (c) 2017-2021 Terje Io + Copyright (c) 2011-2016 Sungeun K. Jeon for Gnea Research LLC + Copyright (c) 2009-2011 Simen Svale Skogsrud + + Backlash compensation code based on code copyright (c) 2017 Patrick F. (Schildkroet) + + Bezier splines based on a pull request for Marlin by Giovanni Mascellani + + 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 . +*/ + +#include +#include + +#include "hal.h" +#include "nuts_bolts.h" +#include "protocol.h" +#include "limits.h" +#include "state_machine.h" +#include "motion_control.h" +#include "tool_change.h" +#ifdef KINEMATICS_API +#include "kinematics.h" +#endif + +#ifndef N_ARC_CORRECTION +#define N_ARC_CORRECTION 12 +#endif +#ifndef ARC_ANGULAR_TRAVEL_EPSILON // Float (radians) +#define ARC_ANGULAR_TRAVEL_EPSILON 5E-7f // Float (radians) +#endif + +#ifndef BEZIER_MIN_STEP +#define BEZIER_MIN_STEP 0.002f +#endif +#ifndef BEZIER_MAX_STEP +#define BEZIER_MAX_STEP 0.1f +#endif +#ifndef BEZIER_SIGMA +#define BEZIER_SIGMA 0.1f +#endif + +#ifdef ENABLE_BACKLASH_COMPENSATION + +static float target_prev[N_AXIS]; +static axes_signals_t dir_negative, backlash_enabled; + +void mc_backlash_init (void) +{ + uint_fast8_t idx = N_AXIS; + + backlash_enabled.mask = dir_negative.value = 0; + + do { + if(settings.axis[--idx].backlash > 0.0001f) + backlash_enabled.mask |= bit(idx); + dir_negative.value |= bit(idx); + } while(idx); + + dir_negative.value ^= settings.homing.dir_mask.value; + + mc_sync_backlash_position(); +} + +void mc_sync_backlash_position (void) +{ + // Update target_prev + system_convert_array_steps_to_mpos(target_prev, sys.position); +} + +#endif + +// Execute linear motion in absolute millimeter coordinates. Feed rate given in millimeters/second +// unless invert_feed_rate is true. Then the feed_rate means that the motion should be completed in +// (1 minute)/feed_rate time. +// NOTE: This is the primary gateway to the grbl planner. All line motions, including arc line +// segments, must pass through this routine before being passed to the planner. The seperation of +// mc_line and plan_buffer_line is done primarily to place non-planner-type functions from being +// in the planner and to let backlash compensation or canned cycle integration simple and direct. +bool mc_line (float *target, plan_line_data_t *pl_data) +{ + + // If enabled, check for soft limit violations. Placed here all line motions are picked up + // from everywhere in Grbl. + // NOTE: Block jog motions. Jogging is a special case and soft limits are handled independently. + if (!pl_data->condition.jog_motion && settings.limits.flags.soft_enabled) + limits_soft_check(target); + + // If in check gcode mode, prevent motion by blocking planner. Soft limits still work. + if (state_get() != STATE_CHECK_MODE && protocol_execute_realtime()) { + + // NOTE: Backlash compensation may be installed here. It will need direction info to track when + // to insert a backlash line motion(s) before the intended line motion and will require its own + // plan_check_full_buffer() and check for system abort loop. Also for position reporting + // backlash steps will need to be also tracked, which will need to be kept at a system level. + // There are likely some other things that will need to be tracked as well. However, we feel + // that backlash compensation should NOT be handled by Grbl itself, because there are a myriad + // of ways to implement it and can be effective or ineffective for different CNC machines. This + // would be better handled by the interface as a post-processor task, where the original g-code + // is translated and inserts backlash motions that best suits the machine. + // NOTE: Perhaps as a middle-ground, all that needs to be sent is a flag or special command that + // indicates to Grbl what is a backlash compensation motion, so that Grbl executes the move but + // doesn't update the machine position values. Since the position values used by the g-code + // parser and planner are separate from the system machine positions, this is doable. + +#ifdef ENABLE_BACKLASH_COMPENSATION + + if(backlash_enabled.mask) { + + bool backlash_comp = false; + uint_fast8_t idx = N_AXIS, axismask = bit(N_AXIS - 1); + + do { + idx--; + if(backlash_enabled.mask & axismask) { + if(target[idx] > target_prev[idx]) { + if (dir_negative.value & axismask) { + dir_negative.value &= ~axismask; + target_prev[idx] += settings.axis[idx].backlash; + backlash_comp = true; + } + } else if(target[idx] < target_prev[idx] && !(dir_negative.value & axismask)) { + dir_negative.value |= axismask; + target_prev[idx] -= settings.axis[idx].backlash; + backlash_comp = true; + } + } + axismask >>= 1; + } while(idx); + + if(backlash_comp) { + + plan_line_data_t pl_backlash; + + memset(&pl_backlash, 0, sizeof(plan_line_data_t)); + + pl_backlash.condition.rapid_motion = On; + pl_backlash.condition.backlash_motion = On; + pl_backlash.line_number = pl_data->line_number; + pl_backlash.spindle.rpm = pl_data->spindle.rpm; + + // If the buffer is full: good! That means we are well ahead of the robot. + // Remain in this loop until there is room in the buffer. + while(plan_check_full_buffer()) { + protocol_auto_cycle_start(); // Auto-cycle start when buffer is full. + if(!protocol_execute_realtime()) // Check for any run-time commands + return false; // Bail, if system abort. + } + + plan_buffer_line(target_prev, &pl_backlash); + } + + memcpy(target_prev, target, sizeof(float) * N_AXIS); + } + +#endif // Backlash comp + +#ifdef KINEMATICS_API + kinematics.segment_line(target, pl_data, true); + + while(kinematics.segment_line(target, pl_data, false)) { +#endif + // If the buffer is full: good! That means we are well ahead of the robot. + // Remain in this loop until there is room in the buffer. + do { + if(!protocol_execute_realtime()) // Check for any run-time commands + return false; // Bail, if system abort. + if(plan_check_full_buffer()) + protocol_auto_cycle_start(); // Auto-cycle start when buffer is full. + else + break; + } while(true); + + // Plan and queue motion into planner buffer + // bool plan_status; // Not used in normal operation. + if(!plan_buffer_line(target, pl_data) && settings.mode == Mode_Laser && pl_data->condition.spindle.on && !pl_data->condition.spindle.ccw) { + // Correctly set spindle state, if there is a coincident position passed. + // Forces a buffer sync while in M3 laser mode only. + hal.spindle.set_state(pl_data->condition.spindle, pl_data->spindle.rpm); + } +#ifdef KINEMATICS_API + } +#endif + } + + return !ABORTED; +} + + +// Execute an arc in offset mode format. position == current xyz, target == target xyz, +// offset == offset from current xyz, axis_X defines circle plane in tool space, axis_linear is +// the direction of helical travel, radius == circle radius, isclockwise boolean. Used +// for vector transformation direction. +// The arc is approximated by generating a huge number of tiny, linear segments. The chordal tolerance +// of each segment is configured in settings.arc_tolerance, which is defined to be the maximum normal +// distance from segment to the circle when the end points both lie on the circle. +void mc_arc (float *target, plan_line_data_t *pl_data, float *position, float *offset, float radius, + plane_t plane, bool is_clockwise_arc) +{ + float center_axis0 = position[plane.axis_0] + offset[plane.axis_0]; + float center_axis1 = position[plane.axis_1] + offset[plane.axis_1]; + float r_axis0 = -offset[plane.axis_0]; // Radius vector from center to current location + float r_axis1 = -offset[plane.axis_1]; + float rt_axis0 = target[plane.axis_0] - center_axis0; + float rt_axis1 = target[plane.axis_1] - center_axis1; + + // CCW angle between position and target from circle center. Only one atan2() trig computation required. + float angular_travel = atan2f(r_axis0 * rt_axis1 - r_axis1 * rt_axis0, r_axis0 * rt_axis0 + r_axis1 * rt_axis1); + + if (is_clockwise_arc) { // Correct atan2 output per direction + if (angular_travel >= -ARC_ANGULAR_TRAVEL_EPSILON) + angular_travel -= 2.0f * M_PI; + } else { + if (angular_travel <= ARC_ANGULAR_TRAVEL_EPSILON) + angular_travel += 2.0f * M_PI; + } + + // NOTE: Segment end points are on the arc, which can lead to the arc diameter being smaller by up to + // (2x) settings.arc_tolerance. For 99% of users, this is just fine. If a different arc segment fit + // is desired, i.e. least-squares, midpoint on arc, just change the mm_per_arc_segment calculation. + // For the intended uses of Grbl, this value shouldn't exceed 2000 for the strictest of cases. + uint16_t segments = (uint16_t)floorf(fabsf(0.5f * angular_travel * radius) / sqrtf(settings.arc_tolerance * (2.0f * radius - settings.arc_tolerance))); + + if (segments) { + + // Multiply inverse feed_rate to compensate for the fact that this movement is approximated + // by a number of discrete segments. The inverse feed_rate should be correct for the sum of + // all segments. + if (pl_data->condition.inverse_time) { + pl_data->feed_rate *= segments; + pl_data->condition.inverse_time = Off; // Force as feed absolute mode over arc segments. + } + + float theta_per_segment = angular_travel/segments; + float linear_per_segment = (target[plane.axis_linear] - position[plane.axis_linear]) / segments; + + /* Vector rotation by transformation matrix: r is the original vector, r_T is the rotated vector, + and phi is the angle of rotation. Solution approach by Jens Geisler. + r_T = [cos(phi) -sin(phi); + sin(phi) cos(phi] * r ; + + For arc generation, the center of the circle is the axis of rotation and the radius vector is + defined from the circle center to the initial position. Each line segment is formed by successive + vector rotations. Single precision values can accumulate error greater than tool precision in rare + cases. So, exact arc path correction is implemented. This approach avoids the problem of too many very + expensive trig operations [sin(),cos(),tan()] which can take 100-200 usec each to compute. + + Small angle approximation may be used to reduce computation overhead further. A third-order approximation + (second order sin() has too much error) holds for most, if not, all CNC applications. Note that this + approximation will begin to accumulate a numerical drift error when theta_per_segment is greater than + ~0.25 rad(14 deg) AND the approximation is successively used without correction several dozen times. This + scenario is extremely unlikely, since segment lengths and theta_per_segment are automatically generated + and scaled by the arc tolerance setting. Only a very large arc tolerance setting, unrealistic for CNC + applications, would cause this numerical drift error. However, it is best to set N_ARC_CORRECTION from a + low of ~4 to a high of ~20 or so to avoid trig operations while keeping arc generation accurate. + + This approximation also allows mc_arc to immediately insert a line segment into the planner + without the initial overhead of computing cos() or sin(). By the time the arc needs to be applied + a correction, the planner should have caught up to the lag caused by the initial mc_arc overhead. + This is important when there are successive arc motions. + */ + + // Computes: cos_T = 1 - theta_per_segment^2/2, sin_T = theta_per_segment - theta_per_segment^3/6) in ~52usec + float cos_T = 2.0f - theta_per_segment * theta_per_segment; + float sin_T = theta_per_segment * 0.16666667f * (cos_T + 4.0f); + cos_T *= 0.5f; + + float sin_Ti; + float cos_Ti; + float r_axisi; + uint32_t i, count = 0; + + for (i = 1; i < segments; i++) { // Increment (segments-1). + + if (count < N_ARC_CORRECTION) { + // Apply vector rotation matrix. ~40 usec + r_axisi = r_axis0 * sin_T + r_axis1 * cos_T; + r_axis0 = r_axis0 * cos_T - r_axis1 * sin_T; + r_axis1 = r_axisi; + count++; + } else { + // Arc correction to radius vector. Computed only every N_ARC_CORRECTION increments. ~375 usec + // Compute exact location by applying transformation matrix from initial radius vector(=-offset). + cos_Ti = cosf(i * theta_per_segment); + sin_Ti = sinf(i * theta_per_segment); + r_axis0 = -offset[plane.axis_0] * cos_Ti + offset[plane.axis_1] * sin_Ti; + r_axis1 = -offset[plane.axis_0] * sin_Ti - offset[plane.axis_1] * cos_Ti; + count = 0; + } + + // Update arc_target location + position[plane.axis_0] = center_axis0 + r_axis0; + position[plane.axis_1] = center_axis1 + r_axis1; + position[plane.axis_linear] += linear_per_segment; + + // Bail mid-circle on system abort. Runtime command check already performed by mc_line. + if(!mc_line(position, pl_data)) + return; + } + } + // Ensure last segment arrives at target location. + mc_line(target, pl_data); +} + +// Bezier splines, from a pull request for Marlin +// By Giovanni Mascellani - https://github.com/giomasce/Marlin + +// Compute the linear interpolation between two real numbers. +static inline float interp(const float a, const float b, const float t) +{ + return (1.0f - t) * a + t * b; +} + +/** + * Compute a B�zier curve using the De Casteljau's algorithm (see + * https://en.wikipedia.org/wiki/De_Casteljau's_algorithm), which is + * easy to code and has good numerical stability (very important, + * since Arudino works with limited precision real numbers). + */ +static inline float eval_bezier(const float a, const float b, const float c, const float d, const float t) +{ + const float iab = interp(a, b, t), + ibc = interp(b, c, t), + icd = interp(c, d, t), + iabc = interp(iab, ibc, t), + ibcd = interp(ibc, icd, t); + + return interp(iabc, ibcd, t); +} + +/** + * We approximate Euclidean distance with the sum of the coordinates + * offset (so-called "norm 1"), which is quicker to compute. + */ +static inline float dist1(const float x1, const float y1, const float x2, const float y2) +{ + return fabsf(x1 - x2) + fabsf(y1 - y2); +} + +/** + * The algorithm for computing the step is loosely based on the one in Kig + * (See https://sources.debian.net/src/kig/4:15.08.3-1/misc/kigpainter.cpp/#L759) + * However, we do not use the stack. + * + * The algorithm goes as it follows: the parameters t runs from 0.0 to + * 1.0 describing the curve, which is evaluated by eval_bezier(). At + * each iteration we have to choose a step, i.e., the increment of the + * t variable. By default the step of the previous iteration is taken, + * and then it is enlarged or reduced depending on how straight the + * curve locally is. The step is always clamped between MIN_STEP/2 and + * 2*MAX_STEP. MAX_STEP is taken at the first iteration. + * + * For some t, the step value is considered acceptable if the curve in + * the interval [t, t+step] is sufficiently straight, i.e., + * sufficiently close to linear interpolation. In practice the + * following test is performed: the distance between eval_bezier(..., + * t+step/2) is evaluated and compared with 0.5*(eval_bezier(..., + * t)+eval_bezier(..., t+step)). If it is smaller than SIGMA, then the + * step value is considered acceptable, otherwise it is not. The code + * seeks to find the larger step value which is considered acceptable. + * + * At every iteration the recorded step value is considered and then + * iteratively halved until it becomes acceptable. If it was already + * acceptable in the beginning (i.e., no halving were done), then + * maybe it was necessary to enlarge it; then it is iteratively + * doubled while it remains acceptable. The last acceptable value + * found is taken, provided that it is between MIN_STEP and MAX_STEP + * and does not bring t over 1.0. + * + * Caveat: this algorithm is not perfect, since it can happen that a + * step is considered acceptable even when the curve is not linear at + * all in the interval [t, t+step] (but its mid point coincides "by + * chance" with the midpoint according to the parametrization). This + * kind of glitches can be eliminated with proper first derivative + * estimates; however, given the improbability of such configurations, + * the mitigation offered by MIN_STEP and the small computational + * power available on Arduino, I think it is not wise to implement it. + */ + +void mc_cubic_b_spline (float *target, plan_line_data_t *pl_data, float *position, float *offset1, float *offset2) +{ + // Absolute first and second control points are recovered. + + float first[2] = { position[X_AXIS] + offset1[X_AXIS], position[Y_AXIS] + offset1[Y_AXIS] }; + float second[2] = { target[X_AXIS] + offset2[X_AXIS], target[Y_AXIS] + offset2[Y_AXIS] }; + float bez_target[N_AXIS]; + + memcpy(bez_target, position, sizeof(float) * N_AXIS); + + float t = 0.0f, step = BEZIER_MAX_STEP; + + while (t < 1.0f) { + + // First try to reduce the step in order to make it sufficiently + // close to a linear interpolation. + bool did_reduce = false; + float new_t = t + step; + + if(new_t > 1.0f) + new_t = 1.0f; + + float new_pos0 = eval_bezier(position[X_AXIS], first[X_AXIS], second[X_AXIS], target[X_AXIS], new_t), + new_pos1 = eval_bezier(position[Y_AXIS], first[Y_AXIS], second[Y_AXIS], target[Y_AXIS], new_t); + + while(new_t - t >= (BEZIER_MIN_STEP)) { + +// if (new_t - t < (BEZIER_MIN_STEP)) +// break; + + const float candidate_t = 0.5f * (t + new_t), + candidate_pos0 = eval_bezier(position[X_AXIS], first[X_AXIS], second[X_AXIS], target[X_AXIS], candidate_t), + candidate_pos1 = eval_bezier(position[Y_AXIS], first[Y_AXIS], second[Y_AXIS], target[Y_AXIS], candidate_t), + interp_pos0 = 0.5f * (bez_target[X_AXIS] + new_pos0), + interp_pos1 = 0.5f * (bez_target[Y_AXIS] + new_pos1); + + if (dist1(candidate_pos0, candidate_pos1, interp_pos0, interp_pos1) <= (BEZIER_SIGMA)) + break; + + new_t = candidate_t; + new_pos0 = candidate_pos0; + new_pos1 = candidate_pos1; + did_reduce = true; + } + + // If we did not reduce the step, maybe we should enlarge it. + if (!did_reduce) while (new_t - t <= BEZIER_MAX_STEP) { + +// if (new_t - t > BEZIER_MAX_STEP) +// break; + + const float candidate_t = t + 2.0f * (new_t - t); + + if (candidate_t >= 1.0f) + break; + + const float candidate_pos0 = eval_bezier(position[X_AXIS], first[X_AXIS], second[X_AXIS], target[X_AXIS], candidate_t), + candidate_pos1 = eval_bezier(position[Y_AXIS], first[Y_AXIS], second[Y_AXIS], target[Y_AXIS], candidate_t), + interp_pos0 = 0.5f * (bez_target[X_AXIS] + candidate_pos0), + interp_pos1 = 0.5f * (bez_target[Y_AXIS] + candidate_pos1); + + if (dist1(new_pos0, new_pos1, interp_pos0, interp_pos1) > (BEZIER_SIGMA)) + break; + + new_t = candidate_t; + new_pos0 = candidate_pos0; + new_pos1 = candidate_pos1; + } + + // Check some postcondition; they are disabled in the actual + // Marlin build, but if you test the same code on a computer you + // may want to check they are respect. + /* + assert(new_t <= 1.0); + if (new_t < 1.0) { + assert(new_t - t >= (MIN_STEP) / 2.0); + assert(new_t - t <= (MAX_STEP) * 2.0); + } + */ + + step = new_t - t; + t = new_t; + + bez_target[X_AXIS] = new_pos0; + bez_target[Y_AXIS] = new_pos1; + + // Bail mid-spline on system abort. Runtime command check already performed by mc_line. + if(!mc_line(bez_target, pl_data)) + return; + } +} + +// end Bezier splines + +void mc_canned_drill (motion_mode_t motion, float *target, plan_line_data_t *pl_data, float *position, plane_t plane, uint32_t repeats, gc_canned_t *canned) +{ + pl_data->condition.rapid_motion = On; // Set rapid motion condition flag. + + // if current Z < R, rapid move to R + if(position[plane.axis_linear] < canned->retract_position) { + position[plane.axis_linear] = canned->retract_position; + if(!mc_line(position, pl_data)) + return; + } + + // rapid move to X, Y + memcpy(position, target, sizeof(float) * N_AXIS); + position[plane.axis_linear] = canned->prev_position > canned->retract_position ? canned->prev_position : canned->retract_position; + if(!mc_line(position, pl_data)) + return; + + // if current Z > R, rapid move to R + if(position[plane.axis_linear] > canned->retract_position) { + position[plane.axis_linear] = canned->retract_position; + if(!mc_line(position, pl_data)) + return; + } + + if(canned->retract_mode == CCRetractMode_RPos) + canned->prev_position = canned->retract_position; + + while(repeats--) { + + float current_z = canned->retract_position; + + while(current_z > canned->xyz[plane.axis_linear]) { + + current_z -= canned->delta; + if(current_z < canned->xyz[plane.axis_linear]) + current_z = canned->xyz[plane.axis_linear]; + + pl_data->condition.rapid_motion = Off; + + position[plane.axis_linear] = current_z; + if(!mc_line(position, pl_data)) // drill + return; + + if(canned->dwell > 0.0f) + mc_dwell(canned->dwell); + + if(canned->spindle_off) + hal.spindle.set_state((spindle_state_t){0}, 0.0f); + + // rapid retract + switch(motion) { + + case MotionMode_DrillChipBreak: + position[plane.axis_linear] = position[plane.axis_linear] == canned->xyz[plane.axis_linear] + ? canned->retract_position + : position[plane.axis_linear] + settings.g73_retract; + break; + + default: + position[plane.axis_linear] = canned->retract_position; + break; + } + + pl_data->condition.rapid_motion = canned->rapid_retract; + if(!mc_line(position, pl_data)) + return; + + if(canned->spindle_off) + spindle_sync(gc_state.modal.spindle, pl_data->spindle.rpm); + } + + // rapid move to next position if incremental mode + if(repeats && gc_state.modal.distance_incremental) { + position[plane.axis_0] += canned->xyz[plane.axis_0]; + position[plane.axis_1] += canned->xyz[plane.axis_1]; + position[plane.axis_linear] = canned->prev_position; + if(!mc_line(position, pl_data)) + return; + } + } + + memcpy(target, position, sizeof(float) * N_AXIS); + + if(canned->retract_mode == CCRetractMode_Previous && motion != MotionMode_DrillChipBreak && target[plane.axis_linear] < canned->prev_position) { + pl_data->condition.rapid_motion = On; + target[plane.axis_linear] = canned->prev_position; + if(!mc_line(target, pl_data)) + return; + } +} + +// Calculates depth-of-cut (DOC) for a given threading pass. +inline static float calc_thread_doc (uint_fast16_t pass, float cut_depth, float inv_degression) +{ + return cut_depth * powf((float)pass, inv_degression); +} + +// Repeated cycle for threading +// G76 P- X- Z- I- J- R- K- Q- H- E- L- +// P - picth, X - main taper distance, Z - final position, I - thread peak offset, J - initial depth, K - full depth +// R - depth regression, Q - compound slide angle, H - spring passes, E - taper, L - taper end + +// TODO: change pitch to follow any tapers + +void mc_thread (plan_line_data_t *pl_data, float *position, gc_thread_data *thread, bool feed_hold_disabled) +{ + uint_fast16_t pass = 1, passes = 0; + float doc = thread->initial_depth, inv_degression = 1.0f / thread->depth_degression, thread_length; + float entry_taper_length = thread->end_taper_type & Taper_Entry ? thread->end_taper_length : 0.0f; + float exit_taper_length = thread->end_taper_type & Taper_Exit ? thread->end_taper_length : 0.0f; + float infeed_factor = tanf(thread->infeed_angle * RADDEG); + float target[N_AXIS], start_z = position[Z_AXIS] + thread->depth * infeed_factor; + + memcpy(target, position, sizeof(float) * N_AXIS); + + // Calculate number of passes + while(calc_thread_doc(++passes, doc, inv_degression) < thread->depth); + + passes += thread->spring_passes + 1; + + if((thread_length = thread->z_final - position[Z_AXIS]) > 0.0f) { + if(thread->end_taper_type & Taper_Entry) + entry_taper_length = -entry_taper_length; + if(thread->end_taper_type & Taper_Exit) + exit_taper_length = - exit_taper_length; + } + + thread_length += entry_taper_length + exit_taper_length; + + if(thread->main_taper_height != 0.0f) + thread->main_taper_height = thread->main_taper_height * thread_length / (thread_length - (entry_taper_length + exit_taper_length)); + + pl_data->condition.rapid_motion = On; // Set rapid motion condition flag. + + // TODO: Add to initial move to compensate for acceleration distance? + /* + float acc_distance = pl_data->feed_rate * hal.spindle.get_data(SpindleData_RPM)->rpm / settings.acceleration[Z_AXIS]; + acc_distance = acc_distance * acc_distance * settings.acceleration[Z_AXIS] * 0.5f; + */ + + // Initial Z-move for compound slide angle offset. + if(infeed_factor != 0.0f) { + target[Z_AXIS] = start_z - doc * infeed_factor; + if(!mc_line(target, pl_data)) + return; + } + + while(--passes) { + + if(thread->end_taper_type & Taper_Entry) + target[X_AXIS] = position[X_AXIS] + (thread->peak + doc - thread->depth) * thread->cut_direction; + else + target[X_AXIS] = position[X_AXIS] + (thread->peak + doc) * thread->cut_direction; + + if(!mc_line(target, pl_data)) + return; + + if(!protocol_buffer_synchronize() && state_get() != STATE_IDLE) // Wait until any previous moves are finished. + return; + + pl_data->condition.rapid_motion = Off; // Clear rapid motion condition flag, + pl_data->condition.spindle.synchronized = On; // enable spindle sync for cut + pl_data->overrides.feed_hold_disable = On; // and disable feed hold + + // Cut thread pass + + // 1. Entry taper + if(thread->end_taper_type & Taper_Entry) { + + target[X_AXIS] += thread->depth * thread->cut_direction; + target[Z_AXIS] -= entry_taper_length; + if(!mc_line(target, pl_data)) + return; + } + + // 2. Main part + target[Z_AXIS] += thread_length; + if(!mc_line(target, pl_data)) + return; + + // 3. Exit taper + if(thread->end_taper_type & Taper_Exit) { + + target[X_AXIS] -= thread->depth * thread->cut_direction; + target[Z_AXIS] -= exit_taper_length; + if(!mc_line(target, pl_data)) + return; + } + + pl_data->condition.rapid_motion = On; // Set rapid motion condition flag and + pl_data->condition.spindle.synchronized = Off; // disable spindle sync for retract & reposition + + if(passes > 1) { + + // Get DOC of next pass. + doc = calc_thread_doc(++pass, thread->initial_depth, inv_degression); + doc = min(doc, thread->depth); + + // 4. Retract + target[X_AXIS] = position[X_AXIS] + (doc - thread->depth) * thread->cut_direction; + if(!mc_line(target, pl_data)) + return; + + // Restore disable feed hold status for reposition move. + pl_data->overrides.feed_hold_disable = feed_hold_disabled; + + // 5. Back to start, add compound slide angle offset when commanded. + target[Z_AXIS] = start_z - (infeed_factor != 0.0f ? doc * infeed_factor : 0.0f); + if(!mc_line(target, pl_data)) + return; + + } else { + + doc = thread->depth; + target[X_AXIS] = position[X_AXIS]; + if(!mc_line(target, pl_data)) + return; + } + } +} + +// Sets up valid jog motion received from g-code parser, checks for soft-limits, and executes the jog. +status_code_t mc_jog_execute (plan_line_data_t *pl_data, parser_block_t *gc_block) +{ + // Initialize planner data struct for jogging motions. + // NOTE: Spindle and coolant are allowed to fully function with overrides during a jog. + pl_data->feed_rate = gc_block->values.f; + pl_data->condition.no_feed_override = On; + pl_data->condition.jog_motion = On; + pl_data->line_number = gc_block->values.n; + + if(settings.limits.flags.jog_soft_limited) + system_apply_jog_limits(gc_block->values.xyz); + else if (settings.limits.flags.soft_enabled && !system_check_travel_limits(gc_block->values.xyz)) + return Status_TravelExceeded; + + // Valid jog command. Plan, set state, and execute. + mc_line(gc_block->values.xyz, pl_data); + sys_state_t state = state_get(); + if ((state == STATE_IDLE || state == STATE_TOOL_CHANGE) && plan_get_current_block() != NULL) { // Check if there is a block to execute. + state_set(STATE_JOG); + st_prep_buffer(); + st_wake_up(); // NOTE: Manual start. No state machine required. + } + + return Status_OK; +} + +// Execute dwell in seconds. +void mc_dwell (float seconds) +{ + if (state_get() != STATE_CHECK_MODE) { + protocol_buffer_synchronize(); + delay_sec(seconds, DelayMode_Dwell); + } +} + + +// Perform homing cycle to locate and set machine zero. Only '$H' executes this command. +// NOTE: There should be no motions in the buffer and Grbl must be in an idle state before +// executing the homing cycle. This prevents incorrect buffered plans after homing. +status_code_t mc_homing_cycle (axes_signals_t cycle) +{ + bool home_all = cycle.mask == 0; + + memset(&sys.last_event.limits, 0, sizeof(limit_signals_t)); + + if(settings.homing.flags.manual && (home_all ? sys.homing.mask : (cycle.mask & sys.homing.mask)) == 0) { + + if(home_all) + cycle.mask = AXES_BITMASK; + + tc_clear_tlo_reference(cycle); + + sys.homed.mask |= cycle.mask; +#ifdef KINEMATICS_API + kinematics.limits_set_machine_positions(cycle); +#else + limits_set_machine_positions(cycle, false); +#endif + } else { + + // Check and abort homing cycle, if hard limits are already enabled. Helps prevent problems + // with machines with limits wired on both ends of travel to one limit pin. + // TODO: Move the pin-specific LIMIT_PIN call to limits.c as a function. + if (settings.limits.flags.two_switches && hal.homing.get_state == hal.limits.get_state && limit_signals_merge(hal.limits.get_state()).value) { + mc_reset(); // Issue system reset and ensure spindle and coolant are shutdown. + system_set_exec_alarm(Alarm_HardLimit); + return Status_Unhandled; + } + + state_set(STATE_HOMING); // Set homing system state. +#if COMPATIBILITY_LEVEL == 0 + hal.stream.enqueue_realtime_command(CMD_STATUS_REPORT); // Force a status report and + delay_sec(0.1f, DelayMode_Dwell); // delay a bit to get it sent (or perhaps wait a bit for a request?) +#endif + hal.limits.enable(false, true); // Disable hard limits pin change register for cycle duration + + // Turn off spindle and coolant (and update parser state) + gc_state.spindle.rpm = 0.0f; + gc_state.modal.spindle.on = gc_state.modal.spindle.ccw = Off; + spindle_set_state(gc_state.modal.spindle, 0.0f); + + gc_state.modal.coolant.mask = 0; + coolant_set_state(gc_state.modal.coolant); + + // ------------------------------------------------------------------------------------- + // Perform homing routine. NOTE: Special motion case. Only system reset works. + + if (!home_all) // Perform homing cycle based on mask. + limits_go_home(cycle); + else { + + uint_fast8_t idx = 0; + + sys.homed.mask &= ~sys.homing.mask; + + do { + if(settings.homing.cycle[idx].mask) { + cycle.mask = settings.homing.cycle[idx].mask; + if(!limits_go_home(cycle)) + break; + } + } while(++idx < N_AXIS); + } + + // If hard limits feature enabled, re-enable hard limits pin change register after homing cycle. + // NOTE: always call at end of homing regadless of setting, may be used to disable + // sensorless homing or switch back to limit switches input (if different from homing switches) + hal.limits.enable(settings.limits.flags.hard_enabled, false); + } + + if(cycle.mask) { + + if(!protocol_execute_realtime()) // Check for reset and set system abort. + return Status_Unhandled; // Did not complete. Alarm state set by mc_alarm. + + if(home_all && settings.homing.flags.manual) + { + cycle.mask = AXES_BITMASK & ~sys.homing.mask; + sys.homed.mask = AXES_BITMASK; +#ifdef KINEMATICS_API + kinematics.limits_set_machine_positions(cycle); +#else + limits_set_machine_positions(cycle, false); +#endif + } + + // Homing cycle complete! Setup system for normal operation. + // ------------------------------------------------------------------------------------- + + // Sync gcode parser and planner positions to homed position. + sync_position(); + } + + sys.report.homed = On; + + return settings.limits.flags.hard_enabled && settings.limits.flags.check_at_init && limit_signals_merge(hal.limits.get_state()).value + ? Status_LimitsEngaged + : Status_OK; +} + + +// Perform tool length probe cycle. Requires probe switch. +// NOTE: Upon probe failure, the program will be stopped and placed into ALARM state. +gc_probe_t mc_probe_cycle (float *target, plan_line_data_t *pl_data, gc_parser_flags_t parser_flags) +{ + // TODO: Need to update this cycle so it obeys a non-auto cycle start. + if (state_get() == STATE_CHECK_MODE) + return GCProbe_CheckMode; + + // Finish all queued commands and empty planner buffer before starting probe cycle. + if (!protocol_buffer_synchronize()) + return GCProbe_Abort; // Return if system reset has been issued. + + // Initialize probing control variables + sys.flags.probe_succeeded = Off; // Re-initialize probe history before beginning cycle. + hal.probe.configure(parser_flags.probe_is_away, true); + + // After syncing, check if probe is already triggered or not connected. If so, halt and issue alarm. + // NOTE: This probe initialization error applies to all probing cycles. + probe_state_t probe = hal.probe.get_state(); + if (probe.triggered || !probe.connected) { // Check probe state. + system_set_exec_alarm(Alarm_ProbeFailInitial); + protocol_execute_realtime(); + hal.probe.configure(false, false); // Re-initialize invert mask before returning. + return GCProbe_FailInit; // Nothing else to do but bail. + } + + // Setup and queue probing motion. Auto cycle-start should not start the cycle. + if(!mc_line(target, pl_data)) + return GCProbe_Abort; + + // Activate the probing state monitor in the stepper module. + sys.probing_state = Probing_Active; + + // Perform probing cycle. Wait here until probe is triggered or motion completes. + system_set_exec_state_flag(EXEC_CYCLE_START); + do { + if(!protocol_execute_realtime()) // Check for system abort + return GCProbe_Abort; + } while (!(state_get() == STATE_IDLE || state_get() == STATE_TOOL_CHANGE)); + + // Probing cycle complete! + + // Set state variables and error out, if the probe failed and cycle with error is enabled. + if (sys.probing_state == Probing_Active) { + if (parser_flags.probe_is_no_error) + memcpy(sys.probe_position, sys.position, sizeof(sys.position)); + else + system_set_exec_alarm(Alarm_ProbeFailContact); + } else + sys.flags.probe_succeeded = On; // Indicate to system the probing cycle completed successfully. + + sys.probing_state = Probing_Off; // Ensure probe state monitor is disabled. + hal.probe.configure(false, false); // Re-initialize invert mask. + protocol_execute_realtime(); // Check and execute run-time commands + + // Reset the stepper and planner buffers to remove the remainder of the probe motion. + st_reset(); // Reset step segment buffer. + plan_reset(); // Reset planner buffer. Zero planner positions. Ensure probing motion is cleared. + plan_sync_position(); // Sync planner position to current machine position. + + // All done! Output the probe position as message if configured. + if(settings.status_report.probe_coordinates) + report_probe_parameters(); + + if(grbl.on_probe_completed) + grbl.on_probe_completed(); + + // Successful probe cycle or Failed to trigger probe within travel. With or without error. + return sys.flags.probe_succeeded ? GCProbe_Found : GCProbe_FailEnd; +} + + +// Plans and executes the single special motion case for parking. Independent of main planner buffer. +// NOTE: Uses the always free planner ring buffer head to store motion parameters for execution. +bool mc_parking_motion (float *parking_target, plan_line_data_t *pl_data) +{ + bool ok; + + if (sys.abort) + return false; // Block during abort. + + if ((ok = plan_buffer_line(parking_target, pl_data))) { + sys.step_control.execute_sys_motion = On; + sys.step_control.end_motion = Off; // Allow parking motion to execute, if feed hold is active. + st_parking_setup_buffer(); // Setup step segment buffer for special parking motion case. + st_prep_buffer(); + st_wake_up(); + } + + return ok; +} + +void mc_override_ctrl_update (gc_override_flags_t override_state) +{ +// Finish all queued commands before altering override control state + protocol_buffer_synchronize(); + if (!sys.abort) + sys.override.control = override_state; +} + +// Method to ready the system to reset by setting the realtime reset command and killing any +// active processes in the system. This also checks if a system reset is issued while Grbl +// is in a motion state. If so, kills the steppers and sets the system alarm to flag position +// lost, since there was an abrupt uncontrolled deceleration. Called at an interrupt level by +// realtime abort command and hard limits. So, keep to a minimum. +ISR_CODE void mc_reset () +{ + // Only this function can set the system reset. Helps prevent multiple kill calls. + if (bit_isfalse(sys.rt_exec_state, EXEC_RESET)) { + + system_set_exec_state_flag(EXEC_RESET); + + if(hal.stream.suspend_read) + hal.stream.suspend_read(false); + + // Kill steppers only if in any motion state, i.e. cycle, actively holding, or homing. + // NOTE: If steppers are kept enabled via the step idle delay setting, this also keeps + // the steppers enabled by avoiding the go_idle call altogether, unless the motion state is + // violated, by which, all bets are off. + if ((state_get() & (STATE_CYCLE|STATE_HOMING|STATE_JOG)) || sys.step_control.execute_hold || sys.step_control.execute_sys_motion) { + + sys.position_lost = true; + + if (state_get() != STATE_HOMING) + system_set_exec_alarm(Alarm_AbortCycle); + else if (!sys.rt_exec_alarm) + system_set_exec_alarm(Alarm_HomingFailReset); + + st_go_idle(); // Force kill steppers. Position has likely been lost. + } + + if(hal.control.get_state().e_stop) + system_set_exec_alarm(Alarm_EStop); + else if(hal.control.get_state().motor_fault) + system_set_exec_alarm(Alarm_MotorFault); + } +} diff --git a/motion_control.h b/motion_control.h new file mode 100644 index 0000000..564d25b --- /dev/null +++ b/motion_control.h @@ -0,0 +1,82 @@ +/* + motion_control.h - high level interface for issuing motion commands + + Part of grblHAL + + Copyright (c) 2017-2019 Terje Io + Copyright (c) 2011-2016 Sungeun K. Jeon for Gnea Research LLC + Copyright (c) 2009-2011 Simen Svale Skogsrud + + 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 . +*/ + +#ifndef _MOTION_CONTROL_H_ +#define _MOTION_CONTROL_H_ + +// System motion commands must have a line number of zero. +#define HOMING_CYCLE_LINE_NUMBER 0 +#define PARKING_MOTION_LINE_NUMBER 0 + +#define HOMING_CYCLE_ALL 0 // Must be zero. +#define HOMING_CYCLE_X bit(X_AXIS) +#define HOMING_CYCLE_Y bit(Y_AXIS) +#define HOMING_CYCLE_Z bit(Z_AXIS) + +// Execute linear motion in absolute millimeter coordinates. Feed rate given in millimeters/second +// unless invert_feed_rate is true. Then the feed_rate means that the motion should be completed in +// (1 minute)/feed_rate time. +bool mc_line(float *target, plan_line_data_t *pl_data); + +// Execute an arc in offset mode format. position == current xyz, target == target xyz, +// offset == offset from current xyz, axis_XXX defines circle plane in tool space, axis_linear is +// the direction of helical travel, radius == circle radius, is_clockwise_arc boolean. Used +// for vector transformation direction. +void mc_arc(float *target, plan_line_data_t *pl_data, float *position, float *offset, float radius, + plane_t plane, bool is_clockwise_arc); + +// Execute canned cycle (drill) +void mc_canned_drill (motion_mode_t motion, float *target, plan_line_data_t *pl_data, float *position, plane_t plane, uint32_t repeats, gc_canned_t *canned); + +// Execute canned cycle (threading) +void mc_thread (plan_line_data_t *pl_data, float *position, gc_thread_data *thread, bool feed_hold_disabled); + +// Sets up valid jog motion received from g-code parser, checks for soft-limits, and executes the jog. +status_code_t mc_jog_execute(plan_line_data_t *pl_data, parser_block_t *gc_block); + +// Dwell for a specific number of seconds +void mc_dwell(float seconds); + +// Perform homing cycle to locate machine zero. Requires limit switches. +status_code_t mc_homing_cycle(axes_signals_t cycle); + +// Perform tool length probe cycle. Requires probe switch. +gc_probe_t mc_probe_cycle(float *target, plan_line_data_t *pl_data, gc_parser_flags_t parser_flags); + +// Handles updating the override control state. +void mc_override_ctrl_update(gc_override_flags_t override_state); + +// Plans and executes the single special motion case for parking. Independent of main planner buffer. +bool mc_parking_motion(float *parking_target, plan_line_data_t *pl_data); + +void mc_cubic_b_spline(float *target, plan_line_data_t *pl_data, float *position, float *offset1, float *offset2); + +// Performs system reset. If in motion state, kills all motion and sets system alarm. +void mc_reset(); + +#ifdef ENABLE_BACKLASH_COMPENSATION +void mc_backlash_init (void); +void mc_sync_backlash_position (void); +#endif + +#endif diff --git a/my_plugin.c b/my_plugin.c new file mode 100644 index 0000000..7409901 --- /dev/null +++ b/my_plugin.c @@ -0,0 +1,27 @@ +/* + my_plugin.c - An embedded CNC Controller with rs274/ngc (g-code) support + + "Dummy" user plugin init, called if no user plugin is provided. + + Part of grblHAL + + Copyright (c) 2020 Terje Io + + Grbl is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + Grbl is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with Grbl. If not, see . +*/ + +__attribute__((weak)) void my_plugin_init (void) +{ + // NOOP +} diff --git a/nuts_bolts.c b/nuts_bolts.c new file mode 100644 index 0000000..d571493 --- /dev/null +++ b/nuts_bolts.c @@ -0,0 +1,293 @@ +/* + nuts_bolts.c - Shared functions + + Part of grblHAL + + Copyright (c) 2017-2021 Terje Io + Copyright (c) 2011-2016 Sungeun K. Jeon for Gnea Research LLC + Copyright (c) 2009-2011 Simen Svale Skogsrud + + 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 . +*/ + +#include +#include +#include + +#include "hal.h" +#include "protocol.h" +#include "state_machine.h" +#include "nuts_bolts.h" + +#ifndef DWELL_TIME_STEP +#define DWELL_TIME_STEP 50 // Integer (1-255) (milliseconds) +#endif + +#define MAX_PRECISION 10 + +static char buf[STRLEN_COORDVALUE + 1]; + +static const float froundvalues[MAX_PRECISION + 1] = +{ + 0.5, // 0 + 0.05, // 1 + 0.005, // 2 + 0.0005, // 3 + 0.00005, // 4 + 0.000005, // 5 + 0.0000005, // 6 + 0.00000005, // 7 + 0.000000005, // 8 + 0.0000000005, // 9 + 0.00000000005 // 10 +}; + +char const *const axis_letter[N_AXIS] = { + "X", + "Y", + "Z" +#if N_AXIS > 3 + ,"A" +#endif +#if N_AXIS > 4 + ,"B" +#endif +#if N_AXIS > 5 + ,"C" +#endif +}; + +// Converts an uint32 variable to string. +char *uitoa (uint32_t n) +{ + char *bptr = buf + sizeof(buf); + + *--bptr = '\0'; + + if (n == 0) + *--bptr = '0'; + else while (n) { + *--bptr = '0' + (n % 10); + n /= 10; + } + + return bptr; +} + +// Convert float to string by immediately converting to integers. +// Number of decimal places, which are tracked by a counter, must be set by the user. +// The integers is then efficiently converted to a string. +char *ftoa (float n, uint8_t decimal_places) +{ + bool isNegative; + char *bptr = buf + sizeof(buf); + + *--bptr = '\0'; + + if ((isNegative = n < 0.0f)) + n = -n; + + n += froundvalues[decimal_places]; + + uint32_t a = (uint32_t)n; + + if (decimal_places) { + + n -= (float)a; + + uint_fast8_t decimals = decimal_places; + while (decimals >= 2) { // Quickly convert values expected to be E0 to E-4. + n *= 100.0f; + decimals -= 2; + } + + if (decimals) + n *= 10.0f; + + uint32_t b = (uint32_t)n; + + while(decimal_places--) { + if(b) { + *--bptr = (b % 10) + '0'; // Get digit + b /= 10; + } else + *--bptr = '0'; + } + } + + *--bptr = '.'; // Always add decimal point (TODO: is this really needed?) + + if(a == 0) + *--bptr = '0'; + + else while(a) { + *--bptr = (a % 10) + '0'; // Get digit + a /= 10; + } + + if(isNegative) + *--bptr = '-'; + + return bptr; +} + +// Extracts a floating point value from a string. The following code is based loosely on +// the avr-libc strtod() function by Michael Stumpf and Dmitry Xmelkov and many freely +// available conversion method examples, but has been highly optimized for Grbl. For known +// CNC applications, the typical decimal value is expected to be in the range of E0 to E-4. +// Scientific notation is officially not supported by g-code, and the 'E' character may +// be a g-code word on some CNC systems. So, 'E' notation will not be recognized. +// NOTE: Thanks to Radu-Eosif Mihailescu for identifying the issues with using strtod(). +bool read_float (char *line, uint_fast8_t *char_counter, float *float_ptr) +{ + char *ptr = line + *char_counter; + int_fast8_t exp = 0; + uint_fast8_t ndigit = 0, c; + uint32_t intval = 0; + bool isnegative, isdecimal = false; + + // Grab first character and increment pointer. No spaces assumed in line. + c = *ptr++; + + // Capture initial positive/minus character + if ((isnegative = (c == '-')) || c == '+') + c = *ptr++; + + // Extract number into fast integer. Track decimal in terms of exponent value. + while(c) { + c -= '0'; + if (c <= 9) { + ndigit++; + if (ndigit <= MAX_INT_DIGITS) { + if (isdecimal) + exp--; + intval = (((intval << 2) + intval) << 1) + c; // intval*10 + c + } else if (!isdecimal) + exp++; // Drop overflow digits + } else if (c == (uint_fast8_t)('.' - '0') && !isdecimal) + isdecimal = true; + else + break; + + c = *ptr++; + } + + // Return if no digits have been read. + if (!ndigit) + return false; + + // Convert integer into floating point. + float fval = (float)intval; + + // Apply decimal. Should perform no more than two floating point multiplications for the + // expected range of E0 to E-4. + if (fval != 0.0f) { + while (exp <= -2) { + fval *= 0.01f; + exp += 2; + } + if (exp < 0) + fval *= 0.1f; + else if (exp > 0) do { + fval *= 10.0f; + } while (--exp > 0); + } + + // Assign floating point value with correct sign. + *float_ptr = isnegative ? - fval : fval; + *char_counter = ptr - line - 1; // Set char_counter to next statement + + return true; +} + +// Returns true if float value is a whole number (integer) +bool isintf (float value) +{ + return value != NAN && fabsf(value - truncf(value)) < 0.001f; +} + +// Non-blocking delay function used for general operation and suspend features. +void delay_sec (float seconds, delaymode_t mode) +{ + uint_fast16_t i = (uint_fast16_t)ceilf((1000.0f / DWELL_TIME_STEP) * seconds) + 1; + + while (--i && !sys.abort) { + if (mode == DelayMode_Dwell) { + protocol_execute_realtime(); + } else { // DelayMode_SysSuspend + // Execute rt_system() only to avoid nesting suspend loops. + protocol_exec_rt_system(); + if (state_door_reopened()) // Bail, if safety door reopens. + return; + } + hal.delay_ms(DWELL_TIME_STEP, 0); // Delay DWELL_TIME_STEP increment + } +} + + +float convert_delta_vector_to_unit_vector (float *vector) +{ + uint_fast8_t idx = N_AXIS; + float magnitude = 0.0f, inv_magnitude; + + do { + if (vector[--idx] != 0.0f) + magnitude += vector[idx] * vector[idx]; + } while(idx); + + idx = N_AXIS; + magnitude = sqrtf(magnitude); + inv_magnitude = 1.0f / magnitude; + + do { + vector[--idx] *= inv_magnitude; + } while(idx); + + return magnitude; +} + + +// calculate checksum byte for data +uint8_t calc_checksum (uint8_t *data, uint32_t size) { + + uint8_t checksum = 0; + + while(size--) { + checksum = (checksum << 1) | (checksum >> 7); + checksum += *(data++); + } + + return checksum; +} + +// Remove spaces from and convert string to uppercase (in situ) +char *strcaps (char *s) +{ + char c, *s1 = s, *s2 = s; + + do { + c = *s1++; + if(c != ' ') + *s2++ = CAPS(c); + } while(c); + + *s2 = '\0'; + + return s; +} + +void dummy_handler (void) +{ + // NOOP +} diff --git a/nuts_bolts.h b/nuts_bolts.h new file mode 100644 index 0000000..70a00a9 --- /dev/null +++ b/nuts_bolts.h @@ -0,0 +1,176 @@ +/* + nuts_bolts.h - Header file for shared definitions, variables, and functions + + Part of grblHAL + + Copyright (c) 2017-2021 Terje Io + Copyright (c) 2011-2016 Sungeun K. Jeon for Gnea Research LLC + Copyright (c) 2009-2011 Simen Svale Skogsrud + + 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 . +*/ + +#ifndef _NUTS_BOLTS_H_ +#define _NUTS_BOLTS_H_ + +#include "grbl.h" + +#ifndef true +#define false 0 +#define true 1 +#endif + +#define Off 0 +#define On 1 + +#define SOME_LARGE_VALUE 1.0E+38f +#ifndef M_PI +#define M_PI 3.14159265358979323846f +#endif + +#define TAN_30 0.57735f // Used for threading calculations (60 degree inserts) +#define RADDEG 0.0174532925f // Radians per degree + +#define ABORTED (sys.abort || sys.cancel) + +// Convert character to uppercase +#define CAPS(c) ((c >= 'a' && c <= 'z') ? c & 0x5F : c) + +#ifndef STM32F103xB +#ifndef UNUSED +#define UNUSED(x) (void)(x) +#endif +#endif + +// Axis array index values. Must start with 0 and be continuous. +#define X_AXIS 0 // Axis indexing value. +#define Y_AXIS 1 +#define Z_AXIS 2 +#define X_AXIS_BIT bit(X_AXIS) +#define Y_AXIS_BIT bit(Y_AXIS) +#define Z_AXIS_BIT bit(Z_AXIS) +#if N_AXIS > 3 +#define A_AXIS 3 +#define A_AXIS_BIT bit(A_AXIS) +#endif +#if N_AXIS > 4 +#define B_AXIS 4 +#define B_AXIS_BIT bit(B_AXIS) +#endif +#if N_AXIS == 6 +#define C_AXIS 5 +#define C_AXIS_BIT bit(C_AXIS) +#define AXES_BITMASK (X_AXIS_BIT|Y_AXIS_BIT|Z_AXIS_BIT|A_AXIS_BIT|B_AXIS_BIT|C_AXIS_BIT) +#endif + +#if N_AXIS == 3 +#define AXES_BITMASK (X_AXIS_BIT|Y_AXIS_BIT|Z_AXIS_BIT) +#elif N_AXIS == 4 +#define AXES_BITMASK (X_AXIS_BIT|Y_AXIS_BIT|Z_AXIS_BIT|A_AXIS_BIT) +#elif N_AXIS == 5 +#define AXES_BITMASK (X_AXIS_BIT|Y_AXIS_BIT|Z_AXIS_BIT|A_AXIS_BIT|B_AXIS_BIT) +#endif + +extern char const *const axis_letter[]; + +typedef union { + uint8_t mask; + uint8_t value; + struct { + uint8_t x :1, + y :1, + z :1, + a :1, + b :1, + c :1; + }; +} axes_signals_t; + +#pragma pack(push, 1) + +typedef struct { + axes_signals_t min; + axes_signals_t max; + axes_signals_t min2; + axes_signals_t max2; +} limit_signals_t; + +#pragma pack(pop) + +typedef enum { + DelayMode_Dwell = 0, + DelayMode_SysSuspend +} delaymode_t; + + // Delay struct, currently not used by core - may be used by drivers +typedef struct { + volatile uint32_t ms; + void (*callback)(void); +} delay_t; + +// Conversions +#define MM_PER_INCH (25.40f) +#define INCH_PER_MM (0.0393701f) + +#define MAX_INT_DIGITS 8 // Maximum number of digits in int32 (and float) +#define STRLEN_COORDVALUE (MAX_INT_DIGITS + N_DECIMAL_COORDVALUE_INCH + 1) // 8.4 format - excluding terminating null + +// Useful macros +#define clear_vector(a) memset(a, 0, sizeof(a)) +#ifndef max +#define max(a,b) (((a) > (b)) ? (a) : (b)) +#endif +#ifndef min +#define min(a,b) (((a) < (b)) ? (a) : (b)) +#endif +#define isequal_position_vector(a,b) !memcmp(a, b, sizeof(coord_data_t)) + +// Bit field and masking macros +#ifndef bit +#define bit(n) (1UL << n) +#endif +#define bit_true(x,mask) (x) |= (mask) +#define bit_false(x,mask) (x) &= ~(mask) +#define BIT_SET(x, bit, v) { if (v) { x |= (bit); } else { x &= ~(bit); } } + +#define bit_istrue(x, mask) ((x & (mask)) != 0) +#define bit_isfalse(x, mask) ((x & (mask)) == 0) + +// Converts an uint32 variable to string. +char *uitoa (uint32_t n); + +// Converts a float variable to string with the specified number of decimal places. +char *ftoa (float n, uint8_t decimal_places); + +// Returns true if float value is a whole number (integer) +bool isintf (float value); + +// Read a floating point value from a string. Line points to the input buffer, char_counter +// is the indexer pointing to the current character of the line, while float_ptr is +// a pointer to the result variable. Returns true when it succeeds +bool read_float(char *line, uint_fast8_t *char_counter, float *float_ptr); + +// Non-blocking delay function used for general operation and suspend features. +void delay_sec(float seconds, delaymode_t mode); + +float convert_delta_vector_to_unit_vector(float *vector); + +// calculate checksum byte for data +uint8_t calc_checksum (uint8_t *data, uint32_t size); + +char *strcaps (char *s); + +void dummy_handler (void); + +#endif diff --git a/nvs.h b/nvs.h new file mode 100644 index 0000000..7ac03de --- /dev/null +++ b/nvs.h @@ -0,0 +1,77 @@ +/* + nvs.h - non-volative storage data structures + + Part of grblHAL + + Copyright (c) 2017-2020 Terje Io + Copyright (c) 2009-2011 Simen Svale Skogsrud + + 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 . +*/ + +#ifndef _NVS_H_ +#define _NVS_H_ + +#ifndef NVS_SIZE +#define NVS_SIZE 2048 +#endif + +#define GRBL_NVS_SIZE 1024 +#define NVS_CRC_BYTES 1 + +// Define persistent storage memory address location values for Grbl settings and parameters +// NOTE: 1KB persistent storage is the minimum required. The upper half is reserved for parameters and +// the startup script. The lower half contains the global settings and space for future +// developments. +#define NVS_ADDR_GLOBAL 1U +#define NVS_ADDR_PARAMETERS 512U +#define NVS_ADDR_BUILD_INFO 942U +#define NVS_ADDR_STARTUP_BLOCK (NVS_ADDR_BUILD_INFO - 1 - N_STARTUP_LINE * (sizeof(stored_line_t) + NVS_CRC_BYTES)) +#ifdef N_TOOLS +#define NVS_ADDR_TOOL_TABLE (NVS_ADDR_PARAMETERS - 1 - N_TOOLS * (sizeof(tool_data_t) + NVS_CRC_BYTES)) +#endif + +typedef enum { + NVS_None = 0, + NVS_EEPROM, + NVS_FRAM, + NVS_Flash, + NVS_Emulated +} nvs_type; + +typedef struct { + uint8_t *mem_address; + uint16_t address; + uint16_t size; +} nvs_driver_area_t; + +typedef enum { + NVS_TransferResult_Failed = 0, + NVS_TransferResult_Busy, + NVS_TransferResult_OK, +} nvs_transfer_result_t; + +typedef struct { + nvs_type type; + uint16_t size; + nvs_driver_area_t driver_area; + uint8_t (*get_byte)(uint32_t addr); + void (*put_byte)(uint32_t addr, uint8_t new_value); + nvs_transfer_result_t (*memcpy_to_nvs)(uint32_t destination, uint8_t *source, uint32_t size, bool with_checksum); + nvs_transfer_result_t (*memcpy_from_nvs)(uint8_t *destination, uint32_t source, uint32_t size, bool with_checksum); + bool (*memcpy_from_flash)(uint8_t *dest); + bool (*memcpy_to_flash)(uint8_t *source); +} nvs_io_t; + +#endif diff --git a/nvs_buffer.c b/nvs_buffer.c new file mode 100644 index 0000000..2903644 --- /dev/null +++ b/nvs_buffer.c @@ -0,0 +1,387 @@ +/* + nvs_buffer.c - RAM based non-volatile storage buffer/emulation + + Part of grblHAL + + Copyright (c) 2017-2021 Terje Io + Copyright (c) 2012-2016 Sungeun K. Jeon for Gnea Research LLC + Copyright (c) 2009-2011 Simen Svale Skogsrud + + 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 . +*/ + +// +// Can be used by MCUs with no nonvolatile storage options, be sure to allocate enough heap memory before use +// + +#include +#include +#include + +#include "hal.h" +#include "nvs_buffer.h" +#include "protocol.h" + +static uint8_t *nvsbuffer = NULL; +static nvs_io_t physical_nvs; +static bool dirty; + +settings_dirty_t settings_dirty; + +typedef struct { + uint16_t addr; + uint8_t type; + uint8_t offset; +} emap_t; + +#define NVS_GROUP_GLOBAL 0 +#define NVS_GROUP_TOOLS 1 +#define NVS_GROUP_PARAMETERS 2 +#define NVS_GROUP_STARTUP 3 +#define NVS_GROUP_BUILD 4 + +#define PARAMETER_ADDR(n) (NVS_ADDR_PARAMETERS + n * (sizeof(coord_data_t) + NVS_CRC_BYTES)) +#define STARTLINE_ADDR(n) (NVS_ADDR_STARTUP_BLOCK + n * (sizeof(stored_line_t) + NVS_CRC_BYTES)) +#ifdef N_TOOLS +#define TOOL_ADDR(n) (NVS_ADDR_TOOL_TABLE + n * (sizeof(tool_data_t) + NVS_CRC_BYTES)) +#endif + +static const emap_t target[] = { + {NVS_ADDR_GLOBAL, NVS_GROUP_GLOBAL, 0}, +#ifdef N_TOOLS + {TOOL_ADDR(0), NVS_GROUP_TOOLS, 0}, + {TOOL_ADDR(1), NVS_GROUP_TOOLS, 1}, + {TOOL_ADDR(2), NVS_GROUP_TOOLS, 2}, + {TOOL_ADDR(3), NVS_GROUP_TOOLS, 3}, + {TOOL_ADDR(4), NVS_GROUP_TOOLS, 4}, + {TOOL_ADDR(5), NVS_GROUP_TOOLS, 5}, + {TOOL_ADDR(6), NVS_GROUP_TOOLS, 6}, + {TOOL_ADDR(7), NVS_GROUP_TOOLS, 7}, +#if N_TOOL > 8 +#error Increase number of tool entries! +#endif +#endif + {PARAMETER_ADDR(0), NVS_GROUP_PARAMETERS, 0}, + {PARAMETER_ADDR(1), NVS_GROUP_PARAMETERS, 1}, + {PARAMETER_ADDR(2), NVS_GROUP_PARAMETERS, 2}, + {PARAMETER_ADDR(3), NVS_GROUP_PARAMETERS, 3}, + {PARAMETER_ADDR(4), NVS_GROUP_PARAMETERS, 4}, + {PARAMETER_ADDR(5), NVS_GROUP_PARAMETERS, 5}, + {PARAMETER_ADDR(6), NVS_GROUP_PARAMETERS, 6}, + {PARAMETER_ADDR(7), NVS_GROUP_PARAMETERS, 7}, + {PARAMETER_ADDR(8), NVS_GROUP_PARAMETERS, 8}, + {PARAMETER_ADDR(9), NVS_GROUP_PARAMETERS, 9}, + {PARAMETER_ADDR(10), NVS_GROUP_PARAMETERS, 10}, + {PARAMETER_ADDR(11), NVS_GROUP_PARAMETERS, 11}, + {STARTLINE_ADDR(0), NVS_GROUP_STARTUP, 0}, + {STARTLINE_ADDR(1), NVS_GROUP_STARTUP, 1}, +#if N_STARTUP_LINE > 2 +#error Increase number of startup line entries! +#endif + {NVS_ADDR_BUILD_INFO, NVS_GROUP_BUILD, 0}, + {0, 0, 0} // list termination - do not remove +}; + +inline static uint8_t ram_get_byte (uint32_t addr) +{ + return nvsbuffer[addr]; +} + +inline static void ram_put_byte (uint32_t addr, uint8_t new_value) +{ + dirty = dirty || nvsbuffer[addr] != new_value; + nvsbuffer[addr] = new_value; +} + +// Extensions added as part of Grbl + +static nvs_transfer_result_t memcpy_to_ram (uint32_t destination, uint8_t *source, uint32_t size, bool with_checksum) +{ + if(hal.nvs.driver_area.address && destination > hal.nvs.driver_area.address + hal.nvs.driver_area.size) + return physical_nvs.memcpy_to_nvs(destination, source, size, with_checksum); + + uint32_t dest = destination; + uint8_t checksum = with_checksum ? calc_checksum(source, size) : 0; + + dirty = false; + + for(; size > 0; size--) + ram_put_byte(dest++, *(source++)); + + if(with_checksum) + ram_put_byte(dest, checksum); + + if(source == hal.nvs.driver_area.mem_address) + dirty = true; + + if(dirty && physical_nvs.type != NVS_None) { + + uint8_t idx = 0; + + settings_dirty.is_dirty = true; + + if(hal.nvs.driver_area.address && destination >= hal.nvs.driver_area.address) + settings_dirty.driver_settings = true; + + else { + + do { + if(target[idx].addr == destination) + break; + } while(target[++idx].addr); + + if(target[idx].addr) switch(target[idx].type) { + + case NVS_GROUP_GLOBAL: + settings_dirty.global_settings = true; + break; +#ifdef N_TOOLS + case NVS_GROUP_TOOLS: + settings_dirty.tool_data |= (1 << target[idx].offset); + break; +#endif + case NVS_GROUP_PARAMETERS: + settings_dirty.coord_data |= (1 << target[idx].offset); + break; + + case NVS_GROUP_STARTUP: + settings_dirty.startup_lines |= (1 << target[idx].offset); + break; + + case NVS_GROUP_BUILD: + settings_dirty.build_info = true; + break; + } + } + } + + return NVS_TransferResult_OK; +} + +static nvs_transfer_result_t memcpy_from_ram (uint8_t *destination, uint32_t source, uint32_t size, bool with_checksum) +{ + if(hal.nvs.driver_area.address && source > hal.nvs.driver_area.address + hal.nvs.driver_area.size) + return physical_nvs.memcpy_from_nvs(destination, source, size, with_checksum); + + uint8_t checksum = with_checksum ? calc_checksum(&nvsbuffer[source], size) : 0; + + for(; size > 0; size--) + *(destination++) = ram_get_byte(source++); + + return with_checksum ? (checksum == ram_get_byte(source) ? NVS_TransferResult_OK : NVS_TransferResult_Failed) : NVS_TransferResult_OK; +} + +static void nvs_warning (sys_state_t state) +{ + report_message("Not enough heap for NVS buffer!", Message_Warning); +} + +// Try to allocate RAM from heap for buffer/emulation. +bool nvs_buffer_alloc (void) +{ + assert(NVS_SIZE >= GRBL_NVS_SIZE); + + nvsbuffer = malloc(NVS_SIZE); + + return nvsbuffer != NULL; +} + +// +// Switch over to RAM based copy. +// Changes to RAM based copy will be written to physical storage when Grbl is in IDLE state. +bool nvs_buffer_init (void) +{ + if(nvsbuffer) { + + memcpy(&physical_nvs, &hal.nvs, sizeof(nvs_io_t)); // save pointers to physical storage handler functions + + // Copy physical storage content to RAM when available + if(physical_nvs.type == NVS_Flash) + physical_nvs.memcpy_from_flash(nvsbuffer); + else if(physical_nvs.type != NVS_None) + physical_nvs.memcpy_from_nvs(nvsbuffer, 0, GRBL_NVS_SIZE + hal.nvs.driver_area.size, false); + + // Switch hal to use RAM version of non-volatile storage data + hal.nvs.type = NVS_Emulated; + hal.nvs.get_byte = &ram_get_byte; + hal.nvs.put_byte = &ram_put_byte; + hal.nvs.memcpy_to_nvs = &memcpy_to_ram; + hal.nvs.memcpy_from_nvs = &memcpy_from_ram; + hal.nvs.memcpy_from_flash = NULL; + hal.nvs.memcpy_to_flash = NULL; + + // If no physical storage available or if NVS import fails copy default settings to RAM + // and write out to physical storage when available. + if(physical_nvs.type == NVS_None || ram_get_byte(0) != SETTINGS_VERSION) { + settings_restore(settings_all); + if(physical_nvs.type == NVS_Flash) + physical_nvs.memcpy_to_flash(nvsbuffer); + else if(physical_nvs.memcpy_to_nvs) + physical_nvs.memcpy_to_nvs(0, nvsbuffer, GRBL_NVS_SIZE + hal.nvs.driver_area.size, false); + if(physical_nvs.type != NVS_None) + grbl.report.status_message(Status_SettingReadFail); + } + } else + protocol_enqueue_rt_command(nvs_warning); + + // Clear settings dirty flags + memset(&settings_dirty, 0, sizeof(settings_dirty_t)); + + return nvsbuffer != NULL; +} + +// Allocate NVS block for driver settings. +// NOTE: allocation has to be done before content is copied from physical storage. +nvs_address_t nvs_alloc (size_t size) +{ + static uint8_t *mem_address; + + nvs_address_t addr = 0; + + // Check if already switched to emulation or buffer allocation failed, return NULL if so. + if(hal.nvs.type == NVS_Emulated || nvsbuffer == NULL) + return 0; + + if(hal.nvs.driver_area.address == 0) { + hal.nvs.driver_area.address = GRBL_NVS_SIZE; + hal.nvs.driver_area.mem_address = mem_address = nvsbuffer + GRBL_NVS_SIZE; + } + + size += NVS_CRC_BYTES; // add room for checksum. + if(hal.nvs.driver_area.size + size < (NVS_SIZE - GRBL_NVS_SIZE)) { + mem_address = (uint8_t *)((uint32_t)(mem_address - 1) | 0x03) + 1; // Align to word boundary + addr = mem_address - nvsbuffer; + mem_address += size; + hal.nvs.driver_area.size = mem_address - hal.nvs.driver_area.mem_address; + hal.nvs.size = GRBL_NVS_SIZE + hal.nvs.driver_area.size + 1; + } + + return addr; +} + +// Write RAM changes to physical storage +void nvs_buffer_sync_physical (void) +{ + if(!settings_dirty.is_dirty) + return; + + if(physical_nvs.memcpy_to_nvs) { + + if(settings_dirty.build_info) + settings_dirty.build_info = physical_nvs.memcpy_to_nvs(NVS_ADDR_BUILD_INFO, (uint8_t *)(nvsbuffer + NVS_ADDR_BUILD_INFO), sizeof(stored_line_t) + NVS_CRC_BYTES, false) != NVS_TransferResult_OK; + + if(settings_dirty.global_settings) + settings_dirty.global_settings = physical_nvs.memcpy_to_nvs(NVS_ADDR_GLOBAL, (uint8_t *)(nvsbuffer + NVS_ADDR_GLOBAL), sizeof(settings_t) + NVS_CRC_BYTES, false) != NVS_TransferResult_OK; + + uint_fast8_t idx = N_STARTUP_LINE, offset; + if(settings_dirty.startup_lines) do { + idx--; + if(bit_istrue(settings_dirty.startup_lines, bit(idx))) { + bit_false(settings_dirty.startup_lines, bit(idx)); + offset = NVS_ADDR_STARTUP_BLOCK + idx * (sizeof(stored_line_t) + NVS_CRC_BYTES); + if(physical_nvs.memcpy_to_nvs(offset, (uint8_t *)(nvsbuffer + offset), sizeof(stored_line_t) + NVS_CRC_BYTES, false) == NVS_TransferResult_OK) + bit_false(settings_dirty.startup_lines, bit(idx)); + } + } while(idx); + + idx = N_CoordinateSystems; + if(settings_dirty.coord_data) do { + if(bit_istrue(settings_dirty.coord_data, bit(idx))) { + offset = NVS_ADDR_PARAMETERS + idx * (sizeof(coord_data_t) + NVS_CRC_BYTES); + if(physical_nvs.memcpy_to_nvs(offset, (uint8_t *)(nvsbuffer + offset), sizeof(coord_data_t) + NVS_CRC_BYTES, false) == NVS_TransferResult_OK) + bit_false(settings_dirty.coord_data, bit(idx)); + } + } while(idx--); + + if(settings_dirty.driver_settings) { + if(hal.nvs.driver_area.size > 0) + settings_dirty.driver_settings = physical_nvs.memcpy_to_nvs(hal.nvs.driver_area.address, (uint8_t *)(nvsbuffer + hal.nvs.driver_area.address), hal.nvs.driver_area.size, false) != NVS_TransferResult_OK; + else + settings_dirty.driver_settings = false; + } + +#ifdef N_TOOLS + idx = N_TOOLS; + if(settings_dirty.tool_data) do { + idx--; + if(bit_istrue(settings_dirty.tool_data, bit(idx))) { + offset = NVS_ADDR_TOOL_TABLE + idx * (sizeof(tool_data_t) + NVS_CRC_BYTES); + if(physical_nvs.memcpy_to_nvs(offset, (uint8_t *)(nvsbuffer + offset), sizeof(tool_data_t) + NVS_CRC_BYTES, false) == NVS_TransferResult_OK) + bit_false(settings_dirty.tool_data, bit(idx)); + } + } while(idx); +#endif + settings_dirty.is_dirty = settings_dirty.coord_data || + settings_dirty.global_settings || + settings_dirty.driver_settings || + settings_dirty.startup_lines || +#ifdef N_TOOLS + settings_dirty.tool_data || +#endif + settings_dirty.build_info; + + } else if(physical_nvs.memcpy_to_flash) { + physical_nvs.memcpy_to_flash(nvsbuffer); + settings_dirty.is_dirty = false; + } +} + +nvs_io_t *nvs_buffer_get_physical (void) +{ + return hal.nvs.type == NVS_Emulated ? &physical_nvs : &hal.nvs; +} + +#ifndef DEBUGOUT + +#include "report.h" + +void nvs_memmap (void) +{ + char buf[30]; + + report_message("NVS Area: addr size", Message_Plain); + + strcpy(buf, "Global: "); + strcat(buf, uitoa(NVS_ADDR_GLOBAL)); + strcat(buf, " "); + strcat(buf, uitoa(sizeof(settings_t) + NVS_CRC_BYTES)); + report_message(buf, Message_Plain); + + strcpy(buf, "Parameters: "); + strcat(buf, uitoa(NVS_ADDR_PARAMETERS)); + strcat(buf, " "); + strcat(buf, uitoa(N_CoordinateSystems * (sizeof(coord_data_t) + NVS_CRC_BYTES))); + report_message(buf, Message_Plain); + + strcpy(buf, "Startup block: "); + strcat(buf, uitoa(NVS_ADDR_STARTUP_BLOCK)); + strcat(buf, " "); + strcat(buf, uitoa(N_STARTUP_LINE * (sizeof(stored_line_t) + NVS_CRC_BYTES))); + report_message(buf, Message_Plain); + + strcpy(buf, "Build info: "); + strcat(buf, uitoa(NVS_ADDR_BUILD_INFO)); + strcat(buf, " "); + strcat(buf, uitoa(sizeof(stored_line_t) + NVS_CRC_BYTES)); + report_message(buf, Message_Plain); + + strcpy(buf, "Driver: "); + strcat(buf, uitoa(hal.nvs.driver_area.address)); + strcat(buf, " "); + strcat(buf, uitoa(hal.nvs.driver_area.size)); + report_message(buf, Message_Plain); +} + +#endif + diff --git a/nvs_buffer.h b/nvs_buffer.h new file mode 100644 index 0000000..9cbb132 --- /dev/null +++ b/nvs_buffer.h @@ -0,0 +1,49 @@ +/* + nvs_buffer.h - RAM based non-volatile storage buffer/emulation + + Part of grblHAL + + Copyright (c) 2017-2020 Terje Io + Copyright (c) 2009-2011 Simen Svale Skogsrud + + 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 . +*/ + +#ifndef _NVS_BUFFER_H_ +#define _NVS_BUFFER_H_ + +typedef uint32_t nvs_address_t; + +typedef struct { + bool is_dirty; + bool global_settings; + bool build_info; + bool driver_settings; + uint8_t startup_lines; + uint16_t coord_data; +#ifdef N_TOOLS + uint16_t tool_data; +#endif +} settings_dirty_t; + +extern settings_dirty_t settings_dirty; + +bool nvs_buffer_init (void); +bool nvs_buffer_alloc (void); +nvs_address_t nvs_alloc (size_t size); +void nvs_buffer_sync_physical (void); +nvs_io_t *nvs_buffer_get_physical (void); +void nvs_memmap (void); + +#endif diff --git a/override.c b/override.c new file mode 100644 index 0000000..f81456c --- /dev/null +++ b/override.c @@ -0,0 +1,85 @@ +/* + override.c - An embedded CNC Controller with rs274/ngc (g-code) support + + Buffer handlers for real-time override commands + + Part of grblHAL + + Copyright (c) 2017-2019 Terje Io + + Grbl is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + Grbl is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with Grbl. If not, see . +*/ + +#include "grbl.h" +#include "override.h" + +typedef struct { + volatile uint_fast8_t head; + volatile uint_fast8_t tail; + uint8_t buf[OVERRIDE_BUFSIZE]; +} override_queue_t; + +static override_queue_t feed = {0}, accessory = {0}; + +ISR_CODE void enqueue_feed_override (uint8_t cmd) +{ + uint_fast8_t bptr = (feed.head + 1) & (OVERRIDE_BUFSIZE - 1); // Get next head pointer + + if(bptr != feed.tail) { // If not buffer full + feed.buf[feed.head] = cmd; // add data to buffer + feed.head = bptr; // and update pointer + } +} + +// Returns 0 if no commands enqueued +uint8_t get_feed_override (void) +{ + uint8_t data = 0; + uint_fast8_t bptr = feed.tail; + + if(bptr != feed.head) { + data = feed.buf[bptr++]; // Get next character, increment tmp pointer + feed.tail = bptr & (OVERRIDE_BUFSIZE - 1); // and update pointer + } + + return data; +} + +ISR_CODE void enqueue_accessory_override (uint8_t cmd) +{ + uint_fast8_t bptr = (accessory.head + 1) & (OVERRIDE_BUFSIZE - 1); // Get next head pointer + + if(bptr != accessory.tail) { // If not buffer full + accessory.buf[accessory.head] = cmd; // add data to buffer + accessory.head = bptr; // and update pointer + } +} + +// Returns 0 if no commands enqueued +uint8_t get_accessory_override (void) +{ + uint8_t data = 0; + uint_fast8_t bptr = accessory.tail; + + if(bptr != accessory.head) { + data = accessory.buf[bptr++]; // Get next character, increment tmp pointer + accessory.tail = bptr & (OVERRIDE_BUFSIZE - 1); // and update pointer + } + + return data; +} + +void flush_override_buffers () { + feed.head = feed.tail = accessory.head = accessory.tail = 0; +} diff --git a/override.h b/override.h new file mode 100644 index 0000000..990ccab --- /dev/null +++ b/override.h @@ -0,0 +1,37 @@ +/* + override.h - An embedded CNC Controller with rs274/ngc (g-code) support + + Buffer handlers for real-time override commands + + Part of grblHAL + + Copyright (c) 2016-2019 Terje Io + + Grbl is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + Grbl is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with Grbl. If not, see . +*/ + +#ifndef _OVERRIDE_H_ +#define _OVERRIDE_H_ + +#ifndef OVERRIDE_BUFSIZE +#define OVERRIDE_BUFSIZE 16 // must be a power of 2 +#endif + +void flush_override_buffers (); +void enqueue_feed_override (uint8_t cmd); +uint8_t get_feed_override (void); +void enqueue_accessory_override (uint8_t cmd); +uint8_t get_accessory_override (void); + +#endif diff --git a/pid.c b/pid.c new file mode 100644 index 0000000..af23915 --- /dev/null +++ b/pid.c @@ -0,0 +1,105 @@ +/* + pid.c - An embedded CNC Controller with rs274/ngc (g-code) support + + PID algorithm for closed loop control + + NOTE: not referenced in the core grbl code + + Part of grblHAL + + Copyright (c) 2020-2021 Terje Io + + Grbl is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + Grbl is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with Grbl. If not, see . +*/ + +#include + +#include "pid.h" + +// Fixed point version: TODO + +// Float version + +void pidf_init (pidf_t *pid, pid_values_t *config) +{ + pidf_reset(pid); + memcpy(&pid->cfg, config, sizeof(pid_values_t)); +} + +bool pidf_config_changed (pidf_t *pid, pid_values_t *config) +{ + return memcmp(&pid->cfg, config, sizeof(pid_values_t)); +} + +void pidf_reset (pidf_t *pid) +{ + pid->error = 0.0f; + pid->i_error = 0.0f; + pid->d_error = 0.0f; + pid->sample_rate_prev = 1.0f; +} + +float pidf (pidf_t *pid, float command, float actual, float sample_rate) +{ + float error = command - actual; +/* + if(error > pid->deadband) + error -= pid->deadband; + else if (error < pid->deadband) + error += pid->deadband; + else + error = 0.0f; +*/ + // calculate the proportional term + float pidres = pid->cfg.p_gain * error; + + // calculate and add the integral term + pid->i_error += error * (pid->sample_rate_prev / sample_rate); + + if(pid->cfg.i_max_error != 0.0f) { + if (pid->i_error > pid->cfg.i_max_error) + pid->i_error = pid->cfg.i_max_error; + else if (pid->i_error < -pid->cfg.i_max_error) + pid->i_error = -pid->cfg.i_max_error; + } + + pidres += pid->cfg.i_gain * pid->i_error; + + // calculate and add the derivative term + if(pid->cfg.d_gain != 0.0f) { + float p_error = (error - pid->d_error) * (sample_rate / pid->sample_rate_prev); + if(pid->cfg.d_max_error != 0.0f) { + if (p_error > pid->cfg.d_max_error) + p_error = pid->cfg.d_max_error; + else if (p_error < -pid->cfg.d_max_error) + p_error = -pid->cfg.d_max_error; + } + pidres += pid->cfg.d_gain * p_error; + pid->d_error = error; + } + + pid->sample_rate_prev = sample_rate; + + // limit error output + if(pid->cfg.max_error != 0.0f) { + if(pidres > pid->cfg.max_error) + pidres = pid->cfg.max_error; + else if(pidres < -pid->cfg.max_error) + pidres = -pid->cfg.max_error; + } + + pid->error = pidres; + + return pidres; +} diff --git a/pid.h b/pid.h new file mode 100644 index 0000000..34ea7c0 --- /dev/null +++ b/pid.h @@ -0,0 +1,48 @@ +/* + pid.h - An embedded CNC Controller with rs274/ngc (g-code) support + + PID algorithm for closed loop control + + NOTE: not referenced in the core grbl code + + Part of grblHAL + + Copyright (c) 2020-2021 Terje Io + + Grbl is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + Grbl is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with Grbl. If not, see . +*/ + +#ifndef _PID_H_ +#define _PID_H_ + +#include + +#include "settings.h" + +typedef struct { + pid_values_t cfg; + float deadband; + float i_error; + float d_error; + float sample_rate_prev; + float error; + float max_error; +} pidf_t; + +void pidf_reset (pidf_t *pid); +void pidf_init(pidf_t *pid, pid_values_t *config); +bool pidf_config_changed (pidf_t *pid, pid_values_t *config); +float pidf (pidf_t *pid, float command, float actual, float sample_rate); + +#endif diff --git a/planner.c b/planner.c new file mode 100644 index 0000000..3aa384d --- /dev/null +++ b/planner.c @@ -0,0 +1,576 @@ +/* + planner.c - buffers movement commands and manages the acceleration profile plan + + Part of grblHAL + + Copyright (c) 2017-2021 Terje Io + Copyright (c) 2011-2016 Sungeun K. Jeon for Gnea Research LLC + Copyright (c) 2009-2011 Simen Svale Skogsrud + Copyright (c) 2011 Jens Geisler + + 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 . +*/ + +#include +#include +#include + +#include "hal.h" +#include "nuts_bolts.h" +#include "planner.h" +#ifdef KINEMATICS_API +#include "kinematics.h" +#endif + +#ifndef MINIMUM_JUNCTION_SPEED +#define MINIMUM_JUNCTION_SPEED 0.0f +#endif +#ifndef MINIMUM_FEED_RATE +#define MINIMUM_FEED_RATE 1.0f +#endif + +static plan_block_t block_buffer[BLOCK_BUFFER_SIZE]; // A ring buffer for motion instructions +static plan_block_t *block_buffer_tail; // Pointer to the block to process now +static plan_block_t *block_buffer_head; // Pointer to the next block to be pushed +static plan_block_t *next_buffer_head; // Pointer to the next buffer head +static plan_block_t *block_buffer_planned; // Pointer to the optimally planned block + +static planner_t pl; + + +/* PLANNER SPEED DEFINITION + +--------+ <- current->nominal_speed + / \ + current->entry_speed -> + \ + | + <- next->entry_speed (aka exit speed) + +-------------+ + time --> + + Recalculates the motion plan according to the following basic guidelines: + + 1. Go over every feasible block sequentially in reverse order and calculate the junction speeds + (i.e. current->entry_speed) such that: + a. No junction speed exceeds the pre-computed maximum junction speed limit or nominal speeds of + neighboring blocks. + b. A block entry speed cannot exceed one reverse-computed from its exit speed (next->entry_speed) + with a maximum allowable deceleration over the block travel distance. + c. The last (or newest appended) block is planned from a complete stop (an exit speed of zero). + 2. Go over every block in chronological (forward) order and dial down junction speed values if + a. The exit speed exceeds the one forward-computed from its entry speed with the maximum allowable + acceleration over the block travel distance. + + When these stages are complete, the planner will have maximized the velocity profiles throughout the all + of the planner blocks, where every block is operating at its maximum allowable acceleration limits. In + other words, for all of the blocks in the planner, the plan is optimal and no further speed improvements + are possible. If a new block is added to the buffer, the plan is recomputed according to the said + guidelines for a new optimal plan. + + To increase computational efficiency of these guidelines, a set of planner block pointers have been + created to indicate stop-compute points for when the planner guidelines cannot logically make any further + changes or improvements to the plan when in normal operation and new blocks are streamed and added to the + planner buffer. For example, if a subset of sequential blocks in the planner have been planned and are + bracketed by junction velocities at their maximums (or by the first planner block as well), no new block + added to the planner buffer will alter the velocity profiles within them. So we no longer have to compute + them. Or, if a set of sequential blocks from the first block in the planner (or a optimal stop-compute + point) are all accelerating, they are all optimal and can not be altered by a new block added to the + planner buffer, as this will only further increase the plan speed to chronological blocks until a maximum + junction velocity is reached. However, if the operational conditions of the plan changes from infrequently + used feed holds or feedrate overrides, the stop-compute pointers will be reset and the entire plan is + recomputed as stated in the general guidelines. + + Planner buffer pointer mapping: + - block_buffer_tail: Points to the beginning of the planner buffer. First to be executed or being executed. + - block_buffer_head: Points to the buffer block after the last block in the buffer. Used to indicate whether + the buffer is full or empty. As described for standard ring buffers, this block is always empty. + - next_buffer_head: Points to next planner buffer block after the buffer head block. When equal to the + buffer tail, this indicates the buffer is full. + - block_buffer_planned: Points to the first buffer block after the last optimally planned block for normal + streaming operating conditions. Use for planning optimizations by avoiding recomputing parts of the + planner buffer that don't change with the addition of a new block, as describe above. In addition, + this block can never be less than block_buffer_tail and will always be pushed forward and maintain + this requirement when encountered by the plan_discard_current_block() routine during a cycle. + + NOTE: Since the planner only computes on what's in the planner buffer, some motions with lots of short + line segments, like G2/3 arcs or complex curves, may seem to move slow. This is because there simply isn't + enough combined distance traveled in the entire buffer to accelerate up to the nominal speed and then + decelerate to a complete stop at the end of the buffer, as stated by the guidelines. If this happens and + becomes an annoyance, there are a few simple solutions: (1) Maximize the machine acceleration. The planner + will be able to compute higher velocity profiles within the same combined distance. (2) Maximize line + motion(s) distance per block to a desired tolerance. The more combined distance the planner has to use, + the faster it can go. (3) Maximize the planner buffer size. This also will increase the combined distance + for the planner to compute over. It also increases the number of computations the planner has to perform + to compute an optimal plan, so select carefully. ARM versions should have enough memory and speed for + look-ahead blocks numbering up to a hundred or more. + +*/ +static void planner_recalculate () +{ + // Initialize block pointer to the last block in the planner buffer. + plan_block_t *block = block_buffer_head->prev; + + // Bail. Can't do anything with one only one plan-able block. + if (block == block_buffer_planned) + return; + + // Reverse Pass: Coarsely maximize all possible deceleration curves back-planning from the last + // block in buffer. Cease planning when the last optimal planned or tail pointer is reached. + // NOTE: Forward pass will later refine and correct the reverse pass to create an optimal plan. + float entry_speed_sqr; + plan_block_t *next; + plan_block_t *current = block; + + // Calculate maximum entry speed for last block in buffer, where the exit speed is always zero. + current->entry_speed_sqr = min(current->max_entry_speed_sqr, 2.0f * current->acceleration * current->millimeters); + + block = block->prev; + if (block == block_buffer_planned) { // Only two plannable blocks in buffer. Reverse pass complete. + // Check if the first block is the tail. If so, notify stepper to update its current parameters. + if (block == block_buffer_tail) + st_update_plan_block_parameters(); + } else while (block != block_buffer_planned) { // Three or more plan-able blocks + + next = current; + current = block; + block = block->prev; + + // Check if next block is the tail block(=planned block). If so, update current stepper parameters. + if (block == block_buffer_tail) + st_update_plan_block_parameters(); + + // Compute maximum entry speed decelerating over the current block from its exit speed. + if (current->entry_speed_sqr != current->max_entry_speed_sqr) { + entry_speed_sqr = next->entry_speed_sqr + 2.0f * current->acceleration * current->millimeters; + current->entry_speed_sqr = entry_speed_sqr < current->max_entry_speed_sqr ? entry_speed_sqr : current->max_entry_speed_sqr; + } + } + + // Forward Pass: Forward plan the acceleration curve from the planned pointer onward. + // Also scans for optimal plan breakpoints and appropriately updates the planned pointer. + next = block_buffer_planned; // Begin at buffer planned pointer + block = block_buffer_planned->next; + + while (block != block_buffer_head) { + + current = next; + next = block; + + // Any acceleration detected in the forward pass automatically moves the optimal planned + // pointer forward, since everything before this is all optimal. In other words, nothing + // can improve the plan from the buffer tail to the planned pointer by logic. + if (current->entry_speed_sqr < next->entry_speed_sqr) { + entry_speed_sqr = current->entry_speed_sqr + 2.0f * current->acceleration * current->millimeters; + // If true, current block is full-acceleration and we can move the planned pointer forward. + if (entry_speed_sqr < next->entry_speed_sqr) { + next->entry_speed_sqr = entry_speed_sqr; // Always <= max_entry_speed_sqr. Backward pass sets this. + block_buffer_planned = block; // Set optimal plan pointer. + } + } + + // Any block set at its maximum entry speed also creates an optimal plan up to this + // point in the buffer. When the plan is bracketed by either the beginning of the + // buffer and a maximum entry speed or two maximum entry speeds, every block in between + // cannot logically be further improved. Hence, we don't have to recompute them anymore. + if (next->entry_speed_sqr == next->max_entry_speed_sqr) + block_buffer_planned = block; + + block = block->next; + } +} + +inline static void plan_cleanup (plan_block_t *block) +{ + if(block->message) { + free(block->message); + block->message = NULL; + } + + while(block->output_commands) { + output_command_t *next = block->output_commands->next; + free(block->output_commands); + block->output_commands = next; + } +} + + +inline static void plan_reset_buffer (bool soft_reset) +{ + if(soft_reset) { + // Free memory for any pending messages and output commands after soft reset + while(block_buffer_tail != block_buffer_head) { + plan_cleanup(block_buffer_tail); + block_buffer_tail = block_buffer_tail->next; + } + } + + block_buffer_tail = block_buffer_head = &block_buffer[0]; // Empty = tail == head + next_buffer_head = block_buffer_head->next; // = next block + block_buffer_planned = block_buffer_tail; // = block_buffer_tail +} + + +void plan_reset () +{ + static bool soft_reset = false; + + memset(&pl, 0, sizeof(planner_t)); // Clear planner struct + + // Set up stepper block ringbuffer as circular doubly linked list + uint_fast8_t idx; + for(idx = 0 ; idx <= BLOCK_BUFFER_SIZE - 1 ; idx++) { + block_buffer[idx].prev = &block_buffer[idx == 0 ? BLOCK_BUFFER_SIZE - 1 : idx - 1]; + block_buffer[idx].next = &block_buffer[idx == BLOCK_BUFFER_SIZE - 1 ? 0 : idx + 1]; + } + + plan_reset_buffer(soft_reset); + soft_reset = true; +} + + +void plan_discard_current_block () +{ + if (block_buffer_tail != block_buffer_head) { // Discard non-empty buffer. + plan_cleanup(block_buffer_tail); + // Push block_buffer_planned pointer, if encountered. + if (block_buffer_tail == block_buffer_planned) + block_buffer_planned = block_buffer_tail->next; + block_buffer_tail = block_buffer_tail->next; + } +} + + +// Returns address of planner buffer block used by system motions. Called by segment generator. +plan_block_t *plan_get_system_motion_block () +{ + return block_buffer_head; +} + + +// Returns address of first planner block, if available. Called by various main program functions. +plan_block_t *plan_get_current_block () +{ + return block_buffer_head == block_buffer_tail ? NULL : block_buffer_tail; +} + + +inline float plan_get_exec_block_exit_speed_sqr () +{ + plan_block_t *block = block_buffer_tail->next; + return block == block_buffer_head ? 0.0f : block->entry_speed_sqr; +} + + +// Returns the availability status of the block ring buffer. True, if full. +bool plan_check_full_buffer () +{ + return block_buffer_tail == next_buffer_head; +} + + +// Computes and returns block nominal speed based on running condition and override values. +// NOTE: All system motion commands, such as homing/parking, are not subject to overrides. +float plan_compute_profile_nominal_speed (plan_block_t *block) +{ + float nominal_speed = block->condition.spindle.synchronized ? block->programmed_rate * hal.spindle.get_data(SpindleData_RPM)->rpm : block->programmed_rate; + + if (block->condition.rapid_motion) + nominal_speed *= (0.01f * sys.override.rapid_rate); + else { + if (!block->condition.no_feed_override) + nominal_speed *= (0.01f * sys.override.feed_rate); + if (nominal_speed > block->rapid_rate) + nominal_speed = block->rapid_rate; + } + +// TODO: if nominal speed is outside bounds when synchronized motion is on then (?? retract and) abort, ignore overrides? + return nominal_speed > MINIMUM_FEED_RATE ? nominal_speed : MINIMUM_FEED_RATE; +} + + +// Computes and updates the max entry speed (sqr) of the block, based on the minimum of the junction's +// previous and current nominal speeds and max junction speed. +inline static float plan_compute_profile_parameters (plan_block_t *block, float nominal_speed, float prev_nominal_speed) +{ + // Compute the junction maximum entry based on the minimum of the junction speed and neighboring nominal speeds. + block->max_entry_speed_sqr = nominal_speed > prev_nominal_speed ? (prev_nominal_speed * prev_nominal_speed) : (nominal_speed * nominal_speed); + if (block->max_entry_speed_sqr > block->max_junction_speed_sqr) + block->max_entry_speed_sqr = block->max_junction_speed_sqr; + return nominal_speed; +} + +// Re-calculates buffered motions profile parameters upon a motion-based override change. +void plan_update_velocity_profile_parameters () +{ + plan_block_t *block = block_buffer_tail; + float prev_nominal_speed = SOME_LARGE_VALUE; // Set high for first block nominal speed calculation. + + while (block != block_buffer_head) { + prev_nominal_speed = plan_compute_profile_parameters(block, plan_compute_profile_nominal_speed(block), prev_nominal_speed); + block = block->next; + } + pl.previous_nominal_speed = prev_nominal_speed; // Update prev nominal speed for next incoming block. +} + +static inline float limit_acceleration_by_axis_maximum (float *unit_vec) +{ + uint_fast8_t idx = N_AXIS; + float limit_value = SOME_LARGE_VALUE; + + do { + if (unit_vec[--idx] != 0.0f) // Avoid divide by zero. + limit_value = min(limit_value, fabsf(settings.axis[idx].acceleration / unit_vec[idx])); + } while(idx); + + return limit_value; +} + +static inline float limit_max_rate_by_axis_maximum (float *unit_vec) +{ + uint_fast8_t idx = N_AXIS; + float limit_value = SOME_LARGE_VALUE; + + do { + if (unit_vec[--idx] != 0.0f) // Avoid divide by zero. + limit_value = min(limit_value, fabsf(settings.axis[idx].max_rate / unit_vec[idx])); + } while(idx); + + return limit_value; +} + + + +/* Add a new linear movement to the buffer. target[N_AXIS] is the signed, absolute target position + in millimeters. Feed rate specifies the speed of the motion. If feed rate is inverted, the feed + rate is taken to mean "frequency" and would complete the operation in 1/feed_rate minutes. + All position data passed to the planner must be in terms of machine position to keep the planner + independent of any coordinate system changes and offsets, which are handled by the g-code parser. + NOTE: Assumes buffer is available. Buffer checks are handled at a higher level by motion_control. + In other words, the buffer head is never equal to the buffer tail. Also the feed rate input value + is used in three ways: as a normal feed rate if invert_feed_rate is false, as inverse time if + invert_feed_rate is true, or as seek/rapids rate if the feed_rate value is negative (and + invert_feed_rate always false). + The system motion condition tells the planner to plan a motion in the always unused block buffer + head. It avoids changing the planner state and preserves the buffer to ensure subsequent gcode + motions are still planned correctly, while the stepper module only points to the block buffer head + to execute the special system motion. */ +bool plan_buffer_line (float *target, plan_line_data_t *pl_data) +{ + // Prepare and initialize new block. Copy relevant pl_data for block execution. + plan_block_t *block = block_buffer_head; + int32_t target_steps[N_AXIS], position_steps[N_AXIS], delta_steps; + uint_fast8_t idx; + float unit_vec[N_AXIS]; + +// plan_cleanup(block); + memset(block, 0, sizeof(plan_block_t) - 2 * sizeof(plan_block_t *)); // Zero all block values (except linked list pointers). + memcpy(&block->spindle, &pl_data->spindle, sizeof(spindle_t)); // Copy spindle data (RPM etc) + block->condition = pl_data->condition; + block->overrides = pl_data->overrides; + block->line_number = pl_data->line_number; + block->output_commands = pl_data->output_commands; + block->message = pl_data->message; + pl_data->message = NULL; + + // Copy position data based on type of motion being planned. + memcpy(position_steps, block->condition.system_motion ? sys.position : pl.position, sizeof(position_steps)); + + // Compute and store initial move distance data. + +#ifdef KINEMATICS_API + kinematics.plan_target_to_steps(target_steps, target); +#endif + + idx = N_AXIS; + do { + idx--; + // Calculate target position in absolute steps, number of steps for each axis, and determine max step events. + // Also, compute individual axes distance for move and prep unit vector calculations. + // NOTE: Computes true distance from converted step values. + +#ifndef KINEMATICS_API + target_steps[idx] = lroundf(target[idx] * settings.axis[idx].steps_per_mm); +#endif + delta_steps = target_steps[idx] - position_steps[idx]; + block->steps[idx] = labs(delta_steps); + block->step_event_count = max(block->step_event_count, block->steps[idx]); + unit_vec[idx] = (float)delta_steps / settings.axis[idx].steps_per_mm; // Store unit vector numerator + + // Set direction bits. Bit enabled always means direction is negative. + if (delta_steps < 0) + block->direction_bits.mask |= bit(idx); + + } while(idx); + + // Calculate RPMs to be used for Constant Surface Speed calculations + if(block->condition.is_rpm_pos_adjusted) { + float pos; + if((pos = (float)position_steps[block->spindle.css.axis] / settings.axis[block->spindle.css.axis].steps_per_mm - block->spindle.css.tool_offset) > 0.0f) { + block->spindle.rpm = block->spindle.css.surface_speed / (pos * (float)(2.0f * M_PI)); + if(block->spindle.rpm > block->spindle.css.max_rpm) + block->spindle.rpm = block->spindle.css.max_rpm; + } else + block->spindle.rpm = block->spindle.css.max_rpm; + if((pos = target[block->spindle.css.axis] - block->spindle.css.tool_offset) > 0.0f) { + block->spindle.css.target_rpm = block->spindle.css.surface_speed / (pos * (float)(2.0f * M_PI)); + if(block->spindle.css.target_rpm > block->spindle.css.max_rpm) + block->spindle.css.target_rpm = block->spindle.css.max_rpm; + } else + block->spindle.css.target_rpm = block->spindle.css.max_rpm; + } + + // Bail if this is a zero-length block. Highly unlikely to occur. + if (block->step_event_count == 0) + return false; + + pl_data->message = NULL; // Indicate message is already queued for display on execution + pl_data->output_commands = NULL; // Indicate commands are already queued for execution + + // Calculate the unit vector of the line move and the block maximum feed rate and acceleration scaled + // down such that no individual axes maximum values are exceeded with respect to the line direction. + // NOTE: This calculation assumes all axes are orthogonal (Cartesian) and works with ABC-axes, + // if they are also orthogonal/independent. Operates on the absolute value of the unit vector. + block->millimeters = convert_delta_vector_to_unit_vector(unit_vec); + block->acceleration = limit_acceleration_by_axis_maximum(unit_vec); + block->rapid_rate = limit_max_rate_by_axis_maximum(unit_vec); + + // Store programmed rate. + if (block->condition.rapid_motion) + block->programmed_rate = block->rapid_rate; + else { + block->programmed_rate = pl_data->feed_rate; + if (block->condition.inverse_time) + block->programmed_rate *= block->millimeters; + } + + // TODO: Need to check this method handling zero junction speeds when starting from rest. + if ((block_buffer_head == block_buffer_tail) || (block->condition.system_motion)) { + + // Initialize block entry speed as zero. Assume it will be starting from rest. Planner will correct this later. + // If system motion, the system motion block always is assumed to start from rest and end at a complete stop. + block->entry_speed_sqr = 0.0f; + block->max_junction_speed_sqr = 0.0f; // Starting from rest. Enforce start from zero velocity. + + } else { + + // Compute maximum allowable entry speed at junction by centripetal acceleration approximation. + // Let a circle be tangent to both previous and current path line segments, where the junction + // deviation is defined as the distance from the junction to the closest edge of the circle, + // colinear with the circle center. The circular segment joining the two paths represents the + // path of centripetal acceleration. Solve for max velocity based on max acceleration about the + // radius of the circle, defined indirectly by junction deviation. This may be also viewed as + // path width or max_jerk in the previous Grbl version. This approach does not actually deviate + // from path, but used as a robust way to compute cornering speeds, as it takes into account the + // nonlinearities of both the junction angle and junction velocity. + // + // NOTE: If the junction deviation value is finite, Grbl executes the motions in an exact path + // mode (G61). If the junction deviation value is zero, Grbl will execute the motion in an exact + // stop mode (G61.1) manner. In the future, if continuous mode (G64) is desired, the math here + // is exactly the same. Instead of motioning all the way to junction point, the machine will + // just follow the arc circle defined here. The Arduino doesn't have the CPU cycles to perform + // a continuous mode path, but ARM-based microcontrollers most certainly do. + // + // NOTE: The max junction speed is a fixed value, since machine acceleration limits cannot be + // changed dynamically during operation nor can the line move geometry. This must be kept in + // memory in the event of a feedrate override changing the nominal speeds of blocks, which can + // change the overall maximum entry speed conditions of all blocks. + + float junction_unit_vec[N_AXIS]; + float junction_cos_theta = 0.0f; + + idx = N_AXIS; + do { + idx--; + junction_cos_theta -= pl.previous_unit_vec[idx] * unit_vec[idx]; + junction_unit_vec[idx] = unit_vec[idx] - pl.previous_unit_vec[idx]; + } while(idx); + + // NOTE: Computed without any expensive trig, sin() or acos(), by trig half angle identity of cos(theta). + if (junction_cos_theta > 0.999999f) + // For a 0 degree acute junction, just set minimum junction speed. + block->max_junction_speed_sqr = MINIMUM_JUNCTION_SPEED * MINIMUM_JUNCTION_SPEED; + else if (junction_cos_theta < -0.999999f) { + // Junction is a straight line or 180 degrees. Junction speed is infinite. + block->max_junction_speed_sqr = SOME_LARGE_VALUE; + } else { + convert_delta_vector_to_unit_vector(junction_unit_vec); + float junction_acceleration = limit_acceleration_by_axis_maximum(junction_unit_vec); + float sin_theta_d2 = sqrtf(0.5f * (1.0f - junction_cos_theta)); // Trig half angle identity. Always positive. + block->max_junction_speed_sqr = max(MINIMUM_JUNCTION_SPEED * MINIMUM_JUNCTION_SPEED, + (junction_acceleration * settings.junction_deviation * sin_theta_d2) / (1.0f - sin_theta_d2)); + } + } + + // Block system motion from updating this data to ensure next g-code motion is computed correctly. + if (!block->condition.system_motion) { + + pl.previous_nominal_speed = plan_compute_profile_parameters(block, plan_compute_profile_nominal_speed(block), pl.previous_nominal_speed); + + if(!block->condition.backlash_motion) { + // Update previous path unit_vector and planner position. + memcpy(pl.previous_unit_vec, unit_vec, sizeof(unit_vec)); // pl.previous_unit_vec[] = unit_vec[] + memcpy(pl.position, target_steps, sizeof(target_steps)); // pl.position[] = target_steps[] + } + // New block is all set. Update buffer head and next buffer head indices. + block_buffer_head = next_buffer_head; + next_buffer_head = block_buffer_head->next; + + // Finish up by recalculating the plan with the new block. + planner_recalculate(); + } + + return true; +} + + +// Reset the planner position vectors. Called by the system abort/initialization routine. +void plan_sync_position () +{ + memcpy(pl.position, sys.position, sizeof(pl.position)); +} + + +// Returns the number of available blocks are in the planner buffer. +uint_fast16_t plan_get_block_buffer_available () +{ + return (uint_fast16_t)(block_buffer_head >= block_buffer_tail + ? ((BLOCK_BUFFER_SIZE - 1) - (block_buffer_head - block_buffer_tail)) + : ((block_buffer_tail - block_buffer_head) - 1)); +} + + +// Re-initialize buffer plan with a partially completed block, assumed to exist at the buffer tail. +// Called after a steppers have come to a complete stop for a feed hold and the cycle is stopped. +void plan_cycle_reinitialize () +{ + // Re-plan from a complete stop. Reset planner entry speeds and buffer planned pointer. + st_update_plan_block_parameters(); + block_buffer_planned = block_buffer_tail; + planner_recalculate(); +} + +// Set feed overrides +void plan_feed_override (uint_fast8_t feed_override, uint_fast8_t rapid_override) +{ + if(sys.override.control.feed_rate_disable) + return; + + feed_override = max(min(feed_override, MAX_FEED_RATE_OVERRIDE), MIN_FEED_RATE_OVERRIDE); + + if ((feed_override != sys.override.feed_rate) || (rapid_override != sys.override.rapid_rate)) { + sys.override.feed_rate = (uint8_t)feed_override; + sys.override.rapid_rate = (uint8_t)rapid_override; + sys.report.overrides = On; // Set to report change immediately + plan_update_velocity_profile_parameters(); + plan_cycle_reinitialize(); + } +} diff --git a/planner.h b/planner.h new file mode 100644 index 0000000..c30f9ec --- /dev/null +++ b/planner.h @@ -0,0 +1,152 @@ +/* + planner.h - buffers movement commands and manages the acceleration profile plan + + Part of grblHAL + + Copyright (c) 2019-2021 Terje Io + Copyright (c) 2011-2016 Sungeun K. Jeon for Gnea Research LLC + Copyright (c) 2009-2011 Simen Svale Skogsrud + + 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 . +*/ + +#ifndef _PLANNER_H_ +#define _PLANNER_H_ + +// The number of linear motions that can be in the plan at any give time +#ifndef BLOCK_BUFFER_SIZE + #define BLOCK_BUFFER_SIZE 36 +#endif + +typedef union { + uint32_t value; + struct { + uint16_t rapid_motion :1, + system_motion :1, + jog_motion :1, + backlash_motion :1, + no_feed_override :1, + inverse_time :1, + is_rpm_rate_adjusted :1, + is_rpm_pos_adjusted :1, + is_laser_ppi_mode :1, + unassigned :7; + spindle_state_t spindle; + coolant_state_t coolant; + }; +} planner_cond_t; + +// This struct stores a linear movement of a g-code block motion with its critical "nominal" values +// are as specified in the source g-code. +typedef struct plan_block { + // Fields used by the bresenham algorithm for tracing the line + // NOTE: Used by stepper algorithm to execute the block correctly. Do not alter these values. + uint32_t steps[N_AXIS]; // Step count along each axis + uint32_t step_event_count; // The maximum step axis count and number of steps required to complete this block. + axes_signals_t direction_bits; // The direction bit set for this block (refers to *_DIRECTION_BIT in config.h) + + // Block condition data to ensure correct execution depending on states and overrides. + planner_cond_t condition; // Block bitfield variable defining block run conditions. Copied from pl_line_data. + gc_override_flags_t overrides; // Block bitfield variable for overrides + int32_t line_number; // Block line number for real-time reporting. Copied from pl_line_data. + + // Fields used by the motion planner to manage acceleration. Some of these values may be updated + // by the stepper module during execution of special motion cases for replanning purposes. + float entry_speed_sqr; // The current planned entry speed at block junction in (mm/min)^2 + float max_entry_speed_sqr; // Maximum allowable entry speed based on the minimum of junction limit and + // neighboring nominal speeds with overrides in (mm/min)^2 + float acceleration; // Axis-limit adjusted line acceleration in (mm/min^2). Does not change. + float millimeters; // The remaining distance for this block to be executed in (mm). + // NOTE: This value may be altered by stepper algorithm during execution. + + // Stored rate limiting data used by planner when changes occur. + float max_junction_speed_sqr; // Junction entry speed limit based on direction vectors in (mm/min)^2 + float rapid_rate; // Axis-limit adjusted maximum rate for this block direction in (mm/min) + float programmed_rate; // Programmed rate of this block (mm/min). + + // Stored spindle speed data used by spindle overrides and resuming methods. + spindle_t spindle; // Block spindle speed. Copied from pl_line_data. + + char *message; // Message to be displayed when block is executed. + output_command_t *output_commands; + struct plan_block *prev, *next; // Linked list pointers, DO NOT MOVE - these MUST be the last elements in the struct! +} plan_block_t; + + +// Planner data prototype. Must be used when passing new motions to the planner. +typedef struct { + float feed_rate; // Desired feed rate for line motion. Value is ignored, if rapid motion. + spindle_t spindle; // Desired spindle speed through line motion. + planner_cond_t condition; // Bitfield variable to indicate planner conditions. See defines above. + gc_override_flags_t overrides; // Block bitfield variable for overrides + int32_t line_number; // Desired line number to report when executing. +// void *parameters; // TODO: pointer to extra parameters, for canned cycles and threading? + char *message; // Message to be displayed when block is executed. + output_command_t *output_commands; +} plan_line_data_t; + + +// Define planner variables +typedef struct { + int32_t position[N_AXIS]; // The planner position of the tool in absolute steps. Kept separate + // from g-code position for movements requiring multiple line motions, + // i.e. arcs, canned cycles, and backlash compensation. + float previous_unit_vec[N_AXIS]; // Unit vector of previous path line segment + float previous_nominal_speed; // Nominal speed of previous path line segment +} planner_t; + +// Initialize and reset the motion plan subsystem +void plan_reset(); // Reset all +//void plan_reset_buffer(); // Reset buffer only. + +// Add a new linear movement to the buffer. target[N_AXIS] is the signed, absolute target position +// in millimeters. Feed rate specifies the speed of the motion. If feed rate is inverted, the feed +// rate is taken to mean "frequency" and would complete the operation in 1/feed_rate minutes. +bool plan_buffer_line(float *target, plan_line_data_t *pl_data); + +// Called when the current block is no longer needed. Discards the block and makes the memory +// availible for new blocks. +void plan_discard_current_block(); + +// Gets the planner block for the special system motion cases. (Parking/Homing) +plan_block_t *plan_get_system_motion_block(); + +// Gets the current block. Returns NULL if buffer empty +plan_block_t *plan_get_current_block(); + +// Called by step segment buffer when computing executing block velocity profile. +float plan_get_exec_block_exit_speed_sqr(); + +// Called by main program during planner calculations and step segment buffer during initialization. +float plan_compute_profile_nominal_speed(plan_block_t *block); + +// Re-calculates buffered motions profile parameters upon a motion-based override change. +void plan_update_velocity_profile_parameters(); + +// Reset the planner position vector (in steps) +void plan_sync_position(); + +// Reinitialize plan with a partially completed block +void plan_cycle_reinitialize(); + +// Returns the number of available blocks in the planner buffer. +uint_fast16_t plan_get_block_buffer_available(); + +// Returns the status of the block ring buffer. True, if buffer is full. +bool plan_check_full_buffer(); + +void plan_get_planner_mpos(float *target); +void plan_feed_override (uint_fast8_t feed_override, uint_fast8_t rapid_override); + +#endif diff --git a/platform.h b/platform.h new file mode 100644 index 0000000..9f04a8f --- /dev/null +++ b/platform.h @@ -0,0 +1,32 @@ +/* + platform.h - platform specific definitions + + Part of grblHAL + + Copyright (c) 2021 Terje Io + + Grbl is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + Grbl is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with Grbl. If not, see . +*/ + +#pragma once + +#if defined(STM32F103xB) || defined(STM32F401xC) || defined(STM32F407xx) || defined(STM32F411xE) || defined(STM32F446xx) +#define STM32_PLATFORM +#endif + +#if defined(STM32_PLATFORM) || defined(__LPC17XX__) || defined(__IMXRT1062__) +#define UINT32FMT "lu" +#else +#define UINT32FMT "u" +#endif diff --git a/plugins.h b/plugins.h new file mode 100644 index 0000000..9bee16d --- /dev/null +++ b/plugins.h @@ -0,0 +1,190 @@ +/* + plugins.h - An embedded CNC Controller with rs274/ngc (g-code) support + + Some data structures and function declarations for plugins that require driver code + + These are NOT referenced in the core grbl code + + Part of grblHAL + + Copyright (c) 2020-2021 Terje Io + + Grbl is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + Grbl is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with Grbl. If not, see . +*/ + +#ifndef _PLUGINS_H_ +#define _PLUGINS_H_ + +#include +#include + +#include "nvs.h" + +// Jogging + +typedef struct { + float fast_speed; + float slow_speed; + float step_speed; + float fast_distance; + float slow_distance; + float step_distance; +} jog_settings_t; + +// Networking + +typedef enum { + IpMode_Static = 0, + IpMode_DHCP, + IpMode_AutoIP +} ip_mode_t; + +typedef union { + uint8_t mask; + struct { + uint8_t telnet :1, + websocket :1, + http :1, + dns :1, + mdns :1, + ssdp :1, + webdav :1, + unassigned :1; + }; +} network_services_t; + +typedef char ssid_t[65]; +typedef char password_t[33]; +typedef char hostname_t[33]; + +typedef struct { + char ip[16]; + char gateway[16]; + char mask[16]; + hostname_t hostname; + uint16_t telnet_port; + uint16_t websocket_port; + uint16_t http_port; + ip_mode_t ip_mode; + network_services_t services; +} network_settings_t; + +typedef enum { + WiFiMode_NULL = 0, + WiFiMode_STA, + WiFiMode_AP, + WiFiMode_APSTA +} grbl_wifi_mode_t; + +typedef struct { + ssid_t ssid; + password_t password; + char country[4]; + uint8_t channel; + network_settings_t network; +} wifi_ap_settings_t; + +typedef struct { + ssid_t ssid; + password_t password; + network_settings_t network; +} wifi_sta_settings_t; + +typedef struct { + char device_name[33]; + char service_name[33]; +} bluetooth_settings_t; + +typedef struct { + uint32_t baud_rate; + uint32_t rx_timeout; +} modbus_settings_t; + +// Quadrature encoder interface + +typedef enum { + Encoder_Universal = 0, + Encoder_FeedRate, + Encoder_RapidRate, + Encoder_Spindle_RPM, + Encoder_MPG, + Encoder_MPG_X, + Encoder_MPG_Y, + Encoder_MPG_Z, + Encoder_MPG_A, + Encoder_MPG_B, + Encoder_MPG_C, + Encoder_Spindle_Position +} encoder_mode_t; + +typedef enum { + Setting_EncoderMode = 0, + Setting_EncoderCPR = 1, // Count Per Revolution + Setting_EncoderCPD = 2, // Count Per Detent + Setting_EncoderDblClickWindow = 3 // ms +} encoder_setting_id_t; + +typedef union { + uint8_t events; + struct { + uint8_t position_changed :1, + click :1, + dbl_click :1, + long_click :1, + index_pulse :1; + }; +} encoder_event_t; + +typedef union { + uint8_t flags; + uint8_t value; + struct { + uint8_t single_count_per_detent :1; + }; +} encoder_flags_t; + +typedef struct { + encoder_mode_t mode; + uint32_t cpr; // count per revolution + uint32_t cpd; // count per detent + uint32_t dbl_click_window; // ms + encoder_flags_t flags; +} encoder_settings_t; + +typedef struct { + encoder_mode_t mode; + uint_fast8_t id; + uint_fast8_t axis; // axis index for MPG encoders, 0xFF for others + int32_t position; + uint32_t velocity; + encoder_event_t event; + encoder_settings_t *settings; +} encoder_t; + +// EEPROM/FRAM interface + +typedef struct { + uint8_t address; + uint8_t word_addr_bytes; + uint16_t word_addr; + volatile uint_fast16_t count; + bool add_checksum; + uint8_t checksum; + uint8_t *data; +} nvs_transfer_t; + +extern nvs_transfer_result_t i2c_nvs_transfer (nvs_transfer_t *i2c, bool read); +extern void my_plugin_init (void) __attribute__((weak)); + +#endif diff --git a/probe.h b/probe.h new file mode 100644 index 0000000..16dba19 --- /dev/null +++ b/probe.h @@ -0,0 +1,43 @@ +/* + probe.h - An embedded CNC Controller with rs274/ngc (g-code) support + + Part of grblHAL + + Copyright (c) 2020 Terje Io + + Grbl is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + Grbl is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with Grbl. If not, see . +*/ + +#ifndef _PROBE_H_ +#define _PROBE_H_ + +// Values that define the probing state machine. + +typedef enum { + Probing_Off = 0, + Probing_Active +} probing_state_t; + +typedef union { + uint8_t value; + struct { + uint8_t triggered :1, + connected :1, + inverted :1, // For driver use + is_probing :1, // For driver use + unassigned :4; + }; +} probe_state_t; + +#endif diff --git a/protocol.c b/protocol.c new file mode 100644 index 0000000..bf2ceeb --- /dev/null +++ b/protocol.c @@ -0,0 +1,930 @@ +/* + protocol.c - controls Grbl execution protocol and procedures + + Part of grblHAL + + Copyright (c) 2017-2021 Terje Io + Copyright (c) 2011-2016 Sungeun K. Jeon for Gnea Research LLC + Copyright (c) 2009-2011 Simen Svale Skogsrud + + 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 . +*/ + +#include +#include + +#include "hal.h" +#include "nuts_bolts.h" +#include "nvs_buffer.h" +#include "override.h" +#include "state_machine.h" +#include "motion_control.h" +#include "sleep.h" +#include "protocol.h" +#include "limits.h" + +#ifndef RT_QUEUE_SIZE +#define RT_QUEUE_SIZE 8 // must be a power of 2 +#endif + +// Define line flags. Includes comment type tracking and line overflow detection. +typedef union { + uint8_t value; + struct { + uint8_t overflow :1, + comment_parentheses :1, + comment_semicolon :1, + line_is_comment :1, + block_delete :1, + unassigned :3; + }; +} line_flags_t; + +typedef struct { + char *message; + uint_fast8_t idx; + uint_fast8_t tracker; + bool show; +} user_message_t; + +typedef struct { + volatile uint_fast8_t head; + volatile uint_fast8_t tail; + on_execute_realtime_ptr fn[RT_QUEUE_SIZE]; +} realtime_queue_t; + +static uint_fast16_t char_counter = 0; +static char line[LINE_BUFFER_SIZE]; // Line to be executed. Zero-terminated. +static char xcommand[LINE_BUFFER_SIZE]; +static bool keep_rt_commands = false; +static user_message_t user_message = {NULL, 0, 0, false}; +static const char *msg = "(MSG,"; +static realtime_queue_t realtime_queue = {0}; + +static void protocol_exec_rt_suspend (); +static void protocol_execute_rt_commands (void); + +// add gcode to execute not originating from normal input stream +bool protocol_enqueue_gcode (char *gcode) +{ + bool ok = xcommand[0] == '\0' && + (state_get() == STATE_IDLE || (state_get() & (STATE_JOG|STATE_TOOL_CHANGE))) && + bit_isfalse(sys.rt_exec_state, EXEC_MOTION_CANCEL); + + if(ok && gc_state.file_run) + ok = gc_state.modal.program_flow != ProgramFlow_Running || strncmp((char *)gcode, "$J=", 3); + + if(ok) + strcpy(xcommand, gcode); + + return ok; +} + +/* + GRBL PRIMARY LOOP: +*/ +bool protocol_main_loop (void) +{ + if(sys.alarm == Alarm_SelftestFailed) { + system_raise_alarm(Alarm_SelftestFailed); + } else if (hal.control.get_state().e_stop) { + // Check for e-stop active. Blocks everything until cleared. + system_raise_alarm(Alarm_EStop); + grbl.report.feedback_message(Message_EStop); + } else if(hal.control.get_state().motor_fault) { + // Check for motor fault active. Blocks everything until cleared. + system_raise_alarm(Alarm_MotorFault); + grbl.report.feedback_message(Message_MotorFault); + } else if (limits_homing_required()) { + // Check for power-up and set system alarm if homing is enabled to force homing cycle + // by setting Grbl's alarm state. Alarm locks out all g-code commands, including the + // startup scripts, but allows access to settings and internal commands. + // Only a successful homing cycle '$H' will disable the alarm. + // NOTE: The startup script will run after successful completion of the homing cycle. Prevents motion startup + // blocks from crashing into things uncontrollably. Very bad. + system_raise_alarm(Alarm_HomingRequried); + grbl.report.feedback_message(Message_HomingCycleRequired); + } else if (settings.limits.flags.hard_enabled && settings.limits.flags.check_at_init && limit_signals_merge(hal.limits.get_state()).value) { + if(sys.alarm == Alarm_LimitsEngaged && hal.control.get_state().limits_override) + state_set(STATE_IDLE); // Clear alarm state to enable limit switch pulloff. + else { + // Check that no limit switches are engaged to make sure everything is good to go. + system_raise_alarm(Alarm_LimitsEngaged); + grbl.report.feedback_message(Message_CheckLimits); + } + } else if(sys.cold_start && (settings.flags.force_initialization_alarm || hal.control.get_state().reset)) { + state_set(STATE_ALARM); // Ensure alarm state is set. + grbl.report.feedback_message(Message_AlarmLock); + } else if (state_get() & (STATE_ALARM|STATE_SLEEP)) { + // Check for and report alarm state after a reset, error, or an initial power up. + // NOTE: Sleep mode disables the stepper drivers and position can't be guaranteed. + // Re-initialize the sleep state as an ALARM mode to ensure user homes or acknowledges. + if(sys.alarm == Alarm_HomingRequried) + sys.alarm = Alarm_None; // Clear Alarm_HomingRequried as the lock has been overridden by a soft reset. + state_set(STATE_ALARM); // Ensure alarm state is set. + grbl.report.feedback_message(Message_AlarmLock); + } else { + state_set(STATE_IDLE); +#ifdef ENABLE_SAFETY_DOOR_INPUT_PIN + // Check if the safety door is open. + if (!settings.flags.safety_door_ignore_when_idle && hal.control.get_state().safety_door_ajar) { + system_set_exec_state_flag(EXEC_SAFETY_DOOR); + protocol_execute_realtime(); // Enter safety door mode. Should return as IDLE state. + } +#endif + // All systems go! + system_execute_startup(); // Execute startup script. + } + + // Ensure spindle and coolant is switched off on a cold start + if(sys.cold_start) { + hal.spindle.set_state((spindle_state_t){0}, 0.0f); + hal.coolant.set_state((coolant_state_t){0}); + if(realtime_queue.head != realtime_queue.tail) + system_set_exec_state_flag(EXEC_RT_COMMAND); // execute any boot up commands + } else + memset(&realtime_queue, 0, sizeof(realtime_queue_t)); + + // --------------------------------------------------------------------------------- + // Primary loop! Upon a system abort, this exits back to main() to reset the system. + // This is also where Grbl idles while waiting for something to do. + // --------------------------------------------------------------------------------- + + int16_t c; + char eol = '\0'; + line_flags_t line_flags = {0}; + bool nocaps = false; + + xcommand[0] = '\0'; + user_message.show = keep_rt_commands = false; + + while(true) { + + // Process one line of incoming stream data, as the data becomes available. Performs an + // initial filtering by removing spaces and comments and capitalizing all letters. + while((c = hal.stream.read()) != SERIAL_NO_DATA) { + + if(c == ASCII_CAN) { + + eol = xcommand[0] = '\0'; + keep_rt_commands = nocaps = user_message.show = false; + char_counter = line_flags.value = 0; + gc_state.last_error = Status_OK; + + if (state_get() == STATE_JOG) // Block all other states from invoking motion cancel. + system_set_exec_state_flag(EXEC_MOTION_CANCEL); + + } else if ((c == '\n') || (c == '\r')) { // End of line reached + + // Check for possible secondary end of line character, do not process as empty line + // if part of crlf (or lfcr pair) as this produces a possibly unwanted double response + if(char_counter == 0 && eol && eol != c) { + eol = '\0'; + continue; + } else + eol = (char)c; + + if(!protocol_execute_realtime()) // Runtime command check point. + return !sys.flags.exit; // Bail to calling function upon system abort + + line[char_counter] = '\0'; // Set string termination character. + + #ifdef REPORT_ECHO_LINE_RECEIVED + report_echo_line_received(line); + #endif + + // Direct and execute one line of formatted input, and report status of execution. + if (line_flags.overflow) // Report line overflow error. + gc_state.last_error = Status_Overflow; + else if ((line[0] == '\0' || char_counter == 0) && !user_message.show && !line_flags.line_is_comment) // Empty or comment line. For syncing purposes. + gc_state.last_error = Status_OK; + else if (line[0] == '$') {// Grbl '$' system command + if((gc_state.last_error = system_execute_line(line)) == Status_LimitsEngaged) { + system_raise_alarm(Alarm_LimitsEngaged); + grbl.report.feedback_message(Message_CheckLimits); + } + } else if (line[0] == '[' && grbl.on_user_command) + gc_state.last_error = grbl.on_user_command(line); + else if (state_get() & (STATE_ALARM|STATE_ESTOP|STATE_JOG)) // Everything else is gcode. Block if in alarm, eStop or jog mode. + gc_state.last_error = Status_SystemGClock; +#if COMPATIBILITY_LEVEL == 0 + else if(gc_state.last_error == Status_OK || gc_state.last_error == Status_GcodeToolChangePending) { // Parse and execute g-code block. +#else + else { // Parse and execute g-code block. + +#endif + gc_state.last_error = gc_execute_block(line, user_message.show ? user_message.message : NULL); + } + + // Add a short delay for each block processed in Check Mode to + // avoid overwhelming the sender with fast reply messages. + // This is likely to happen when streaming is done via a protocol where + // the speed is not limited to 115200 baud. An example is native USB streaming. +#if CHECK_MODE_DELAY + if(state_get() == STATE_CHECK_MODE) + hal.delay_ms(CHECK_MODE_DELAY, NULL); +#endif + + grbl.report.status_message(gc_state.last_error); + + // Reset tracking data for next line. + keep_rt_commands = nocaps = user_message.show = false; + char_counter = line_flags.value = 0; + + } else if (c <= (nocaps ? ' ' - 1 : ' ') || line_flags.value) { + // Throw away all whitepace, control characters, comment characters and overflow characters. + if(c >= ' ' && line_flags.comment_parentheses) { + if(user_message.tracker == 5) + user_message.message[user_message.idx++] = c == ')' ? '\0' : c; + else if(user_message.tracker > 0 && CAPS(c) == msg[user_message.tracker]) + user_message.tracker++; + else + user_message.tracker = 0; + if (c == ')') { + // End of '()' comment. Resume line. + line_flags.comment_parentheses = Off; + keep_rt_commands = false; + user_message.show = user_message.show || user_message.tracker == 5; + } + } + } else { + switch(c) { + + case '/': + if(char_counter == 0) + line_flags.block_delete = sys.flags.block_delete_enabled; + break; + + case '$': + case '[': + // Do not uppercase system or user commands - will destroy passwords etc... + if(char_counter == 0) + nocaps = keep_rt_commands = true; + break; + + case '(': + if(char_counter == 0) + line_flags.line_is_comment = On; + if(!keep_rt_commands) { + // Enable comments flag and ignore all characters until ')' or EOL unless it is a message. + // NOTE: This doesn't follow the NIST definition exactly, but is good enough for now. + // In the future, we could simply remove the items within the comments, but retain the + // comment control characters, so that the g-code parser can error-check it. + if((line_flags.comment_parentheses = !line_flags.comment_semicolon)) { + if(!hal.driver_cap.no_gcode_message_handling) { + if(user_message.message == NULL) + user_message.message = malloc(LINE_BUFFER_SIZE); + if(user_message.message) { + user_message.idx = 0; + user_message.tracker = 1; + } + } + keep_rt_commands = true; + } + } + break; + + case ';': + if(char_counter == 0) + line_flags.line_is_comment = On; + // NOTE: ';' comment to EOL is a LinuxCNC definition. Not NIST. + if(!keep_rt_commands) { + if((line_flags.comment_semicolon = !line_flags.comment_parentheses)) + keep_rt_commands = true; + } + break; + } + if (line_flags.value == 0 && !(line_flags.overflow = char_counter >= (LINE_BUFFER_SIZE - 1))) + line[char_counter++] = nocaps ? c : CAPS(c); + } + } + + // Handle extra command (internal stream) + if(xcommand[0] != '\0') { + + if (xcommand[0] == '$') // Grbl '$' system command + system_execute_line(xcommand); + else if (state_get() & (STATE_ALARM|STATE_ESTOP|STATE_JOG)) // Everything else is gcode. Block if in alarm, eStop or jog state. + grbl.report.status_message(Status_SystemGClock); + else // Parse and execute g-code block. + gc_execute_block(xcommand, NULL); + + xcommand[0] = '\0'; + } + + // If there are no more characters in the input stream buffer to be processed and executed, + // this indicates that g-code streaming has either filled the planner buffer or has + // completed. In either case, auto-cycle start, if enabled, any queued moves. + protocol_auto_cycle_start(); + + if(!protocol_execute_realtime() && sys.abort) // Runtime command check point. + return !sys.flags.exit; // Bail to main() program loop to reset system. + + sys.cancel = false; + + // Check for sleep conditions and execute auto-park, if timeout duration elapses. + if(settings.flags.sleep_enable) + sleep_check(); + } +} + + +// Block until all buffered steps are executed or in a cycle state. Works with feed hold +// during a synchronize call, if it should happen. Also, waits for clean cycle end. +bool protocol_buffer_synchronize (void) +{ + bool ok = true; + // If system is queued, ensure cycle resumes if the auto start flag is present. + protocol_auto_cycle_start(); + while ((ok = protocol_execute_realtime()) && (plan_get_current_block() || state_get() == STATE_CYCLE)); + + return ok; +} + + +// Auto-cycle start triggers when there is a motion ready to execute and if the main program is not +// actively parsing commands. +// NOTE: This function is called from the main loop, buffer sync, and mc_line() only and executes +// when one of these conditions exist respectively: There are no more blocks sent (i.e. streaming +// is finished, single commands), a command that needs to wait for the motions in the buffer to +// execute calls a buffer sync, or the planner buffer is full and ready to go. +void protocol_auto_cycle_start (void) +{ + if (plan_get_current_block() != NULL) // Check if there are any blocks in the buffer. + system_set_exec_state_flag(EXEC_CYCLE_START); // If so, execute them! +} + + +// This function is the general interface to Grbl's real-time command execution system. It is called +// from various check points in the main program, primarily where there may be a while loop waiting +// for a buffer to clear space or any point where the execution time from the last check point may +// be more than a fraction of a second. This is a way to execute realtime commands asynchronously +// (aka multitasking) with grbl's g-code parsing and planning functions. This function also serves +// as an interface for the interrupts to set the system realtime flags, where only the main program +// handles them, removing the need to define more computationally-expensive volatile variables. This +// also provides a controlled way to execute certain tasks without having two or more instances of +// the same task, such as the planner recalculating the buffer upon a feedhold or overrides. +// NOTE: The sys_rt_exec_state variable flags are set by any process, step or input stream events, pinouts, +// limit switches, or the main program. +// Returns false if aborted +bool protocol_execute_realtime (void) +{ + if(protocol_exec_rt_system()) { + + if (sys.suspend) + protocol_exec_rt_suspend(); + + #ifdef BUFFER_NVSDATA + if((state_get() == STATE_IDLE || (state_get() & (STATE_ALARM|STATE_ESTOP))) && settings_dirty.is_dirty && !gc_state.file_run) + nvs_buffer_sync_physical(); + #endif + } + + return !ABORTED; +} + +// Executes run-time commands, when required. This function primarily operates as Grbl's state +// machine and controls the various real-time features Grbl has to offer. +// NOTE: Do not alter this unless you know exactly what you are doing! +bool protocol_exec_rt_system (void) +{ + uint_fast16_t rt_exec; + bool killed = false; + + if (sys.rt_exec_alarm && (rt_exec = system_clear_exec_alarm())) { // Enter only if any bit flag is true + + // System alarm. Everything has shutdown by something that has gone severely wrong. Report + // the source of the error to the user. If critical, Grbl disables by entering an infinite + // loop until system reset/abort. + system_raise_alarm((alarm_code_t)rt_exec); + + if(sys.rt_exec_state & EXEC_RESET) { + // Kill spindle and coolant. + killed = true; + hal.spindle.set_state((spindle_state_t){0}, 0.0f); + hal.coolant.set_state((coolant_state_t){0}); + // Tell driver/plugins about reset. + hal.driver_reset(); + } + + // Halt everything upon a critical event flag. Currently hard and soft limits flag this. + if ((alarm_code_t)rt_exec == Alarm_HardLimit || + (alarm_code_t)rt_exec == Alarm_SoftLimit || + (alarm_code_t)rt_exec == Alarm_EStop || + (alarm_code_t)rt_exec == Alarm_MotorFault) { + system_set_exec_alarm(rt_exec); + switch((alarm_code_t)rt_exec) { + + case Alarm_EStop: + grbl.report.feedback_message(Message_EStop); + break; + + case Alarm_MotorFault: + grbl.report.feedback_message(Message_MotorFault); + break; + + default: + grbl.report.feedback_message(Message_CriticalEvent); + break; + } + system_clear_exec_state_flag(EXEC_RESET); // Disable any existing reset + while (bit_isfalse(sys.rt_exec_state, EXEC_RESET)) { + // Block everything, except reset and status reports, until user issues reset or power + // cycles. Hard limits typically occur while unattended or not paying attention. Gives + // the user and a GUI time to do what is needed before resetting, like killing the + // incoming stream. The same could be said about soft limits. While the position is not + // lost, continued streaming could cause a serious crash if by chance it gets executed. + if(bit_istrue(sys.rt_exec_state, EXEC_STATUS_REPORT)) { + system_clear_exec_state_flag(EXEC_STATUS_REPORT); + report_realtime_status(); + } + + grbl.on_execute_realtime(STATE_ESTOP); + } + system_clear_exec_alarm(); // Clear alarm + } + } + + if (sys.rt_exec_state && (rt_exec = system_clear_exec_states())) { // Get and clear volatile sys.rt_exec_state atomically. + + // Execute system abort. + if (rt_exec & EXEC_RESET) { + if(!killed) { + // Kill spindle and coolant. + hal.spindle.set_state((spindle_state_t){0}, 0.0f); + hal.coolant.set_state((coolant_state_t){0}); + // Tell driver/plugins about reset. + hal.driver_reset(); + } + + // Only place sys.abort is set true, when E-stop is not asserted. + if(!(sys.abort = !hal.control.get_state().e_stop)) { + hal.stream.reset_read_buffer(); + system_raise_alarm(Alarm_EStop); + grbl.report.feedback_message(Message_EStop); + } else if(hal.control.get_state().motor_fault) { + sys.abort = false; + hal.stream.reset_read_buffer(); + system_raise_alarm(Alarm_MotorFault); + grbl.report.feedback_message(Message_MotorFault); + } + + return !sys.abort; // Nothing else to do but exit. + } + + if(rt_exec & EXEC_STOP) { // Experimental for now, must be verified. Do NOT move to interrupt context! + + sys.cancel = true; + sys.step_control.flags = 0; + sys.flags.feed_hold_pending = Off; + sys.flags.delay_overrides = Off; + if(sys.override.control.sync) + sys.override.control = gc_state.modal.override_ctrl; + + gc_state.tool_change = false; + gc_state.modal.coolant.value = 0; + gc_state.modal.spindle.value = 0; + gc_state.spindle.rpm = sys.spindle_rpm = 0.0f; + gc_state.modal.spindle_rpm_mode = SpindleSpeedMode_RPM; + + // Kill spindle and coolant. TODO: Check Mach3 behaviour? + hal.spindle.set_state(gc_state.modal.spindle, 0.0f); + hal.coolant.set_state(gc_state.modal.coolant); + sys.report.spindle = sys.report.coolant = On; // Set to report change immediately + // Tell driver/plugins about reset. + hal.driver_reset(); + + if(hal.stream.suspend_read && hal.stream.suspend_read(false)) + hal.stream.cancel_read_buffer(); // flush pending blocks (after M6) + + gc_init(); + plan_reset(); +/* if(sys.alarm_pending == Alarm_ProbeProtect) { + st_go_idle(); + system_set_exec_alarm(sys.alarm_pending); + sys.alarm_pending = Alarm_None; + } else*/ + st_reset(); + sync_position(); + flush_override_buffers(); + if(!((state_get() == STATE_ALARM) && (sys.alarm == Alarm_LimitsEngaged || sys.alarm == Alarm_HomingRequried))) + state_set(STATE_IDLE); + } + + // Execute and print status to output stream + if (rt_exec & EXEC_STATUS_REPORT) + report_realtime_status(); + + if(rt_exec & EXEC_GCODE_REPORT) + report_gcode_modes(); + + if(rt_exec & EXEC_TLO_REPORT) + report_tool_offsets(); + + // Execute and print PID log to output stream + if (rt_exec & EXEC_PID_REPORT) + report_pid_log(); + + if(rt_exec & EXEC_RT_COMMAND) + protocol_execute_rt_commands(); + + rt_exec &= ~(EXEC_STOP|EXEC_STATUS_REPORT|EXEC_GCODE_REPORT|EXEC_PID_REPORT|EXEC_TLO_REPORT|EXEC_RT_COMMAND); // clear requests already processed + + if(sys.flags.feed_hold_pending) { + if(rt_exec & EXEC_CYCLE_START) + sys.flags.feed_hold_pending = Off; + else if(!sys.override.control.feed_hold_disable) + rt_exec |= EXEC_FEED_HOLD; + } + + // Let state machine handle any remaining requests + if(rt_exec) + state_update(rt_exec); + } + + grbl.on_execute_realtime(state_get()); + + if(!sys.flags.delay_overrides) { + + // Execute overrides. + + if((rt_exec = get_feed_override())) { + + uint_fast8_t new_f_override = sys.override.feed_rate, new_r_override = sys.override.rapid_rate; + + do { + + switch(rt_exec) { + + case CMD_OVERRIDE_FEED_RESET: + new_f_override = DEFAULT_FEED_OVERRIDE; + break; + + case CMD_OVERRIDE_FEED_COARSE_PLUS: + new_f_override += FEED_OVERRIDE_COARSE_INCREMENT; + break; + + case CMD_OVERRIDE_FEED_COARSE_MINUS: + new_f_override -= FEED_OVERRIDE_COARSE_INCREMENT; + break; + + case CMD_OVERRIDE_FEED_FINE_PLUS: + new_f_override += FEED_OVERRIDE_FINE_INCREMENT; + break; + + case CMD_OVERRIDE_FEED_FINE_MINUS: + new_f_override -= FEED_OVERRIDE_FINE_INCREMENT; + break; + + case CMD_OVERRIDE_RAPID_RESET: + new_r_override = DEFAULT_RAPID_OVERRIDE; + break; + + case CMD_OVERRIDE_RAPID_MEDIUM: + new_r_override = RAPID_OVERRIDE_MEDIUM; + break; + + case CMD_OVERRIDE_RAPID_LOW: + new_r_override = RAPID_OVERRIDE_LOW; + break; + } + + } while((rt_exec = get_feed_override())); + + plan_feed_override(new_f_override, new_r_override); + } + + if((rt_exec = get_accessory_override())) { + + bool spindle_stop = false; + uint_fast8_t last_s_override = sys.override.spindle_rpm; + coolant_state_t coolant_state = gc_state.modal.coolant; + + do { + + switch(rt_exec) { + + case CMD_OVERRIDE_SPINDLE_RESET: + last_s_override = DEFAULT_SPINDLE_RPM_OVERRIDE; + break; + + case CMD_OVERRIDE_SPINDLE_COARSE_PLUS: + last_s_override += SPINDLE_OVERRIDE_COARSE_INCREMENT; + break; + + case CMD_OVERRIDE_SPINDLE_COARSE_MINUS: + last_s_override -= SPINDLE_OVERRIDE_COARSE_INCREMENT; + break; + + case CMD_OVERRIDE_SPINDLE_FINE_PLUS: + last_s_override += SPINDLE_OVERRIDE_FINE_INCREMENT; + break; + + case CMD_OVERRIDE_SPINDLE_FINE_MINUS: + last_s_override -= SPINDLE_OVERRIDE_FINE_INCREMENT; + break; + + case CMD_OVERRIDE_SPINDLE_STOP: + spindle_stop = !spindle_stop; + break; + + case CMD_OVERRIDE_COOLANT_MIST_TOGGLE: + if (hal.driver_cap.mist_control && ((state_get() == STATE_IDLE) || (state_get() & (STATE_CYCLE | STATE_HOLD)))) { + coolant_state.mist = !coolant_state.mist; + } + break; + + case CMD_OVERRIDE_COOLANT_FLOOD_TOGGLE: + if ((state_get() == STATE_IDLE) || (state_get() & (STATE_CYCLE | STATE_HOLD))) { + coolant_state.flood = !coolant_state.flood; + } + break; + + default: + if(grbl.on_unknown_accessory_override) + grbl.on_unknown_accessory_override(rt_exec); + break; + } + + } while((rt_exec = get_accessory_override())); + + spindle_set_override(last_s_override); + + // NOTE: Since coolant state always performs a planner sync whenever it changes, the current + // run state can be determined by checking the parser state. + if(coolant_state.value != gc_state.modal.coolant.value) { + coolant_set_state(coolant_state); // Report flag set in coolant_set_state(). + gc_state.modal.coolant = coolant_state; + } + + if (spindle_stop && state_get() == STATE_HOLD && gc_state.modal.spindle.on) { + // Spindle stop override allowed only while in HOLD state. + // NOTE: Report flag is set in spindle_set_state() when spindle stop is executed. + if (!sys.override.spindle_stop.value) + sys.override.spindle_stop.initiate = On; + else if (sys.override.spindle_stop.enabled) + sys.override.spindle_stop.restore = On; + } + } + } // End execute overrides. + + // Reload step segment buffer + if (state_get() & (STATE_CYCLE | STATE_HOLD | STATE_SAFETY_DOOR | STATE_HOMING | STATE_SLEEP| STATE_JOG)) + st_prep_buffer(); + + return !ABORTED; +} + +// Handles Grbl system suspend procedures, such as feed hold, safety door, and parking motion. +// The system will enter this loop, create local variables for suspend tasks, and return to +// whatever function that invoked the suspend, such that Grbl resumes normal operation. +// This function is written in a way to promote custom parking motions. Simply use this as a +// template. +static void protocol_exec_rt_suspend (void) +{ + while (sys.suspend) { + + if (sys.abort) + return; + + // Handle spindle overrides during suspend + state_suspend_manager(); + + // If door closed keep issuing cycle start requests until resumed + if(state_get() == STATE_SAFETY_DOOR && !hal.control.get_state().safety_door_ajar) + system_set_exec_state_flag(EXEC_CYCLE_START); + + // Check for sleep conditions and execute auto-park, if timeout duration elapses. + // Sleep is valid for both hold and door states, if the spindle or coolant are on or + // set to be re-enabled. + if(settings.flags.sleep_enable) + sleep_check(); + + protocol_exec_rt_system(); + } +} + +// Pick off (drop) real-time command characters from input stream. +// These characters are not passed into the main buffer, +// but rather sets system state flag bits for later execution by protocol_exec_rt_system(). +// Called from input stream interrupt handler. +ISR_CODE bool protocol_enqueue_realtime_command (char c) +{ + static bool esc = false; + + bool drop = false; + + // 1. Process characters in the ranges 0x - 1x and 8x-Ax + // Characters with functions assigned are always acted upon even when the input stream + // is redirected to a non-interactive stream such as from a SD card. + + switch ((unsigned char)c) { + + case '\n': + case '\r': + break; + + case CMD_STOP: + system_set_exec_state_flag(EXEC_STOP); + char_counter = 0; + hal.stream.cancel_read_buffer(); + drop = true; + break; + + case CMD_RESET: // Call motion control reset routine. + if(!hal.control.get_state().e_stop) + mc_reset(); + drop = true; + break; + +#if COMPATIBILITY_LEVEL == 0 + case CMD_EXIT: // Call motion control reset routine. + mc_reset(); + sys.flags.exit = On; + drop = true; + break; +#endif + + case CMD_STATUS_REPORT_ALL: // Add all statuses on to report + { + bool tlo = sys.report.tool_offset; + sys.report.value = (uint16_t)-1; + sys.report.tool_offset = tlo; + } + // no break + + case CMD_STATUS_REPORT: + case 0x05: + system_set_exec_state_flag(EXEC_STATUS_REPORT); + drop = true; + break; + + case CMD_CYCLE_START: + system_set_exec_state_flag(EXEC_CYCLE_START); + // Cancel any pending tool change + gc_state.tool_change = false; + drop = true; + break; + + case CMD_FEED_HOLD: + system_set_exec_state_flag(EXEC_FEED_HOLD); + drop = true; + break; + + case CMD_SAFETY_DOOR: + if(!hal.signals_cap.safety_door_ajar) { + system_set_exec_state_flag(EXEC_SAFETY_DOOR); + drop = true; + } + break; + + case CMD_JOG_CANCEL: + char_counter = 0; + drop = true; + hal.stream.cancel_read_buffer(); +#ifdef KINEMATICS_API // needed when kinematics algorithm segments long jog distances (as it blocks reading from input stream) + if (state_get() & STATE_JOG) // Block all other states from invoking motion cancel. + system_set_exec_state_flag(EXEC_MOTION_CANCEL); +#endif + break; + + case CMD_GCODE_REPORT: + system_set_exec_state_flag(EXEC_GCODE_REPORT); + drop = true; + break; + + case CMD_PROBE_CONNECTED_TOGGLE: + if(hal.probe.connected_toggle) + hal.probe.connected_toggle(); + break; + + case CMD_OPTIONAL_STOP_TOGGLE: + if(!hal.signals_cap.stop_disable) // Not available as realtime command if HAL supports physical switch + sys.flags.optional_stop_disable = !sys.flags.optional_stop_disable; + break; + + case CMD_PID_REPORT: + system_set_exec_state_flag(EXEC_PID_REPORT); + drop = true; + break; + + case CMD_OVERRIDE_FEED_RESET: + case CMD_OVERRIDE_FEED_COARSE_PLUS: + case CMD_OVERRIDE_FEED_COARSE_MINUS: + case CMD_OVERRIDE_FEED_FINE_PLUS: + case CMD_OVERRIDE_FEED_FINE_MINUS: + case CMD_OVERRIDE_RAPID_RESET: + case CMD_OVERRIDE_RAPID_MEDIUM: + case CMD_OVERRIDE_RAPID_LOW: + drop = true; + enqueue_feed_override(c); + break; + + case CMD_OVERRIDE_SPINDLE_RESET: + case CMD_OVERRIDE_SPINDLE_COARSE_PLUS: + case CMD_OVERRIDE_SPINDLE_COARSE_MINUS: + case CMD_OVERRIDE_SPINDLE_FINE_PLUS: + case CMD_OVERRIDE_SPINDLE_FINE_MINUS: + case CMD_OVERRIDE_SPINDLE_STOP: + case CMD_OVERRIDE_COOLANT_FLOOD_TOGGLE: + case CMD_OVERRIDE_COOLANT_MIST_TOGGLE: + drop = true; + enqueue_accessory_override((uint8_t)c); + break; + + case CMD_REBOOT: + if(esc && hal.reboot) + hal.reboot(); // Force MCU reboot. This call should never return. + break; + + default: + if(c < ' ' || (c >= 0x7F && c <= 0xBF)) + drop = true; + break; + } + + // 2. Process printable ASCII characters and top-bit set characters + // If legacy realtime commands are disabled they are returned to the input stream + // when appering in settings ($ commands) or comments + + if(!drop) switch ((unsigned char)c) { + + case CMD_STATUS_REPORT_LEGACY: + if(!keep_rt_commands || settings.flags.legacy_rt_commands) { + system_set_exec_state_flag(EXEC_STATUS_REPORT); + drop = true; + } + break; + + case CMD_CYCLE_START_LEGACY: + if(!keep_rt_commands || settings.flags.legacy_rt_commands) { + system_set_exec_state_flag(EXEC_CYCLE_START); + // Cancel any pending tool change + gc_state.tool_change = false; + drop = true; + } + break; + + case CMD_FEED_HOLD_LEGACY: + if(!keep_rt_commands || settings.flags.legacy_rt_commands) { + system_set_exec_state_flag(EXEC_FEED_HOLD); + drop = true; + } + break; + + default: // Drop top bit set characters + drop = !(keep_rt_commands || (unsigned char)c < 0x7F); + break; + } + + esc = c == ASCII_ESC; + + return drop; +} + +// Enqueue a function to be called once by the +// foreground process, typically enqueued from an interrupt handler. +ISR_CODE bool protocol_enqueue_rt_command (on_execute_realtime_ptr fn) +{ + bool ok; + uint_fast8_t bptr = (realtime_queue.head + 1) & (RT_QUEUE_SIZE - 1); // Get next head pointer + + if((ok = bptr != realtime_queue.tail)) { // If not buffer full + realtime_queue.fn[realtime_queue.head] = fn; // add function pointer to buffer, + realtime_queue.head = bptr; // update pointer and + system_set_exec_state_flag(EXEC_RT_COMMAND); // flag it for execute + } + + return ok; +} + +// Execute enqueued functions. +static void protocol_execute_rt_commands (void) +{ + while(realtime_queue.tail != realtime_queue.head) { + uint_fast8_t bptr = realtime_queue.tail; + on_execute_realtime_ptr call; + if((call = realtime_queue.fn[bptr])) { + realtime_queue.fn[bptr] = NULL; + call(state_get()); + } + realtime_queue.tail = (bptr + 1) & (RT_QUEUE_SIZE - 1); + } +} + +void protocol_execute_noop (sys_state_t state) +{ + (void)state; +} + diff --git a/protocol.h b/protocol.h new file mode 100644 index 0000000..ad2f0ac --- /dev/null +++ b/protocol.h @@ -0,0 +1,56 @@ +/* + protocol.h - controls Grbl execution protocol and procedures + + Part of grblHAL + + Copyright (c) 2011-2016 Sungeun K. Jeon for Gnea Research LLC + Copyright (c) 2009-2011 Simen Svale Skogsrud + + 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 . +*/ + +#ifndef _PROTOCOL_H_ +#define _PROTOCOL_H_ + +// Line buffer size from the input stream to be executed. +// NOTE: Not a problem except for extreme cases, but the line buffer size can be too small +// and g-code blocks can get truncated. Officially, the g-code standards support up to 256 +// characters. In future versions, this will be increased, when we know how much extra +// memory space we can invest into here or we re-write the g-code parser not to have this +// buffer. +#ifndef LINE_BUFFER_SIZE + #define LINE_BUFFER_SIZE 257 // 256 characters plus terminator +#endif + +// Starts Grbl main loop. It handles all incoming characters from the input stream and executes +// them as they complete. It is also responsible for finishing the initialization procedures. +bool protocol_main_loop (void); + +// Checks and executes a realtime command at various stop points in main program +bool protocol_execute_realtime (void); +bool protocol_exec_rt_system (void); +void protocol_execute_noop (uint_fast16_t state); +bool protocol_enqueue_rt_command (on_execute_realtime_ptr fn); + +// Executes the auto cycle feature, if enabled. +void protocol_auto_cycle_start (void); + +// Block until all buffered steps are executed +bool protocol_buffer_synchronize (void); + +bool protocol_enqueue_realtime_command (char c); +bool protocol_enqueue_gcode (char *data); +void protocol_message (char *message); + +#endif diff --git a/report.c b/report.c new file mode 100644 index 0000000..4ca1362 --- /dev/null +++ b/report.c @@ -0,0 +1,1869 @@ +/* + report.c - reporting and messaging methods + + Part of grblHAL + + Copyright (c) 2017-2021 Terje Io + Copyright (c) 2012-2016 Sungeun K. Jeon for Gnea Research LLC + + Grbl is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + Grbl is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with Grbl. If not, see . +*/ + +/* + This file functions as the primary feedback interface for Grbl. Any outgoing data, such + as the protocol status messages, feedback messages, and status reports, are stored here. + For the most part, these functions primarily are called from protocol.c methods. If a + different style feedback is desired (i.e. JSON), then a user can change these following + methods to accomodate their needs. +*/ + +#include +#include +#include +#include + +#include "hal.h" +#include "report.h" +#include "nvs_buffer.h" +#include "limits.h" +#include "state_machine.h" + +#ifdef ENABLE_SPINDLE_LINEARIZATION +#include +#endif + +#ifndef REPORT_OVERRIDE_REFRESH_BUSY_COUNT +#define REPORT_OVERRIDE_REFRESH_BUSY_COUNT 20 // (1-255) +#endif +#ifndef REPORT_OVERRIDE_REFRESH_IDLE_COUNT +#define REPORT_OVERRIDE_REFRESH_IDLE_COUNT 10 // (1-255) Must be less than or equal to the busy count +#endif +#ifndef REPORT_WCO_REFRESH_BUSY_COUNT +#define REPORT_WCO_REFRESH_BUSY_COUNT 30 // (2-255) +#endif +#ifndef REPORT_WCO_REFRESH_IDLE_COUNT +#define REPORT_WCO_REFRESH_IDLE_COUNT 10 // (2-255) Must be less than or equal to the busy count +#endif + +// Compile-time sanity check of defines + +#if (REPORT_WCO_REFRESH_BUSY_COUNT < REPORT_WCO_REFRESH_IDLE_COUNT) + #error "WCO busy refresh is less than idle refresh." +#endif +#if (REPORT_OVERRIDE_REFRESH_BUSY_COUNT < REPORT_OVERRIDE_REFRESH_IDLE_COUNT) + #error "Override busy refresh is less than idle refresh." +#endif +#if (REPORT_WCO_REFRESH_IDLE_COUNT < 2) + #error "WCO refresh must be greater than one." +#endif +#if (REPORT_OVERRIDE_REFRESH_IDLE_COUNT < 1) + #error "Override refresh must be greater than zero." +#endif + +static char buf[(STRLEN_COORDVALUE + 1) * N_AXIS]; +static char *(*get_axis_values)(float *axis_values); +static char *(*get_axis_value)(float value); +static char *(*get_rate_value)(float value); +static uint8_t override_counter = 0; // Tracks when to add override data to status reports. +static uint8_t wco_counter = 0; // Tracks when to add work coordinate offset data to status reports. +static const char vbar[2] = { '|', '\0' }; + +static bool report_setting (const setting_detail_t *setting, uint_fast16_t offset, void *data); + +static const report_t report_fns = { + .setting = report_setting, + .status_message = report_status_message, + .feedback_message = report_feedback_message +}; + +// Append a number of strings to the static buffer +// NOTE: do NOT use for several int/float conversions as these share the same underlying buffer! +static char *appendbuf (int argc, ...) +{ + char c, *s = buf, *arg; + + va_list list; + va_start(list, argc); + + while(argc--) { + arg = va_arg(list, char *); + do { + c = *s++ = *arg++; + } while(c); + s--; + } + + va_end(list); + + return buf; +} + +static char *map_coord_system (coord_system_id_t id) +{ + uint8_t g5x = id + 54; + + strcpy(buf, uitoa((uint32_t)(g5x > 59 ? 59 : g5x))); + if(g5x > 59) { + strcat(buf, "."); + strcat(buf, uitoa((uint32_t)(g5x - 59))); + } + + return buf; +} + +// Convert axis position values to null terminated string (mm). +static char *get_axis_values_mm (float *axis_values) +{ + uint_fast32_t idx; + + buf[0] = '\0'; + + for (idx = 0; idx < N_AXIS; idx++) { + if(idx == X_AXIS && gc_state.modal.diameter_mode) + strcat(buf, ftoa(axis_values[idx] * 2.0f, N_DECIMAL_COORDVALUE_MM)); + else + strcat(buf, ftoa(axis_values[idx], N_DECIMAL_COORDVALUE_MM)); + if (idx < (N_AXIS - 1)) + strcat(buf, ","); + } + + return buf; +} + +// Convert axis position values to null terminated string (inch). +static char *get_axis_values_inches (float *axis_values) +{ + uint_fast32_t idx; + + buf[0] = '\0'; + + for (idx = 0; idx < N_AXIS; idx++) { + if(idx == X_AXIS && gc_state.modal.diameter_mode) + strcat(buf, ftoa(axis_values[idx] * INCH_PER_MM * 2.0f, N_DECIMAL_COORDVALUE_INCH)); + else + strcat(buf, ftoa(axis_values[idx] * INCH_PER_MM, N_DECIMAL_COORDVALUE_INCH)); + if (idx < (N_AXIS - 1)) + strcat(buf, ","); + } + + return buf; +} + +// Convert rate value to null terminated string (mm). +static char *get_axis_value_mm (float value) +{ + return strcpy(buf, ftoa(value, N_DECIMAL_COORDVALUE_MM)); +} + +// Convert rate value to null terminated string (mm). +static char *get_axis_value_inches (float value) +{ + return strcpy(buf, ftoa(value * INCH_PER_MM, N_DECIMAL_COORDVALUE_INCH)); +} + +// Convert rate value to null terminated string (mm). +static char *get_rate_value_mm (float value) +{ + return uitoa((uint32_t)value); +} + +// Convert rate value to null terminated string (mm). +static char *get_rate_value_inch (float value) +{ + return uitoa((uint32_t)(value * INCH_PER_MM)); +} + +// Convert axes signals bits to string representation. +// NOTE: returns pointer to null terminator! +inline static char *axis_signals_tostring (char *buf, axes_signals_t signals) +{ + if(signals.x) + *buf++ = 'X'; + + if(signals.y) + *buf++ = 'Y'; + + if (signals.z) + *buf++ = 'Z'; + +#ifdef A_AXIS + if (signals.a) + *buf++ = 'A'; +#endif + +#ifdef B_AXIS + if (signals.b) + *buf++ = 'B'; +#endif + +#ifdef C_AXIS + if (signals.c) + *buf++ = 'C'; +#endif + + *buf = '\0'; + + return buf; +} + +// Convert control signals bits to string representation. +// NOTE: returns pointer to null terminator! +inline static char *control_signals_tostring (char *buf, control_signals_t signals) +{ + if (signals.safety_door_ajar && hal.signals_cap.safety_door_ajar) + *buf++ = 'D'; + if (signals.reset) + *buf++ = 'R'; + if (signals.feed_hold) + *buf++ = 'H'; + if (signals.cycle_start) + *buf++ = 'S'; + if (signals.e_stop) + *buf++ = 'E'; + if (signals.block_delete && sys.flags.block_delete_enabled) + *buf++ = 'L'; + if (hal.signals_cap.stop_disable ? signals.stop_disable : sys.flags.optional_stop_disable) + *buf++ = 'T'; + if (signals.motor_warning) + *buf++ = 'W'; + if (signals.motor_fault) + *buf++ = 'M'; + + *buf = '\0'; + + return buf; +} + +void report_init (void) +{ + get_axis_value = settings.flags.report_inches ? get_axis_value_inches : get_axis_value_mm; + get_axis_values = settings.flags.report_inches ? get_axis_values_inches : get_axis_values_mm; + get_rate_value = settings.flags.report_inches ? get_rate_value_inch : get_rate_value_mm; +} + +void report_init_fns (void) +{ + memcpy(&grbl.report, &report_fns, sizeof(report_t)); +} + +// Handles the primary confirmation protocol response for streaming interfaces and human-feedback. +// For every incoming line, this method responds with an 'ok' for a successful command or an +// 'error:' to indicate some error event with the line or some critical system error during +// operation. Errors events can originate from the g-code parser, settings module, or asynchronously +// from a critical error, such as a triggered hard limit. Interface should always monitor for these +// responses. +status_code_t report_status_message (status_code_t status_code) +{ + switch(status_code) { + + case Status_OK: // STATUS_OK + hal.stream.write("ok" ASCII_EOL); + break; + + default: + hal.stream.write(appendbuf(3, "error:", uitoa((uint32_t)status_code), ASCII_EOL)); + break; + } + + return status_code; +} + + +// Prints alarm messages. +alarm_code_t report_alarm_message (alarm_code_t alarm_code) +{ + hal.stream.write_all(appendbuf(3, "ALARM:", uitoa((uint32_t)alarm_code), ASCII_EOL)); + hal.delay_ms(500, NULL); // Force delay to ensure message clears output stream buffer. + + return alarm_code; +} + +// Prints feedback message, typically from gcode. +void report_message (const char *msg, message_type_t type) +{ + hal.stream.write("[MSG:"); + + switch(type) { + + case Message_Info: + hal.stream.write("Info: "); + break; + + case Message_Warning: + hal.stream.write("Warning: "); + break; + + default: + break; + } + + hal.stream.write(msg); + hal.stream.write("]" ASCII_EOL); +} + +// Prints feedback messages. This serves as a centralized method to provide additional +// user feedback for things that are not of the status/alarm message protocol. These are +// messages such as setup warnings, switch toggling, and how to exit alarms. +// NOTE: For interfaces, messages are always placed within brackets. And if silent mode +// is installed, the message number codes are less than zero. +message_code_t report_feedback_message (message_code_t message_code) +{ + hal.stream.write_all("[MSG:"); + + switch(message_code) { + + case Message_None: + break; + + case Message_CriticalEvent: + hal.stream.write_all("Reset to continue"); + break; + + case Message_AlarmLock: + hal.stream.write_all("'$H'|'$X' to unlock"); + break; + + case Message_AlarmUnlock: + hal.stream.write_all("Caution: Unlocked"); + break; + + case Message_Enabled: + hal.stream.write_all("Enabled"); + break; + + case Message_Disabled: + hal.stream.write_all("Disabled"); + break; + + case Message_SafetyDoorAjar: + hal.stream.write_all("Check Door"); + break; + + case Message_CheckLimits: + hal.stream.write_all("Check Limits"); + break; + + case Message_ProgramEnd: + hal.stream.write_all("Pgm End"); + break; + + case Message_RestoreDefaults: + hal.stream.write_all("Restoring defaults"); + break; + + case Message_SpindleRestore: + hal.stream.write_all("Restoring spindle"); + break; + + case Message_SleepMode: + hal.stream.write_all("Sleeping"); + break; + + case Message_EStop: + hal.stream.write_all("Emergency stop"); + break; + + case Message_HomingCycleRequired: + hal.stream.write_all("Homing cycle required"); + break; + + case Message_CycleStartToRerun: + hal.stream.write_all("Press cycle start to rerun job"); + break; + + case Message_ReferenceTLOEstablished: + hal.stream.write_all("Reference tool length offset established"); + break; + + case Message_MotorFault: + hal.stream.write_all("Motor fault"); + break; + + default: + if(grbl.on_unknown_feedback_message) + grbl.on_unknown_feedback_message(hal.stream.write_all); + break; + } + + hal.stream.write_all("]" ASCII_EOL); + + return message_code; +} + + +// Welcome message +void report_init_message (void) +{ + override_counter = wco_counter = 0; +#if COMPATIBILITY_LEVEL == 0 + hal.stream.write_all(ASCII_EOL "GrblHAL " GRBL_VERSION " ['$' or '$HELP' for help]" ASCII_EOL); +#else + hal.stream.write_all(ASCII_EOL "Grbl " GRBL_VERSION " ['$' for help]" ASCII_EOL); +#endif +} + +// Grbl help message +void report_grbl_help (void) +{ + hal.stream.write("[HLP:$$ $# $G $I $N $x=val $Nx=line $J=line $SLP $C $X $H $B ~ ! ? ctrl-x]" ASCII_EOL); +} + +#define CAPS(c) ((c >= 'a' && c <= 'z') ? c & 0x5F : c) + +static void report_group_settings (const setting_group_detail_t *groups, const uint_fast8_t n_groups, char *lcargs) +{ + uint_fast8_t idx; + uint_fast8_t len = strlen(lcargs); + + for(idx = 0; idx < n_groups; idx++) { + if(strlen(groups[idx].name) == len) { + char *s1 = lcargs, *s2 = (char *)groups[idx].name; + while(*s1 && CAPS(*s1) == CAPS(*s2)) { + s1++; + s2++; + } + if(*s1 == '\0') { + report_settings_details(true, Setting_SettingsAll, groups[idx].id); + break; + } + } + } +} + +status_code_t report_help (char *args, char *lcargs) +{ + setting_details_t *settings_info = settings_get_details(); + + // Strip leading spaces + while(*args == ' ') + args++; + + if(*args == '\0') { + + hal.stream.write("Help arguments:" ASCII_EOL); + hal.stream.write(" Commands" ASCII_EOL); + hal.stream.write(" Settings" ASCII_EOL); + report_setting_group_details(false, " "); + } else { + if(!strncmp(args, "COMMANDS", 8)) { + hal.stream.write("$I - list system information" ASCII_EOL); + hal.stream.write("$$ - list settings" ASCII_EOL); + hal.stream.write("$# - list offsets, tool table, probing and home position" ASCII_EOL); + hal.stream.write("$G - list parser state" ASCII_EOL); + hal.stream.write("$N - list startup lines" ASCII_EOL); + if(settings.homing.flags.enabled) + hal.stream.write("$H - home configured axes" ASCII_EOL); + if(settings.homing.flags.single_axis_commands) + hal.stream.write("$H - home single axis" ASCII_EOL); + hal.stream.write("$X - unlock machine" ASCII_EOL); + hal.stream.write("$SLP - enter sleep mode" ASCII_EOL); + hal.stream.write("$HELP - help" ASCII_EOL); + hal.stream.write("$RST=* - restore/reset all" ASCII_EOL); + hal.stream.write("$RST=$ - restore default settings" ASCII_EOL); + if(settings_info->on_get_settings) + hal.stream.write("$RST=& - restore driver and plugin default settings" ASCII_EOL); +#ifdef N_TOOLS + hal.stream.write("$RST=# - reset offsets and tool data" ASCII_EOL); +#else + hal.stream.write("$RST=# - reset offsets" ASCII_EOL); +#endif + if(grbl.on_report_command_help) + grbl.on_report_command_help(); + } else if(!strncmp(args, "SETTINGS", 8)) + report_settings_details(true, Setting_SettingsAll, Group_All); + else { + + // Strip leading spaces from lowercase version + while(*lcargs == ' ') + lcargs++; + + report_group_settings(settings_info->groups, settings_info->n_groups, lcargs); + + if(grbl.on_get_settings) { + + on_get_settings_ptr on_get_settings = grbl.on_get_settings; + + while(on_get_settings) { + settings_info = on_get_settings(); + if(settings_info->groups) + report_group_settings(settings_info->groups, settings_info->n_groups, lcargs); + on_get_settings = settings_info->on_get_settings; + } + } + } + } + + return Status_OK; +} + + +// Grbl settings print out. + +static int cmp_settings (const void *a, const void *b) +{ + return (*(setting_detail_t **)(a))->id - (*(setting_detail_t **)(b))->id; +} + +static bool report_setting (const setting_detail_t *setting, uint_fast16_t offset, void *data) +{ + appendbuf(3, "$", uitoa(setting->id + offset), "="); + + char *value = setting_get_value(setting, offset); + + if(value) { + hal.stream.write(buf); + hal.stream.write(value); + hal.stream.write(ASCII_EOL); + } + + return true; +} + +status_code_t report_grbl_setting (setting_id_t id, void *data) +{ + status_code_t status = Status_OK; + + const setting_detail_t *setting = setting_get_details(id, NULL); + + if(setting) + grbl.report.setting(setting, id - setting->id, data); + else + status = Status_SettingDisabled; + + return status; +} + +static bool print_setting (const setting_detail_t *setting, uint_fast16_t offset, void *data) +{ + if(setting->value != NULL) + grbl.report.setting(setting, offset, data); + else { + hal.stream.write("$"); + hal.stream.write(uitoa(setting->id)); + hal.stream.write("=N/A" ASCII_EOL); + } + + return true; +} + +void report_grbl_settings (bool all, void *data) +{ + setting_details_t *details = settings_get_details(); + uint16_t n_settings = details->n_settings; + const setting_detail_t *setting; + + while(details->on_get_settings) { + details = details->on_get_settings(); + n_settings += details->n_settings; + } + + setting_detail_t **all_settings, **psetting; + + if((all_settings = calloc(n_settings, sizeof(setting_detail_t *)))) { + + uint_fast16_t idx; + + details = settings_get_details(); + psetting = all_settings; + n_settings = 0; + + for(idx = 0; idx < details->n_settings; idx++) { + setting = &details->settings[idx]; + if((all || setting->type == Setting_IsLegacy || setting->type == Setting_IsLegacyFn) && + (setting->is_available == NULL ||setting->is_available(setting))) { + *psetting++ = (setting_detail_t *)setting; + n_settings++; + } + } + + if(all) while(details->on_get_settings) { + details = details->on_get_settings(); + for(idx = 0; idx < details->n_settings; idx++) { + setting = &details->settings[idx]; + if(setting->is_available == NULL ||setting->is_available(setting)) { + *psetting++ = (setting_detail_t *)setting; + n_settings++; + } + } + } + + qsort(all_settings, n_settings, sizeof(setting_detail_t *), cmp_settings); + + for(idx = 0; idx < n_settings; idx++) + settings_iterator(all_settings[idx], print_setting, data); + + free(all_settings); + } +} + + +// Prints current probe parameters. Upon a probe command, these parameters are updated upon a +// successful probe or upon a failed probe with the G38.3 without errors command (if supported). +// These values are retained until Grbl is power-cycled, whereby they will be re-zeroed. +void report_probe_parameters (void) +{ + // Report in terms of machine position. + float print_position[N_AXIS]; + system_convert_array_steps_to_mpos(print_position, sys.probe_position); + hal.stream.write("[PRB:"); + hal.stream.write(get_axis_values(print_position)); + hal.stream.write(sys.flags.probe_succeeded ? ":1" : ":0"); + hal.stream.write("]" ASCII_EOL); +} + +// Prints current home position in terms of machine position. +// Bitmask for homed axes attached. +void report_home_position (void) +{ + hal.stream.write("[HOME:"); + hal.stream.write(get_axis_values(sys.home_position)); + hal.stream.write(":"); + hal.stream.write(uitoa(sys.homed.mask)); + hal.stream.write("]" ASCII_EOL); +} + +// Prints current tool offsets. +void report_tool_offsets (void) +{ + hal.stream.write("[TLO:"); +#ifdef TOOL_LENGTH_OFFSET_AXIS + hal.stream.write(get_axis_value(gc_state.tool_length_offset[Z_AXIS])); +#else + hal.stream.write(get_axis_values(gc_state.tool_length_offset)); +#endif + hal.stream.write("]" ASCII_EOL); +} + +// Prints Grbl NGC parameters (coordinate offsets, probing, tool table) +void report_ngc_parameters (void) +{ + uint_fast8_t idx; + float coord_data[N_AXIS]; + + if(gc_state.modal.scaling_active) { + hal.stream.write("[G51:"); + hal.stream.write(get_axis_values(gc_get_scaling())); + hal.stream.write("]" ASCII_EOL); + } + + for (idx = 0; idx < N_CoordinateSystems; idx++) { + + if (!(settings_read_coord_data((coord_system_id_t)idx, &coord_data))) { + grbl.report.status_message(Status_SettingReadFail); + return; + } + + hal.stream.write("[G"); + + switch (idx) { + + case CoordinateSystem_G28: + hal.stream.write("28"); + break; + + case CoordinateSystem_G30: + hal.stream.write("30"); + break; + + case CoordinateSystem_G92: + break; + + default: // G54-G59 + hal.stream.write(map_coord_system((coord_system_id_t)idx)); + break; + } + + if(idx != CoordinateSystem_G92) { + hal.stream.write(":"); + hal.stream.write(get_axis_values(coord_data)); + hal.stream.write("]" ASCII_EOL); + } + } + + // Print G92, G92.1 which are not persistent in memory + hal.stream.write("92:"); + hal.stream.write(get_axis_values(gc_state.g92_coord_offset)); + hal.stream.write("]" ASCII_EOL); + +#ifdef N_TOOLS + for (idx = 1; idx <= N_TOOLS; idx++) { + hal.stream.write("[T:"); + hal.stream.write(uitoa((uint32_t)idx)); + hal.stream.write("|"); + hal.stream.write(get_axis_values(tool_table[idx].offset)); + hal.stream.write("|"); + hal.stream.write(get_axis_value(tool_table[idx].radius)); + hal.stream.write("]" ASCII_EOL); + } +#endif + +#if COMPATIBILITY_LEVEL < 10 + if(settings.homing.flags.enabled) + report_home_position(); +#endif + + report_tool_offsets(); // Print tool length offset value. + report_probe_parameters(); // Print probe parameters. Not persistent in memory. + if(sys.tlo_reference_set.mask) { // Print tool length reference offset. Not persistent in memory. + plane_t plane; + gc_get_plane_data(&plane, gc_state.modal.plane_select); + hal.stream.write("[TLR:"); + hal.stream.write(get_axis_value(sys.tlo_reference[plane.axis_linear] / settings.axis[plane.axis_linear].steps_per_mm)); + hal.stream.write("]" ASCII_EOL); + } +} + +static inline bool is_g92_active (void) +{ + bool active = false; + uint_fast32_t idx = N_AXIS; + + do { + idx--; + active = !(gc_state.g92_coord_offset[idx] == 0.0f || gc_state.g92_coord_offset[idx] == -0.0f); + } while(idx && !active); + + return active; +} + +// Print current gcode parser mode state +void report_gcode_modes (void) +{ + hal.stream.write("[GC:G"); + if (gc_state.modal.motion >= MotionMode_ProbeToward) { + hal.stream.write("38."); + hal.stream.write(uitoa((uint32_t)(gc_state.modal.motion - (MotionMode_ProbeToward - 2)))); + } else + hal.stream.write(uitoa((uint32_t)gc_state.modal.motion)); + + hal.stream.write(" G"); + hal.stream.write(map_coord_system(gc_state.modal.coord_system.id)); + +#if COMPATIBILITY_LEVEL < 10 + + if(is_g92_active()) + hal.stream.write(" G92"); + +#endif + + if(settings.mode == Mode_Lathe) + hal.stream.write(gc_state.modal.diameter_mode ? " G7" : " G8"); + + hal.stream.write(" G"); + hal.stream.write(uitoa((uint32_t)(gc_state.modal.plane_select + 17))); + + hal.stream.write(gc_state.modal.units_imperial ? " G20" : " G21"); + + hal.stream.write(gc_state.modal.distance_incremental ? " G91" : " G90"); + + hal.stream.write(" G"); + hal.stream.write(uitoa((uint32_t)(94 - gc_state.modal.feed_mode))); + + if(settings.mode == Mode_Lathe && hal.driver_cap.variable_spindle) + hal.stream.write(gc_state.modal.spindle_rpm_mode == SpindleSpeedMode_RPM ? " G97" : " G96"); + +#if COMPATIBILITY_LEVEL < 10 + + if(gc_state.modal.tool_offset_mode == ToolLengthOffset_Cancel) + hal.stream.write(" G49"); + else { + hal.stream.write(" G43"); + if(gc_state.modal.tool_offset_mode != ToolLengthOffset_Enable) + hal.stream.write(gc_state.modal.tool_offset_mode == ToolLengthOffset_EnableDynamic ? ".1" : ".2"); + } + + hal.stream.write(gc_state.canned.retract_mode == CCRetractMode_RPos ? " G99" : " G98"); + + if(gc_state.modal.scaling_active) { + hal.stream.write(" G51:"); + axis_signals_tostring(buf, gc_get_g51_state()); + hal.stream.write(buf); + } else + hal.stream.write(" G50"); + +#endif + + if (gc_state.modal.program_flow) { + + switch (gc_state.modal.program_flow) { + + case ProgramFlow_Paused: + hal.stream.write(" M0"); + break; + + case ProgramFlow_OptionalStop: + hal.stream.write(" M1"); + break; + + case ProgramFlow_CompletedM2: + hal.stream.write(" M2"); + break; + + case ProgramFlow_CompletedM30: + hal.stream.write(" M30"); + break; + + case ProgramFlow_CompletedM60: + hal.stream.write(" M60"); + break; + + default: + break; + } + } + + hal.stream.write(gc_state.modal.spindle.on ? (gc_state.modal.spindle.ccw ? " M4" : " M3") : " M5"); + + if(gc_state.tool_change) + hal.stream.write(" M6"); + + if (gc_state.modal.coolant.value) { + + if (gc_state.modal.coolant.mist) + hal.stream.write(" M7"); + + if (gc_state.modal.coolant.flood) + hal.stream.write(" M8"); + + } else + hal.stream.write(" M9"); + + if (sys.override.control.feed_rate_disable) + hal.stream.write(" M50"); + + if (sys.override.control.spindle_rpm_disable) + hal.stream.write(" M51"); + + if (sys.override.control.feed_hold_disable) + hal.stream.write(" M53"); + + if (settings.parking.flags.enable_override_control && sys.override.control.parking_disable) + hal.stream.write(" M56"); + + hal.stream.write(appendbuf(2, " T", uitoa((uint32_t)gc_state.tool->tool))); + + hal.stream.write(appendbuf(2, " F", get_rate_value(gc_state.feed_rate))); + + if(hal.driver_cap.variable_spindle) + hal.stream.write(appendbuf(2, " S", ftoa(gc_state.spindle.rpm, N_DECIMAL_RPMVALUE))); + + hal.stream.write("]" ASCII_EOL); +} + +// Prints specified startup line +void report_startup_line (uint8_t n, char *line) +{ + hal.stream.write(appendbuf(3, "$N", uitoa((uint32_t)n), "=")); + hal.stream.write(line); + hal.stream.write(ASCII_EOL); +} + +void report_execute_startup_message (char *line, status_code_t status_code) +{ + hal.stream.write(">"); + hal.stream.write(line); + hal.stream.write(":"); + grbl.report.status_message(status_code); +} + +// Prints build info line +void report_build_info (char *line, bool extended) +{ + hal.stream.write("[VER:" GRBL_VERSION "." GRBL_VERSION_BUILD ":"); + hal.stream.write(line); + hal.stream.write("]" ASCII_EOL); + +#if COMPATIBILITY_LEVEL == 0 + extended = true; +#endif + + // Generate compile-time build option list + + char *append = &buf[5]; + + strcpy(buf, "[OPT:"); + + if(hal.driver_cap.variable_spindle) + *append++ = 'V'; + + *append++ = 'N'; + + if(hal.driver_cap.mist_control) + *append++ = 'M'; + +#ifdef COREXY + *append++ = 'C'; +#endif + + if(settings.parking.flags.enabled) + *append++ = 'P'; + + if(settings.homing.flags.force_set_origin) + *append++ = 'Z'; + + if(settings.homing.flags.single_axis_commands) + *append++ = 'H'; + + if(settings.limits.flags.two_switches) + *append++ = 'T'; + + if(settings.probe.allow_feed_override) + *append++ = 'A'; + + if(settings.spindle.flags.pwm_action == SpindleAction_DisableWithZeroSPeed) + *append++ = '0'; + + if(hal.driver_cap.software_debounce) + *append++ = 'S'; + + if(settings.parking.flags.enable_override_control) + *append++ = 'R'; + + if(!settings.homing.flags.init_lock) + *append++ = 'L'; + + if(hal.signals_cap.safety_door_ajar) + *append++ = '+'; + + #ifdef DISABLE_RESTORE_NVS_WIPE_ALL // NOTE: Shown when disabled. + *append++ = '*'; + #endif + + #ifdef DISABLE_RESTORE_NVS_DEFAULT_SETTINGS // NOTE: Shown when disabled. + *append++ = '$'; + #endif + + #ifdef DISABLE_RESTORE_NVS_CLEAR_PARAMETERS // NOTE: Shown when disabled. + *append++ = '#'; + #endif + + #ifdef DISABLE_BUILD_INFO_WRITE_COMMAND // NOTE: Shown when disabled. + *append++ = 'I'; + #endif + + if(!settings.status_report.sync_on_wco_change) // NOTE: Shown when disabled. + *append++ = 'W'; + + if(hal.stepper.get_auto_squared) + *append++ = '2'; + + *append++ = ','; + *append = '\0'; + hal.stream.write(buf); + + // NOTE: Compiled values, like override increments/max/min values, may be added at some point later. + hal.stream.write(uitoa((uint32_t)(BLOCK_BUFFER_SIZE - 1))); + hal.stream.write(","); + hal.stream.write(uitoa(hal.rx_buffer_size)); + if(extended) { + hal.stream.write(","); + hal.stream.write(uitoa((uint32_t)N_AXIS)); + hal.stream.write(","); + #ifdef N_TOOLS + hal.stream.write(uitoa((uint32_t)N_TOOLS)); + #else + hal.stream.write("0"); + #endif + } + hal.stream.write("]" ASCII_EOL); + + if(extended) { + + nvs_io_t *nvs = nvs_buffer_get_physical(); + + strcpy(buf, "[NEWOPT:ENUMS,RT"); + strcat(buf, settings.flags.legacy_rt_commands ? "+," : "-,"); + + if(settings.homing.flags.enabled) + strcat(buf, "HOME,"); + + if(!hal.probe.get_state) + strcat(buf, "NOPROBE,"); + else if(hal.signals_cap.probe_disconnected) + strcat(buf, "PC,"); + + if(hal.signals_cap.stop_disable) + strcat(buf, "OS,"); + + if(hal.signals_cap.block_delete) + strcat(buf, "BD,"); + + if(hal.signals_cap.e_stop) + strcat(buf, "ES,"); + + if(hal.driver_cap.mpg_mode) + strcat(buf, "MPG,"); + + if(settings.mode == Mode_Lathe) + strcat(buf, "LATHE,"); + + #ifdef N_TOOLS + if(hal.driver_cap.atc && hal.tool.change) + strcat(buf, "ATC,"); + else + #endif + if(hal.stream.suspend_read) + strcat(buf, "TC,"); // Manual tool change supported (M6) + + if(hal.driver_cap.spindle_sync) + strcat(buf, "SS,"); + + #ifdef PID_LOG + strcat(buf, "PID,"); + #endif + + append = &buf[strlen(buf) - 1]; + if(*append == ',') + *append = '\0'; + + hal.stream.write(buf); + grbl.on_report_options(true); + hal.stream.write("]" ASCII_EOL); + + hal.stream.write("[FIRMWARE:grblHAL]" ASCII_EOL); + + if(!(nvs->type == NVS_None || nvs->type == NVS_Emulated)) { + hal.stream.write("[NVS STORAGE:"); + *buf = '\0'; + if(hal.nvs.type == NVS_Emulated) + strcat(buf, "*"); + strcat(buf, nvs->type == NVS_Flash ? "FLASH" : (nvs->type == NVS_FRAM ? "FRAM" : "EEPROM")); + hal.stream.write(buf); + hal.stream.write("]" ASCII_EOL); + } + + if(hal.info) { + hal.stream.write("[DRIVER:"); + hal.stream.write(hal.info); + hal.stream.write("]" ASCII_EOL); + } + + if(hal.driver_version) { + hal.stream.write("[DRIVER VERSION:"); + hal.stream.write(hal.driver_version); + hal.stream.write("]" ASCII_EOL); + } + + if(hal.driver_options) { + hal.stream.write("[DRIVER OPTIONS:"); + hal.stream.write(hal.driver_options); + hal.stream.write("]" ASCII_EOL); + } + + if(hal.board) { + hal.stream.write("[BOARD:"); + hal.stream.write(hal.board); + hal.stream.write("]" ASCII_EOL); + } + + if(hal.max_step_rate) { + hal.stream.write("[MAX STEP RATE:"); + hal.stream.write(uitoa(hal.max_step_rate)); + hal.stream.write(" Hz]" ASCII_EOL); + } + +#if COMPATIBILITY_LEVEL > 0 + hal.stream.write("[COMPATIBILITY LEVEL:"); + hal.stream.write(uitoa(COMPATIBILITY_LEVEL)); + hal.stream.write("]" ASCII_EOL); +#endif + + grbl.on_report_options(false); + } +} + + +// Prints the character string line Grbl has received from the user, which has been pre-parsed, +// and has been sent into protocol_execute_line() routine to be executed by Grbl. +void report_echo_line_received (char *line) +{ + hal.stream.write("[echo: "); + hal.stream.write(line); + hal.stream.write("]" ASCII_EOL); +} + + + // Prints real-time data. This function grabs a real-time snapshot of the stepper subprogram + // and the actual location of the CNC machine. Users may change the following function to their + // specific needs, but the desired real-time data report must be as short as possible. This is + // requires as it minimizes the computational overhead and allows grbl to keep running smoothly, + // especially during g-code programs with fast, short line segments and high frequency reports (5-20Hz). +void report_realtime_status (void) +{ + static bool probing = false; + + int32_t current_position[N_AXIS]; // Copy current state of the system position variable + float print_position[N_AXIS]; + probe_state_t probe_state = { + .connected = On, + .triggered = Off + }; + + memcpy(current_position, sys.position, sizeof(sys.position)); + system_convert_array_steps_to_mpos(print_position, current_position); + + if(hal.probe.get_state) + probe_state = hal.probe.get_state(); + + // Report current machine state and sub-states + hal.stream.write_all("<"); + + sys_state_t state = state_get(); + + switch (gc_state.tool_change && state == STATE_CYCLE ? STATE_TOOL_CHANGE : state) { + + case STATE_IDLE: + hal.stream.write_all("Idle"); + break; + + case STATE_CYCLE: + hal.stream.write_all("Run"); + if(sys.probing_state == Probing_Active && settings.status_report.run_substate) + probing = true; + else if (probing) + probing = probe_state.triggered; + if(sys.flags.feed_hold_pending) + hal.stream.write_all(":1"); + else if(probing) + hal.stream.write_all(":2"); + break; + + case STATE_HOLD: + hal.stream.write_all(appendbuf(2, "Hold:", uitoa((uint32_t)(sys.holding_state - 1)))); + break; + + case STATE_JOG: + hal.stream.write_all("Jog"); + break; + + case STATE_HOMING: + hal.stream.write_all("Home"); + break; + + case STATE_ESTOP: + case STATE_ALARM: + if((sys.report.all || settings.status_report.alarm_substate) && sys.alarm) + hal.stream.write_all(appendbuf(2, "Alarm:", uitoa((uint32_t)sys.alarm))); + else + hal.stream.write_all("Alarm"); + break; + + case STATE_CHECK_MODE: + hal.stream.write_all("Check"); + break; + + case STATE_SAFETY_DOOR: + hal.stream.write_all(appendbuf(2, "Door:", uitoa((uint32_t)sys.parking_state))); + break; + + case STATE_SLEEP: + hal.stream.write_all("Sleep"); + break; + + case STATE_TOOL_CHANGE: + hal.stream.write_all("Tool"); + break; + } + + uint_fast8_t idx; + float wco[N_AXIS]; + if (!settings.status_report.machine_position || sys.report.wco) { + for (idx = 0; idx < N_AXIS; idx++) { + // Apply work coordinate offsets and tool length offset to current position. + wco[idx] = gc_get_offset(idx); + if (!settings.status_report.machine_position) + print_position[idx] -= wco[idx]; + } + } + + // Report position + hal.stream.write_all(settings.status_report.machine_position ? "|MPos:" : "|WPos:"); + hal.stream.write_all(get_axis_values(print_position)); + + // Returns planner and output stream buffer states. + + if (settings.status_report.buffer_state) { + hal.stream.write_all("|Bf:"); + hal.stream.write_all(uitoa((uint32_t)plan_get_block_buffer_available())); + hal.stream.write_all(","); + hal.stream.write_all(uitoa(hal.stream.get_rx_buffer_available())); + } + + if(settings.status_report.line_numbers) { + // Report current line number + plan_block_t *cur_block = plan_get_current_block(); + if (cur_block != NULL && cur_block->line_number > 0) + hal.stream.write_all(appendbuf(2, "|Ln:", uitoa((uint32_t)cur_block->line_number))); + } + + spindle_state_t sp_state = hal.spindle.get_state(); + + // Report realtime feed speed + if(settings.status_report.feed_speed) { + if(hal.driver_cap.variable_spindle) { + hal.stream.write_all(appendbuf(2, "|FS:", get_rate_value(st_get_realtime_rate()))); + hal.stream.write_all(appendbuf(2, ",", uitoa(sp_state.on ? lroundf(sys.spindle_rpm) : 0))); + if(hal.spindle.get_data /* && sys.mpg_mode */) + hal.stream.write_all(appendbuf(2, ",", uitoa(lroundf(hal.spindle.get_data(SpindleData_RPM)->rpm)))); + } else + hal.stream.write_all(appendbuf(2, "|F:", get_rate_value(st_get_realtime_rate()))); + } + + if(settings.status_report.pin_state) { + + axes_signals_t lim_pin_state = limit_signals_merge(hal.limits.get_state()); + control_signals_t ctrl_pin_state = hal.control.get_state(); + + if (lim_pin_state.value | ctrl_pin_state.value | probe_state.triggered | !probe_state.connected | sys.flags.block_delete_enabled) { + + char *append = &buf[4]; + + strcpy(buf, "|Pn:"); + + if (probe_state.triggered) + *append++ = 'P'; + + if(!probe_state.connected) + *append++ = 'O'; + + if (lim_pin_state.value && !hal.control.get_state().limits_override) + append = axis_signals_tostring(append, lim_pin_state); + + if (ctrl_pin_state.value) + append = control_signals_tostring(append, ctrl_pin_state); + + *append = '\0'; + hal.stream.write_all(buf); + } + } + + if(settings.status_report.work_coord_offset) { + + if (wco_counter > 0 && !sys.report.wco) + wco_counter--; + else + wco_counter = state_get() & (STATE_HOMING|STATE_CYCLE|STATE_HOLD|STATE_JOG|STATE_SAFETY_DOOR) + ? (REPORT_WCO_REFRESH_BUSY_COUNT - 1) // Reset counter for slow refresh + : (REPORT_WCO_REFRESH_IDLE_COUNT - 1); + } else + sys.report.wco = Off; + + if(settings.status_report.overrides) { + + if (override_counter > 0 && !sys.report.overrides) + override_counter--; + else { + sys.report.overrides = On; + sys.report.spindle = sys.report.spindle || hal.spindle.get_state().on; + sys.report.coolant = sys.report.coolant || hal.coolant.get_state().value != 0; + override_counter = state_get() & (STATE_HOMING|STATE_CYCLE|STATE_HOLD|STATE_JOG|STATE_SAFETY_DOOR) + ? (REPORT_OVERRIDE_REFRESH_BUSY_COUNT - 1) // Reset counter for slow refresh + : (REPORT_OVERRIDE_REFRESH_IDLE_COUNT - 1); + } + } else + sys.report.overrides = Off; + + if(sys.report.value || gc_state.tool_change) { + + if(sys.report.wco) { + hal.stream.write_all("|WCO:"); + hal.stream.write_all(get_axis_values(wco)); + } + + if(sys.report.gwco) { + hal.stream.write_all("|WCS:G"); + hal.stream.write_all(map_coord_system(gc_state.modal.coord_system.id)); + } + + if(sys.report.overrides) { + hal.stream.write_all(appendbuf(2, "|Ov:", uitoa((uint32_t)sys.override.feed_rate))); + hal.stream.write_all(appendbuf(2, ",", uitoa((uint32_t)sys.override.rapid_rate))); + hal.stream.write_all(appendbuf(2, ",", uitoa((uint32_t)sys.override.spindle_rpm))); + } + + if(sys.report.spindle || sys.report.coolant || sys.report.tool || gc_state.tool_change) { + + coolant_state_t cl_state = hal.coolant.get_state(); + + char *append = &buf[3]; + + strcpy(buf, "|A:"); + + if (sp_state.on) + + *append++ = sp_state.ccw ? 'C' : 'S'; + +#if COMPATIBILITY_LEVEL == 0 + if(sp_state.encoder_error && hal.driver_cap.spindle_sync) + *append++ = 'E'; +#endif + + if (cl_state.flood) + *append++ = 'F'; + + if (cl_state.mist) + *append++ = 'M'; + + if(gc_state.tool_change && !sys.report.tool) + *append++ = 'T'; + + *append = '\0'; + hal.stream.write_all(buf); + } + + if(sys.report.scaling) { + axis_signals_tostring(buf, gc_get_g51_state()); + hal.stream.write_all("|Sc:"); + hal.stream.write_all(buf); + } + + if(sys.report.mpg_mode && hal.driver_cap.mpg_mode) + hal.stream.write_all(sys.mpg_mode ? "|MPG:1" : "|MPG:0"); + + if(sys.report.homed && (sys.homing.mask || settings.homing.flags.single_axis_commands || settings.homing.flags.manual)) { + axes_signals_t homing = {sys.homing.mask ? sys.homing.mask : AXES_BITMASK}; + hal.stream.write_all(appendbuf(2, "|H:", (homing.mask & sys.homed.mask) == homing.mask ? "1" : "0")); + if(settings.homing.flags.single_axis_commands) + hal.stream.write_all(appendbuf(2, ",", uitoa(sys.homed.mask))); + } + + if(sys.report.xmode && settings.mode == Mode_Lathe) + hal.stream.write_all(gc_state.modal.diameter_mode ? "|D:1" : "|D:0"); + + if(sys.report.tool) + hal.stream.write_all(appendbuf(2, "|T:", uitoa(gc_state.tool->tool))); + + if(sys.report.tlo_reference) + hal.stream.write_all(appendbuf(2, "|TLR:", uitoa(sys.tlo_reference_set.mask != 0))); + } + + if(grbl.on_realtime_report) + grbl.on_realtime_report(hal.stream.write_all, sys.report); + +#if COMPATIBILITY_LEVEL <= 1 + if(sys.report.all) + hal.stream.write_all("|FW:grblHAL"); + else +#endif + if(settings.status_report.parser_state) { + + static uint32_t tool; + static float feed_rate, spindle_rpm; + static gc_modal_t last_state; + static bool g92_active; + + bool is_changed = feed_rate != gc_state.feed_rate || spindle_rpm != gc_state.spindle.rpm || tool != gc_state.tool->tool; + + if(is_changed) { + feed_rate = gc_state.feed_rate; + tool = gc_state.tool->tool; + spindle_rpm = gc_state.spindle.rpm; + } else if ((is_changed = g92_active != is_g92_active())) + g92_active = !g92_active; + else if(memcmp(&last_state, &gc_state.modal, sizeof(gc_modal_t))) { + last_state = gc_state.modal; + is_changed = true; + } + + if (is_changed) + system_set_exec_state_flag(EXEC_GCODE_REPORT); + + if(sys.report.tool_offset) + system_set_exec_state_flag(EXEC_TLO_REPORT); + } + + hal.stream.write_all(">" ASCII_EOL); + + sys.report.value = 0; + sys.report.wco = settings.status_report.work_coord_offset && wco_counter == 0; // Set to report on next request +} + +static void report_bitfield (const char *format, bool bitmap) +{ + char *s; + uint_fast8_t bit = 0; + uint_fast16_t val = 1; + + // Copy string from Flash to RAM, strtok cannot be used unless doing so. + if((s = (char *)malloc(strlen(format) + 1))) { + + strcpy(s, format); + char *element = strtok(s, ","); + + while(element) { + hal.stream.write(ASCII_EOL); + hal.stream.write(" "); + hal.stream.write(uitoa(bit++)); + hal.stream.write(" - "); + hal.stream.write(element); + if(bitmap) { + hal.stream.write(" ("); + hal.stream.write(uitoa(val)); + hal.stream.write(")"); + val <<= 1; + } + element = strtok(NULL, ","); + } + + free(s); + } +} + +static void report_settings_detail (bool human_readable, const setting_detail_t *setting, uint_fast8_t offset) +{ + if(human_readable) + hal.stream.write("$"); + else + hal.stream.write("[SETTING:"); + + hal.stream.write(uitoa(setting->id + offset)); + + if(human_readable) { + hal.stream.write(": "); + if(setting->group == Group_Axis0) + hal.stream.write(axis_letter[offset]); + hal.stream.write(setting->name[0] == '?' ? &setting->name[1] : setting->name); // temporary hack for ? prefix... + + switch(setting_datatype_to_external(setting->datatype)) { + + case Format_AxisMask: + hal.stream.write(" as axismask"); + break; + + case Format_Bool: + hal.stream.write(" as boolean"); + break; + + case Format_Bitfield: + hal.stream.write(" as bitfield:"); + report_bitfield(setting->format, true); + break; + + case Format_XBitfield: + hal.stream.write(" as bitfield where setting bit 0 enables the rest:"); + report_bitfield(setting->format, true); + break; + + case Format_RadioButtons: + hal.stream.write(":"); + report_bitfield(setting->format, false); + break; + + case Format_IPv4: + hal.stream.write(" as IP address"); + break; + + default: + if(setting->unit) { + hal.stream.write(" in "); + hal.stream.write(setting->unit); + } + break; + } + + if(setting->min_value && setting->max_value) { + hal.stream.write(", range: "); + hal.stream.write(setting->min_value); + hal.stream.write(" - "); + hal.stream.write(setting->max_value); + } else if(!setting_is_list(setting)) { + if(setting->min_value) { + hal.stream.write(", min: "); + hal.stream.write(setting->min_value); + } + if(setting->max_value) { + hal.stream.write(", max: "); + hal.stream.write(setting->max_value); + } + } + } else { + hal.stream.write(vbar); + hal.stream.write(uitoa(setting->group + (setting->group == Group_Axis0 ? offset : 0))); + hal.stream.write(vbar); + if(setting->group == Group_Axis0) + hal.stream.write(axis_letter[offset]); + hal.stream.write(setting->name[0] == '?' ? &setting->name[1] : setting->name); // temporary hack for ? prefix... + hal.stream.write(vbar); + if(setting->unit) + hal.stream.write(setting->unit); + hal.stream.write(vbar); + hal.stream.write(uitoa(setting_datatype_to_external(setting->datatype))); + hal.stream.write(vbar); + if(setting->format) + hal.stream.write(setting->format); + hal.stream.write(vbar); + if(setting->min_value && !setting_is_list(setting)) + hal.stream.write(setting->min_value); + hal.stream.write(vbar); + if(setting->max_value) + hal.stream.write(setting->max_value); + } + + if(!human_readable) + hal.stream.write("]"); + + hal.stream.write(ASCII_EOL); +} + +typedef struct { + bool human_readable; + setting_group_t group; + uint_fast16_t offset; +} report_args_t; + +static bool print_sorted (const setting_detail_t *setting, uint_fast16_t offset, void *args) +{ + if(!(((report_args_t *)args)->group == setting->group && ((report_args_t *)args)->offset != offset)) + report_settings_detail (((report_args_t *)args)->human_readable, setting, offset); + + return true; +} + +static status_code_t sort_settings_details (bool human_readable, setting_group_t group) +{ + bool reported = group == Group_All; + + setting_details_t *details = settings_get_details(); + uint16_t n_settings = details->n_settings; + const setting_detail_t *setting; + report_args_t args; + + args.group = settings_normalize_group(group); + args.offset = group - args.group; + args.human_readable = human_readable; + + while(details->on_get_settings) { + details = details->on_get_settings(); + n_settings += details->n_settings; + } + + setting_detail_t **all_settings, **psetting; + + if((all_settings = calloc(n_settings, sizeof(setting_detail_t *)))) { + + uint_fast16_t idx; + + details = settings_get_details(); + + psetting = all_settings; + n_settings = 0; + + for(idx = 0; idx < details->n_settings; idx++) { + setting = &details->settings[idx]; + if((group == Group_All || setting->group == args.group) && (setting->is_available == NULL ||setting->is_available(setting))) { + *psetting++ = (setting_detail_t *)setting; + n_settings++; + } + } + + while(details->on_get_settings) { + details = details->on_get_settings(); + for(idx = 0; idx < details->n_settings; idx++) { + setting = &details->settings[idx]; + if((group == Group_All || setting->group == args.group) && (setting->is_available == NULL ||setting->is_available(setting))) { + *psetting++ = (setting_detail_t *)setting; + n_settings++; + } + } + } + + qsort(all_settings, n_settings, sizeof(setting_detail_t *), cmp_settings); + + for(idx = 0; idx < n_settings; idx++) { + if(settings_iterator(all_settings[idx], print_sorted, &args)) + reported = true; + } + + free(all_settings); + } + + return all_settings == NULL ? Status_Unhandled : (reported ? Status_OK : Status_SettingDisabled); +} + +static bool print_unsorted (const setting_detail_t *setting, uint_fast16_t offset, void *args) +{ + if(!(((report_args_t *)args)->group == setting->group && ((report_args_t *)args)->offset != offset) && + (setting->is_available == NULL ||setting->is_available(setting))) + report_settings_detail(((report_args_t *)args)->human_readable, setting, offset); + + return true; +} + +static status_code_t print_settings_details (bool human_readable, setting_group_t group, uint_fast16_t axis_rpt) +{ + status_code_t status; + + if((status = sort_settings_details(human_readable, group)) != Status_Unhandled) + return status; + + bool reported = group == Group_All; + uint_fast16_t idx; + const setting_detail_t *setting; + setting_details_t *settings = settings_get_details(); + report_args_t args; + + args.group = settings_normalize_group(group); + args.offset = group - args.group; + args.human_readable = human_readable; + + do { + for(idx = 0; idx < settings->n_settings; idx++) { + + setting = &settings->settings[idx]; + + if(group == Group_All || setting->group == args.group) { + if(settings_iterator(setting, print_unsorted, &args)) + reported = true; + } + } + settings = settings->on_get_settings ? settings->on_get_settings() : NULL; + } while(settings); + + return reported ? Status_OK : Status_SettingDisabled; +} + +status_code_t report_settings_details (bool human_readable, setting_id_t id, setting_group_t group) +{ + uint_fast16_t axis_rpt = 0; + + if(id != Setting_SettingsAll) { + status_code_t status = Status_OK; + + const setting_detail_t *setting = setting_get_details(id, NULL); + + if(setting) + report_settings_detail(human_readable, setting, id - setting->id); + else + status = Status_SettingDisabled; + + return status; + } + + return print_settings_details(human_readable, group, axis_rpt); +} + +status_code_t report_alarm_details (void) +{ + uint_fast16_t idx, n_alarms = sizeof(alarm_detail) / sizeof(alarm_detail_t); + + for(idx = 0; idx < n_alarms; idx++) { + + hal.stream.write("[ALARMCODE:"); + + hal.stream.write(uitoa(alarm_detail[idx].id)); + hal.stream.write(vbar); + hal.stream.write(alarm_detail[idx].name); + hal.stream.write(vbar); + if(alarm_detail[idx].description) + hal.stream.write(alarm_detail[idx].description); + hal.stream.write("]" ASCII_EOL); + } + + return Status_OK; +} + +status_code_t report_error_details (void) +{ + uint_fast16_t idx, n_alarms = sizeof(status_detail) / sizeof(status_detail_t); + + for(idx = 0; idx < n_alarms; idx++) { + + hal.stream.write("[ERRORCODE:"); + + hal.stream.write(uitoa(status_detail[idx].id)); + hal.stream.write(vbar); + hal.stream.write(status_detail[idx].name); + hal.stream.write(vbar); + if(status_detail[idx].description) + hal.stream.write(status_detail[idx].description); + hal.stream.write("]" ASCII_EOL); + } + + return Status_OK; +} + +static void print_setting_group (const setting_group_detail_t *group, char *prefix) +{ + if(settings_is_group_available(group->id)) { + if(!prefix) { + hal.stream.write("[SETTINGGROUP:"); + hal.stream.write(uitoa(group->id)); + hal.stream.write(vbar); + hal.stream.write(uitoa(group->parent)); + hal.stream.write(vbar); + hal.stream.write(group->name); + hal.stream.write("]" ASCII_EOL); + } else if(group->id != Group_Root && settings_is_group_available(group->id)) { + hal.stream.write(prefix); + hal.stream.write(group->name); + hal.stream.write(ASCII_EOL); + } + } +} + +static int cmp_setting_group_id (const void *a, const void *b) +{ + return (*(setting_detail_t **)(a))->id - (*(setting_detail_t **)(b))->id; +} + +static int cmp_setting_group_name (const void *a, const void *b) +{ + return strcmp((*(setting_detail_t **)(a))->name, (*(setting_detail_t **)(b))->name); +} + +static status_code_t sort_setting_group_details (bool by_id, char *prefix) +{ + setting_details_t *details = settings_get_details(); + uint8_t n_groups = details->n_groups; + + while(details->on_get_settings) { + details = details->on_get_settings(); + n_groups += details->n_groups; + } + + setting_group_detail_t **all_groups, **group; + + if((all_groups = calloc(n_groups, sizeof(setting_group_detail_t *)))) { + + uint_fast16_t idx; + + group = all_groups; + details = settings_get_details(); + + do { + for(idx = 0; idx < details->n_groups; idx++) + *group++ = (setting_group_detail_t *)&details->groups[idx]; + details = details->on_get_settings ? details->on_get_settings() : NULL; + } while(details); + + qsort(all_groups, n_groups, sizeof(setting_group_detail_t *), by_id ? cmp_setting_group_id : cmp_setting_group_name); + + for(idx = 0; idx < n_groups; idx++) + print_setting_group(all_groups[idx], prefix); + + free(all_groups); + } + + return all_groups == NULL ? Status_Unhandled : Status_OK; +} + +status_code_t report_setting_group_details (bool by_id, char *prefix) +{ + if(sort_setting_group_details(by_id, prefix) != Status_Unhandled) + return Status_OK; + + uint_fast16_t idx; + setting_details_t *details = settings_get_details(); + + do { + for(idx = 0; idx < details->n_groups; idx++) + print_setting_group(&details->groups[idx], prefix); + details = details->on_get_settings ? details->on_get_settings() : NULL; + } while(details); + + return Status_OK; +} + +static char *add_limits (char *buf, limit_signals_t limits) +{ + buf = axis_signals_tostring(buf, limits.min); + *buf++ = ','; + buf = axis_signals_tostring(buf, limits.max); + *buf++ = ','; + buf = axis_signals_tostring(buf, limits.min2); + *buf++ = ','; + buf = axis_signals_tostring(buf, limits.max2); + + return buf; +} + +status_code_t report_last_signals_event (sys_state_t state, char *args) +{ + char *append = &buf[12]; + + strcpy(buf, "[LASTEVENTS:"); + + append = control_signals_tostring(append, sys.last_event.control); + *append++ = ','; + append = add_limits(append, sys.last_event.limits); + + hal.stream.write(buf); + hal.stream.write("]" ASCII_EOL); + + return Status_OK; +} + +status_code_t report_current_limit_state (sys_state_t state, char *args) +{ + char *append = &buf[8]; + + strcpy(buf, "[LIMITS:"); + + append = add_limits(append, hal.limits.get_state()); + + hal.stream.write(buf); + hal.stream.write("]" ASCII_EOL); + + return Status_OK; +} + + +// Prints spindle data (encoder pulse and index count, angular position). +status_code_t report_spindle_data (sys_state_t state, char *args) +{ + if(hal.spindle.get_data) { + + float apos = hal.spindle.get_data(SpindleData_AngularPosition)->angular_position; + spindle_data_t *spindle = hal.spindle.get_data(SpindleData_Counters); + + hal.stream.write("[SPINDLE:"); + hal.stream.write(uitoa(spindle->index_count)); + hal.stream.write(","); + hal.stream.write(uitoa(spindle->pulse_count)); + hal.stream.write(","); + hal.stream.write(uitoa(spindle->error_count)); + hal.stream.write(","); + hal.stream.write(ftoa(apos, 3)); + hal.stream.write("]" ASCII_EOL); + } + + return hal.spindle.get_data ? Status_OK : Status_InvalidStatement; +} + +void report_pid_log (void) +{ +#ifdef PID_LOG + uint_fast16_t idx = 0; + + hal.stream.write("[PID:"); + hal.stream.write(ftoa(sys.pid_log.setpoint, N_DECIMAL_PIDVALUE)); + hal.stream.write(","); + hal.stream.write(ftoa(sys.pid_log.t_sample, N_DECIMAL_PIDVALUE)); + hal.stream.write(",2|"); // 2 is number of values per sample! + + if(sys.pid_log.idx) do { + hal.stream.write(ftoa(sys.pid_log.target[idx], N_DECIMAL_PIDVALUE)); + hal.stream.write(","); + hal.stream.write(ftoa(sys.pid_log.actual[idx], N_DECIMAL_PIDVALUE)); + idx++; + if(idx != sys.pid_log.idx) + hal.stream.write(","); + } while(idx != sys.pid_log.idx); + + hal.stream.write("]" ASCII_EOL); + grbl.report.status_message(Status_OK); +#else + grbl.report.status_message(Status_GcodeUnsupportedCommand); +#endif +} diff --git a/report.h b/report.h new file mode 100644 index 0000000..5a6f14d --- /dev/null +++ b/report.h @@ -0,0 +1,103 @@ +/* + report.h - reporting and messaging methods + + Part of grblHAL + + Copyright (c) 2018-2021 Terje Io + Copyright (c) 2012-2016 Sungeun K. Jeon for Gnea Research LLC + + Grbl is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + Grbl is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with Grbl. If not, see . +*/ + +#ifndef _REPORT_H_ +#define _REPORT_H_ + +#include "system.h" + +// Message types for uncoded messages +typedef enum { + Message_Plain = 0, + Message_Info, + Message_Warning +} message_type_t; + +// Initialize reporting subsystem +void report_init (void); +void report_init_fns (void); + +// Prints system status messages. +status_code_t report_status_message (status_code_t status_code); + +// Prints system alarm messages. +alarm_code_t report_alarm_message (alarm_code_t alarm_code); + +// Prints feedback message, typically from gcode. +void report_message (const char *msg, message_type_t type); + +// Prints miscellaneous feedback messages. +message_code_t report_feedback_message (message_code_t message_code); + +// Prints welcome message. +void report_init_message (void); + +// Prints Grbl help. +status_code_t report_help (char *args, char *lcargs); +void report_grbl_help(); + +// Prints Grbl settings +void report_grbl_settings (bool all, void *data); + +// Prints Grbl setting +status_code_t report_grbl_setting (setting_id_t id, void *data); + +// Prints an echo of the pre-parsed line received right before execution. +void report_echo_line_received (char *line); + +// Prints realtime status report. +void report_realtime_status (void); + +// Prints recorded probe position. +void report_probe_parameters (void); + +// Prints current tool offsets. +void report_tool_offsets (void); + +// Prints Grbl NGC parameters (coordinate offsets, probe). +void report_ngc_parameters (void); + +// Prints current g-code parser mode state. +void report_gcode_modes (void); + +// Prints startup line when requested and executed. +void report_startup_line (uint8_t n, char *line); +void report_execute_startup_message (char *line, status_code_t status_code); + +// Prints build info and user info. +void report_build_info (char *line, bool extended); + +status_code_t report_alarm_details (void); +status_code_t report_error_details (void); +status_code_t report_setting_group_details (bool by_id, char *prefix); +status_code_t report_settings_details (bool human_readable, setting_id_t setting, setting_group_t group); + +status_code_t report_last_signals_event (sys_state_t state, char *args); +status_code_t report_current_limit_state (sys_state_t state, char *args); + +// Prints spindle data (encoder pulse and index count, angular position). +status_code_t report_spindle_data (sys_state_t state, char *args); + +// Prints current PID log. +void report_pid_log (void); + +#endif diff --git a/settings.c b/settings.c new file mode 100644 index 0000000..12b1958 --- /dev/null +++ b/settings.c @@ -0,0 +1,1807 @@ +/* + settings.c - non-volatile storage configuration handling + + Part of grblHAL + + Copyright (c) 2017-2021 Terje Io + Copyright (c) 2011-2015 Sungeun K. Jeon + Copyright (c) 2009-2011 Simen Svale Skogsrud + + 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 . +*/ + +#include +#include +#include +#include +#include + +#include "hal.h" +#include "defaults.h" +#include "limits.h" +#include "nvs_buffer.h" +#include "tool_change.h" +#ifdef ENABLE_BACKLASH_COMPENSATION +#include "motion_control.h" +#endif +#ifdef ENABLE_SPINDLE_LINEARIZATION +#include +#endif + +#ifndef SETTINGS_RESTORE_DEFAULTS +#define SETTINGS_RESTORE_DEFAULTS 1 +#endif +#ifndef SETTINGS_RESTORE_PARAMETERS +#define SETTINGS_RESTORE_PARAMETERS 1 +#endif +#ifndef SETTINGS_RESTORE_STARTUP_LINES +#define SETTINGS_RESTORE_STARTUP_LINES 1 +#endif +#ifndef SETTINGS_RESTORE_BUILD_INFO +#define SETTINGS_RESTORE_BUILD_INFO 1 +#endif +#ifndef SETTINGS_RESTORE_DRIVER_PARAMETERS +#define SETTINGS_RESTORE_DRIVER_PARAMETERS 1 +#endif + +settings_t settings; + +const settings_restore_t settings_all = { + .defaults = SETTINGS_RESTORE_DEFAULTS, + .parameters = SETTINGS_RESTORE_PARAMETERS, + .startup_lines = SETTINGS_RESTORE_STARTUP_LINES, + .build_info = SETTINGS_RESTORE_BUILD_INFO, + .driver_parameters = SETTINGS_RESTORE_DRIVER_PARAMETERS +}; + +PROGMEM const settings_t defaults = { + + .version = SETTINGS_VERSION, + + .junction_deviation = DEFAULT_JUNCTION_DEVIATION, + .arc_tolerance = DEFAULT_ARC_TOLERANCE, + .g73_retract = DEFAULT_G73_RETRACT, + + .flags.legacy_rt_commands = DEFAULT_LEGACY_RTCOMMANDS, + .flags.report_inches = DEFAULT_REPORT_INCHES, + .flags.sleep_enable = DEFAULT_SLEEP_ENABLE, +#if DEFAULT_LASER_MODE + .mode = Mode_Laser, + .flags.disable_laser_during_hold = DEFAULT_DISABLE_LASER_DURING_HOLD, +#else + .flags.disable_laser_during_hold = 0, + #if DEFAULT_LATHE_MODE + .mode = Mode_Lathe, + #endif +#endif + .flags.restore_after_feed_hold = DEFAULT_RESTORE_AFTER_FEED_HOLD, + .flags.force_initialization_alarm = DEFAULT_FORCE_INITIALIZATION_ALARM, + + .probe.disable_probe_pullup = DISABLE_PROBE_PIN_PULL_UP, + .probe.allow_feed_override = ALLOW_FEED_OVERRIDE_DURING_PROBE_CYCLES, + .probe.invert_probe_pin = DEFAULT_INVERT_PROBE_PIN, + + .steppers.pulse_microseconds = DEFAULT_STEP_PULSE_MICROSECONDS, + .steppers.pulse_delay_microseconds = DEFAULT_STEP_PULSE_DELAY, + .steppers.idle_lock_time = DEFAULT_STEPPER_IDLE_LOCK_TIME, + .steppers.step_invert.mask = DEFAULT_STEPPING_INVERT_MASK, + .steppers.dir_invert.mask = DEFAULT_DIRECTION_INVERT_MASK, + .steppers.enable_invert.mask = INVERT_ST_ENABLE_MASK, + .steppers.deenergize.mask = ST_DEENERGIZE_MASK, +#if DEFAULT_HOMING_ENABLE + .homing.flags.enabled = DEFAULT_HOMING_ENABLE, + .homing.flags.init_lock = DEFAULT_HOMING_INIT_LOCK, + .homing.flags.single_axis_commands = HOMING_SINGLE_AXIS_COMMANDS, + .homing.flags.force_set_origin = HOMING_FORCE_SET_ORIGIN, + .homing.flags.manual = DEFAULT_HOMING_ALLOW_MANUAL, + .homing.flags.override_locks = DEFAULT_HOMING_OVERRIDE_LOCKS, +#else + .homing.flags.value = 0, +#endif + .homing.dir_mask.value = DEFAULT_HOMING_DIR_MASK, + .homing.feed_rate = DEFAULT_HOMING_FEED_RATE, + .homing.seek_rate = DEFAULT_HOMING_SEEK_RATE, + .homing.debounce_delay = DEFAULT_HOMING_DEBOUNCE_DELAY, + .homing.pulloff = DEFAULT_HOMING_PULLOFF, + .homing.locate_cycles = DEFAULT_N_HOMING_LOCATE_CYCLE, + .homing.cycle[0].mask = HOMING_CYCLE_0, + .homing.cycle[1].mask = HOMING_CYCLE_1, + .homing.cycle[2].mask = HOMING_CYCLE_2, + .homing.dual_axis.fail_length_percent = DUAL_AXIS_HOMING_FAIL_AXIS_LENGTH_PERCENT, + .homing.dual_axis.fail_distance_min = DUAL_AXIS_HOMING_FAIL_DISTANCE_MIN, + .homing.dual_axis.fail_distance_max = DUAL_AXIS_HOMING_FAIL_DISTANCE_MAX, + + .status_report.machine_position = DEFAULT_REPORT_BUFFER_STATE, + .status_report.buffer_state = DEFAULT_REPORT_BUFFER_STATE, + .status_report.line_numbers = DEFAULT_REPORT_LINE_NUMBERS, + .status_report.feed_speed = DEFAULT_REPORT_CURRENT_FEED_SPEED, + .status_report.pin_state = DEFAULT_REPORT_PIN_STATE, + .status_report.work_coord_offset = DEFAULT_REPORT_WORK_COORD_OFFSET, + .status_report.overrides = DEFAULT_REPORT_OVERRIDES, + .status_report.probe_coordinates = DEFAULT_REPORT_PROBE_COORDINATES, + .status_report.sync_on_wco_change = DEFAULT_REPORT_SYNC_ON_WCO_CHANGE, + .status_report.parser_state = DEFAULT_REPORT_PARSER_STATE, + .status_report.alarm_substate = DEFAULT_REPORT_ALARM_SUBSTATE, + + .limits.flags.hard_enabled = DEFAULT_HARD_LIMIT_ENABLE, + .limits.flags.soft_enabled = DEFAULT_SOFT_LIMIT_ENABLE, + .limits.flags.jog_soft_limited = DEFAULT_JOG_LIMIT_ENABLE, + .limits.flags.check_at_init = DEFAULT_CHECK_LIMITS_AT_INIT, + .limits.flags.two_switches = DEFAULT_LIMITS_TWO_SWITCHES_ON_AXES, + .limits.invert.mask = INVERT_LIMIT_PIN_MASK, + .limits.disable_pullup.mask = DISABLE_LIMIT_PINS_PULL_UP_MASK, + + .control_invert.mask = INVERT_CONTROL_PIN_MASK, + .control_disable_pullup.mask = DISABLE_CONTROL_PINS_PULL_UP_MASK, + + .spindle.rpm_max = DEFAULT_SPINDLE_RPM_MAX, + .spindle.rpm_min = DEFAULT_SPINDLE_RPM_MIN, + .spindle.flags.pwm_action = DEFAULT_SPINDLE_PWM_ACTION, + .spindle.invert.on = INVERT_SPINDLE_ENABLE_PIN, + .spindle.invert.ccw = INVERT_SPINDLE_CCW_PIN, + .spindle.invert.pwm = INVERT_SPINDLE_PWM_PIN, + .spindle.pwm_freq = DEFAULT_SPINDLE_PWM_FREQ, + .spindle.pwm_off_value = DEFAULT_SPINDLE_PWM_OFF_VALUE, + .spindle.pwm_min_value = DEFAULT_SPINDLE_PWM_MIN_VALUE, + .spindle.pwm_max_value = DEFAULT_SPINDLE_PWM_MAX_VALUE, + .spindle.at_speed_tolerance = DEFAULT_SPINDLE_AT_SPEED_TOLERANCE, + .spindle.ppr = DEFAULT_SPINDLE_PPR, + .spindle.pid.p_gain = DEFAULT_SPINDLE_P_GAIN, + .spindle.pid.i_gain = DEFAULT_SPINDLE_I_GAIN, + .spindle.pid.d_gain = DEFAULT_SPINDLE_D_GAIN, + .spindle.pid.i_max_error = DEFAULT_SPINDLE_I_MAX, +#if SPINDLE_NPWM_PIECES > 0 + .spindle.pwm_piece[0] = { .rpm = NAN, .start = 0.0f, .end = 0.0f }, +#endif +#if SPINDLE_NPWM_PIECES > 1 + .spindle.pwm_piece[1] = { .rpm = NAN, .start = 0.0f, .end = 0.0f }, +#endif +#if SPINDLE_NPWM_PIECES > 2 + .spindle.pwm_piece[2] = { .rpm = NAN, .start = 0.0f, .end = 0.0f }, +#endif +#if SPINDLE_NPWM_PIECES > 3 + .spindle.pwm_piece[3] = { .rpm = NAN, .start = 0.0f, .end = 0.0f }, +#endif + + .coolant_invert.flood = INVERT_COOLANT_FLOOD_PIN, + .coolant_invert.mist = INVERT_COOLANT_MIST_PIN, + + .axis[X_AXIS].steps_per_mm = DEFAULT_X_STEPS_PER_MM, + .axis[X_AXIS].max_rate = DEFAULT_X_MAX_RATE, + .axis[X_AXIS].acceleration = DEFAULT_X_ACCELERATION, + .axis[X_AXIS].max_travel = (-DEFAULT_X_MAX_TRAVEL), + .axis[X_AXIS].dual_axis_offset = 0.0f, +#ifdef ENABLE_BACKLASH_COMPENSATION + .axis[X_AXIS].backlash = 0.0f, +#endif + + .axis[Y_AXIS].steps_per_mm = DEFAULT_Y_STEPS_PER_MM, + .axis[Y_AXIS].max_rate = DEFAULT_Y_MAX_RATE, + .axis[Y_AXIS].max_travel = (-DEFAULT_Y_MAX_TRAVEL), + .axis[Y_AXIS].acceleration = DEFAULT_Y_ACCELERATION, + .axis[Y_AXIS].dual_axis_offset = 0.0f, +#ifdef ENABLE_BACKLASH_COMPENSATION + .axis[Y_AXIS].backlash = 0.0f, +#endif + + .axis[Z_AXIS].steps_per_mm = DEFAULT_Z_STEPS_PER_MM, + .axis[Z_AXIS].max_rate = DEFAULT_Z_MAX_RATE, + .axis[Z_AXIS].acceleration = DEFAULT_Z_ACCELERATION, + .axis[Z_AXIS].max_travel = (-DEFAULT_Z_MAX_TRAVEL), + .axis[Z_AXIS].dual_axis_offset = 0.0f, +#ifdef ENABLE_BACKLASH_COMPENSATION + .axis[Z_AXIS].backlash = 0.0f, +#endif + +#ifdef A_AXIS + .axis[A_AXIS].steps_per_mm = DEFAULT_A_STEPS_PER_MM, + .axis[A_AXIS].max_rate = DEFAULT_A_MAX_RATE, + .axis[A_AXIS].acceleration = DEFAULT_A_ACCELERATION, + .axis[A_AXIS].max_travel = (-DEFAULT_A_MAX_TRAVEL), + .axis[A_AXIS].dual_axis_offset = 0.0f, +#ifdef ENABLE_BACKLASH_COMPENSATION + .axis[A_AXIS].backlash = 0.0f, +#endif + .homing.cycle[3].mask = HOMING_CYCLE_3, +#endif + +#ifdef B_AXIS + .axis[B_AXIS].steps_per_mm = DEFAULT_B_STEPS_PER_MM, + .axis[B_AXIS].max_rate = DEFAULT_B_MAX_RATE, + .axis[B_AXIS].acceleration = DEFAULT_B_ACCELERATION, + .axis[B_AXIS].max_travel = (-DEFAULT_B_MAX_TRAVEL), + .axis[B_AXIS].dual_axis_offset = 0.0f, +#ifdef ENABLE_BACKLASH_COMPENSATION + .axis[B_AXIS].backlash = 0.0f, +#endif + .homing.cycle[4].mask = HOMING_CYCLE_4, +#endif + +#ifdef C_AXIS + .axis[C_AXIS].steps_per_mm = DEFAULT_C_STEPS_PER_MM, + .axis[C_AXIS].acceleration = DEFAULT_C_ACCELERATION, + .axis[C_AXIS].max_rate = DEFAULT_C_MAX_RATE, + .axis[C_AXIS].max_travel = (-DEFAULT_C_MAX_TRAVEL), + .axis[C_AXIS].dual_axis_offset = 0.0f, +#ifdef ENABLE_BACKLASH_COMPENSATION + .axis[C_AXIS].backlash = 0.0f, +#endif + .homing.cycle[5].mask = HOMING_CYCLE_5, +#endif + + .tool_change.mode = (toolchange_mode_t)DEFAULT_TOOLCHANGE_MODE, + .tool_change.probing_distance = DEFAULT_TOOLCHANGE_PROBING_DISTANCE, + .tool_change.feed_rate = DEFAULT_TOOLCHANGE_FEED_RATE, + .tool_change.seek_rate = DEFAULT_TOOLCHANGE_SEEK_RATE, + .tool_change.pulloff_rate = DEFAULT_TOOLCHANGE_PULLOFF_RATE, + + .parking.flags.enabled = DEFAULT_PARKING_ENABLE, + .parking.flags.deactivate_upon_init = DEFAULT_DEACTIVATE_PARKING_UPON_INIT, + .parking.flags.enable_override_control= DEFAULT_ENABLE_PARKING_OVERRIDE_CONTROL, + .parking.axis = DEFAULT_PARKING_AXIS, + .parking.target = DEFAULT_PARKING_TARGET, + .parking.rate = DEFAULT_PARKING_RATE, + .parking.pullout_rate = DEFAULT_PARKING_PULLOUT_RATE, + .parking.pullout_increment = DEFAULT_PARKING_PULLOUT_INCREMENT +}; + +PROGMEM static const setting_group_detail_t setting_group_detail [] = { + { Group_Root, Group_Root, "Root"}, + { Group_Root, Group_General, "General"}, + { Group_Root, Group_ControlSignals, "Control signals"}, + { Group_Root, Group_Limits, "Limits"}, + { Group_Limits, Group_Limits_DualAxis, "Dual axis"}, + { Group_Root, Group_Coolant, "Coolant"}, + { Group_Root, Group_Spindle, "Spindle"}, + { Group_Spindle, Group_Spindle_Sync, "Spindle sync"}, + { Group_Root, Group_Toolchange, "Tool change"}, + { Group_Root, Group_Homing, "Homing"}, + { Group_Root, Group_Probing, "Probing"}, + { Group_Root, Group_SafetyDoor, "Safety door"}, + { Group_Root, Group_Jogging, "Jogging"}, + { Group_Root, Group_Stepper, "Stepper"}, + { Group_Root, Group_MotorDriver, "Stepper driver"}, + { Group_Root, Group_Axis, "Axis"}, + { Group_Axis, Group_XAxis, "X-axis"}, + { Group_Axis, Group_YAxis, "Y-axis"}, + { Group_Axis, Group_ZAxis, "Z-axis"}, +#ifdef A_AXIS + { Group_Axis, Group_AAxis, "A-axis"}, +#endif +#ifdef B_AXIS + { Group_Axis, Group_BAxis, "B-axis"}, +#endif +#ifdef C_AXIS + { Group_Axis, Group_CAxis, "C-axis"} +#endif +}; + +static status_code_t set_probe_invert (setting_id_t id, uint_fast16_t int_value); +static status_code_t set_report_mask (setting_id_t id, uint_fast16_t int_value); +static status_code_t set_report_inches (setting_id_t id, uint_fast16_t int_value); +static status_code_t set_control_invert (setting_id_t id, uint_fast16_t int_value); +static status_code_t set_spindle_invert (setting_id_t id, uint_fast16_t int_value); +static status_code_t set_control_disable_pullup (setting_id_t id, uint_fast16_t int_value); +static status_code_t set_probe_disable_pullup (setting_id_t id, uint_fast16_t int_value); +static status_code_t set_soft_limits_enable (setting_id_t id, uint_fast16_t int_value); +static status_code_t set_hard_limits_enable (setting_id_t id, uint_fast16_t int_value); +static status_code_t set_jog_soft_limited (setting_id_t id, uint_fast16_t int_value); +static status_code_t set_homing_enable (setting_id_t id, uint_fast16_t int_value); +static status_code_t set_enable_legacy_rt_commands (setting_id_t id, uint_fast16_t int_value); +static status_code_t set_homing_cycle (setting_id_t id, uint_fast16_t int_value); +static status_code_t set_mode (setting_id_t id, uint_fast16_t int_value); +static status_code_t set_sleep_enable (setting_id_t id, uint_fast16_t int_value); +static status_code_t set_hold_actions (setting_id_t id, uint_fast16_t int_value); +static status_code_t set_force_initialization_alarm (setting_id_t id, uint_fast16_t int_value); +static status_code_t set_probe_allow_feed_override (setting_id_t id, uint_fast16_t int_value); +static status_code_t set_tool_change_mode (setting_id_t id, uint_fast16_t int_value); +static status_code_t set_tool_change_probing_distance (setting_id_t id, float value); +#ifdef ENABLE_SAFETY_DOOR_INPUT_PIN +static status_code_t set_parking_enable (setting_id_t id, uint_fast16_t int_value); +static status_code_t set_restore_overrides (setting_id_t id, uint_fast16_t int_value); +static status_code_t set_door_options (setting_id_t id, uint_fast16_t int_value); +#endif +#ifdef ENABLE_SPINDLE_LINEARIZATION +static status_code_t set_linear_piece (setting_id_t id, char *svalue); +static char *get_linear_piece (setting_id_t id); +#endif +static status_code_t set_axis_setting (setting_id_t setting, float value); +static float get_float (setting_id_t setting); +static uint32_t get_int (setting_id_t id); +static bool is_setting_available (const setting_detail_t *setting); + +static char control_signals[] = "Reset,Feed hold,Cycle start,Safety door,Block delete,Optional stop,EStop,Probe connected,Motor fault"; +static char control_signals_map[] = "0,1,2,3,4,5,6,7,8"; +static char spindle_signals[] = "Spindle enable,Spindle direction,PWM"; + +PROGMEM static const setting_detail_t setting_detail[] = { + { Setting_PulseMicroseconds, Group_Stepper, "Step pulse time", "microseconds", Format_Decimal, "#0.0", "2.0", NULL, Setting_IsLegacy, &settings.steppers.pulse_microseconds, NULL, NULL }, + { Setting_StepperIdleLockTime, Group_Stepper, "Step idle delay", "milliseconds", Format_Int16, "####0", NULL, "65535", Setting_IsLegacy, &settings.steppers.idle_lock_time, NULL, NULL }, + { Setting_StepInvertMask, Group_Stepper, "Step pulse invert", NULL, Format_AxisMask, NULL, NULL, NULL, Setting_IsLegacy, &settings.steppers.step_invert.mask, NULL, NULL }, + { Setting_DirInvertMask, Group_Stepper, "Step direction invert", NULL, Format_AxisMask, NULL, NULL, NULL, Setting_IsLegacy, &settings.steppers.dir_invert.mask, NULL, NULL }, + { Setting_InvertStepperEnable, Group_Stepper, "Invert step enable pin(s)", NULL, Format_AxisMask, NULL, NULL, NULL, Setting_IsLegacy, &settings.steppers.enable_invert.mask, NULL, NULL }, + { Setting_LimitPinsInvertMask, Group_Limits, "Invert limit pins", NULL, Format_AxisMask, NULL, NULL, NULL, Setting_IsLegacy, &settings.limits.invert.mask, NULL, NULL }, + { Setting_InvertProbePin, Group_Probing, "Invert probe pin", NULL, Format_Bool, NULL, NULL, NULL, Setting_IsLegacyFn, set_probe_invert, get_int, NULL }, + { Setting_SpindlePWMBehaviour, Group_Spindle, "Disable spindle with zero speed", NULL, Format_Bool, NULL, NULL, NULL, Setting_IsExtended, &settings.spindle.flags.mask, NULL, is_setting_available }, +// { Setting_SpindlePWMBehaviour, Group_Spindle, "Spindle enable vs. speed behaviour", NULL, Format_RadioButtons, "No action,Disable spindle with zero speed,Enable spindle with all speeds", NULL, NULL, Setting_IsExtended, &settings.spindle.flags.mask, NULL, NULL }, +#if COMPATIBILITY_LEVEL <= 1 + { Setting_StatusReportMask, Group_General, "Status report options", NULL, Format_Bitfield, "Position in machine coordinate,Buffer state,Line numbers,Feed & speed,Pin state,Work coordinate offset,Overrides,Probe coordinates,Buffer sync on WCO change,Parser state,Alarm substatus,Run substatus", NULL, NULL, Setting_IsExtendedFn, set_report_mask, get_int, NULL }, +#else + { Setting_StatusReportMask, Group_General, "Status report options", NULL, Format_Bitfield, "Position in machine coordinate,Buffer state", NULL, NULL, Setting_IsLegacyFn, set_report_mask, get_int, NULL }, +#endif + { Setting_JunctionDeviation, Group_General, "Junction deviation", "mm", Format_Decimal, "#####0.000", NULL, NULL, Setting_IsLegacy, &settings.junction_deviation, NULL, NULL }, + { Setting_ArcTolerance, Group_General, "Arc tolerance", "mm", Format_Decimal, "#####0.000", NULL, NULL, Setting_IsLegacy, &settings.arc_tolerance, NULL, NULL }, + { Setting_ReportInches, Group_General, "Report in inches", NULL, Format_Bool, NULL, NULL, NULL, Setting_IsLegacyFn, set_report_inches, get_int, NULL }, + { Setting_ControlInvertMask, Group_ControlSignals, "Invert control pins", NULL, Format_Bitfield, control_signals, control_signals_map, NULL, Setting_IsExpandedFn, set_control_invert, get_int, NULL }, + { Setting_CoolantInvertMask, Group_Coolant, "Invert coolant pins", NULL, Format_Bitfield, "Flood,Mist", NULL, NULL, Setting_IsExtended, &settings.coolant_invert.mask, NULL, NULL }, + { Setting_SpindleInvertMask, Group_Spindle, "Invert spindle signals", NULL, Format_Bitfield, spindle_signals, NULL, NULL, Setting_IsExtendedFn, set_spindle_invert, get_int, NULL }, + { Setting_ControlPullUpDisableMask, Group_ControlSignals, "Pullup disable control pins", NULL, Format_Bitfield, control_signals, control_signals_map, NULL, Setting_IsExtendedFn, set_control_disable_pullup, get_int, NULL }, + { Setting_LimitPullUpDisableMask, Group_Limits, "Pullup disable limit pins", NULL, Format_AxisMask, NULL, NULL, NULL, Setting_IsExtended, &settings.limits.disable_pullup.mask, NULL, NULL }, + { Setting_ProbePullUpDisable, Group_Probing, "Pullup disable probe pin", NULL, Format_Bool, NULL, NULL, NULL, Setting_IsExtendedFn, set_probe_disable_pullup, get_int, NULL }, + { Setting_SoftLimitsEnable, Group_Limits, "Soft limits enable", NULL, Format_Bool, NULL, NULL, NULL, Setting_IsLegacyFn, set_soft_limits_enable, get_int, NULL }, +#if COMPATIBILITY_LEVEL <= 1 + { Setting_HardLimitsEnable, Group_Limits, "Hard limits enable", NULL, Format_XBitfield, "Enable,Strict mode", NULL, NULL, Setting_IsExpandedFn, set_hard_limits_enable, get_int, NULL }, +#else + { Setting_HardLimitsEnable, Group_Limits, "Hard limits enable", NULL, Format_Bool, NULL, NULL, NULL, Setting_IsLegacyFn, set_hard_limits_enable, get_int, NULL }, +#endif +#if COMPATIBILITY_LEVEL <= 1 + { Setting_HomingEnable, Group_Homing, "Homing cycle", NULL, Format_XBitfield, "Enable,Enable single axis commands,Homing on startup required,Set machine origin to 0,Two switches shares one input pin,Allow manual,Override locks,Keep homed status on reset", NULL, NULL, Setting_IsExpandedFn, set_homing_enable, get_int, NULL }, +#else + { Setting_HomingEnable, Group_Homing, "Homing cycle enable", NULL, Format_Bool, NULL, NULL, NULL, Setting_IsLegacyFn, set_homing_enable, get_int, NULL }, +#endif + { Setting_HomingDirMask, Group_Homing, "Homing direction invert", NULL, Format_AxisMask, NULL, NULL, NULL, Setting_IsLegacy, &settings.homing.dir_mask.value, NULL, NULL }, + { Setting_HomingFeedRate, Group_Homing, "Homing locate feed rate", "mm/min", Format_Decimal, "#####0.0", NULL, NULL, Setting_IsLegacy, &settings.homing.feed_rate, NULL, NULL }, + { Setting_HomingSeekRate, Group_Homing, "Homing search seek rate", "mm/min", Format_Decimal, "#####0.0", NULL, NULL, Setting_IsLegacy, &settings.homing.seek_rate, NULL, NULL }, + { Setting_HomingDebounceDelay, Group_Homing, "Homing switch debounce delay", "milliseconds", Format_Int16, "##0", NULL, NULL, Setting_IsLegacy, &settings.homing.debounce_delay, NULL, NULL }, + { Setting_HomingPulloff, Group_Homing, "Homing switch pull-off distance", "mm", Format_Decimal, "#####0.000", NULL, NULL, Setting_IsLegacy, &settings.homing.pulloff, NULL, NULL }, + { Setting_G73Retract, Group_General, "G73 Retract distance", "mm", Format_Decimal, "#####0.000", NULL, NULL, Setting_IsExtended, &settings.g73_retract, NULL, NULL }, + { Setting_PulseDelayMicroseconds, Group_Stepper, "Pulse delay", "microseconds", Format_Decimal, "#0.0", NULL, "10", Setting_IsExtended, &settings.steppers.pulse_delay_microseconds, NULL, NULL }, + { Setting_RpmMax, Group_Spindle, "Maximum spindle speed", "RPM", Format_Decimal, "#####0.000", NULL, NULL, Setting_IsLegacy, &settings.spindle.rpm_max, NULL, is_setting_available }, + { Setting_RpmMin, Group_Spindle, "Minimum spindle speed", "RPM", Format_Decimal, "#####0.000", NULL, NULL, Setting_IsLegacy, &settings.spindle.rpm_min, NULL, is_setting_available }, + { Setting_Mode, Group_General, "Mode of operation", NULL, Format_RadioButtons, "Normal,Laser mode,Lathe mode", NULL, NULL, Setting_IsLegacyFn, set_mode, get_int, NULL }, + { Setting_PWMFreq, Group_Spindle, "Spindle PWM frequency", "Hz", Format_Decimal, "#####0", NULL, NULL, Setting_IsExtended, &settings.spindle.pwm_freq, NULL, is_setting_available }, + { Setting_PWMOffValue, Group_Spindle, "Spindle PWM off value", "percent", Format_Decimal, "##0.0", NULL, "100", Setting_IsExtended, &settings.spindle.pwm_off_value, NULL, is_setting_available }, + { Setting_PWMMinValue, Group_Spindle, "Spindle PWM min value", "percent", Format_Decimal, "##0.0", NULL, "100", Setting_IsExtended, &settings.spindle.pwm_min_value, NULL, is_setting_available }, + { Setting_PWMMaxValue, Group_Spindle, "Spindle PWM max value", "percent", Format_Decimal, "##0.0", NULL, "100", Setting_IsExtended, &settings.spindle.pwm_max_value, NULL, is_setting_available }, + { Setting_StepperDeenergizeMask, Group_Stepper, "Steppers deenergize", NULL, Format_AxisMask, NULL, NULL, NULL, Setting_IsExtended, &settings.steppers.deenergize.mask, NULL, NULL }, + { Setting_SpindlePPR, Group_Spindle, "Spindle pulses per revolution (PPR)", NULL, Format_Int16, "###0", NULL, NULL, Setting_IsExtended, &settings.spindle.ppr, NULL, is_setting_available }, + { Setting_EnableLegacyRTCommands, Group_General, "Enable legacy RT commands", NULL, Format_Bool, NULL, NULL, NULL, Setting_IsExtendedFn, set_enable_legacy_rt_commands, get_int, NULL }, + { Setting_JogSoftLimited, Group_Jogging, "Limit jog commands", NULL, Format_Bool, NULL, NULL, NULL, Setting_IsExtendedFn, set_jog_soft_limited, get_int, NULL }, +#ifdef ENABLE_SAFETY_DOOR_INPUT_PIN + { Setting_ParkingEnable, Group_SafetyDoor, "Parking cycle", NULL, Format_XBitfield, "Enable,Enable parking override control,Deactivate upon init", NULL, NULL, Setting_IsExtendedFn, set_parking_enable, get_int, NULL }, + { Setting_ParkingAxis, Group_SafetyDoor, "Parking axis", NULL, Format_RadioButtons, "X,Y,Z", NULL, NULL, Setting_IsExtended, &settings.parking.axis, NULL, NULL }, +#endif + { Setting_HomingLocateCycles, Group_Homing, "Homing passes", NULL, Format_Int8, "##0", "1", "128", Setting_IsExtended, &settings.homing.locate_cycles, NULL, NULL }, + { Setting_HomingCycle_1, Group_Homing, "Axes homing, first pass", NULL, Format_AxisMask, NULL, NULL, NULL, Setting_IsExtendedFn, set_homing_cycle, get_int, NULL }, + { Setting_HomingCycle_2, Group_Homing, "Axes homing, second pass", NULL, Format_AxisMask, NULL, NULL, NULL, Setting_IsExtendedFn, set_homing_cycle, get_int, NULL }, + { Setting_HomingCycle_3, Group_Homing, "Axes homing, third pass", NULL, Format_AxisMask, NULL, NULL, NULL, Setting_IsExtendedFn, set_homing_cycle, get_int, NULL }, +#ifdef A_AXIS + { Setting_HomingCycle_4, Group_Homing, "Axes homing, fourth pass", NULL, Format_AxisMask, NULL, NULL, NULL, Setting_IsExtendedFn, set_homing_cycle, get_int, NULL }, +#endif +#ifdef B_AXIS + { Setting_HomingCycle_5, Group_Homing, "Axes homing, fifth pass", NULL, Format_AxisMask, NULL, NULL, NULL, Setting_IsExtendedFn, set_homing_cycle, get_int, NULL }, +#endif +#ifdef C_AXIS + { Setting_HomingCycle_6, Group_Homing, "Axes homing, sixth pass", NULL, Format_AxisMask, NULL, NULL, NULL, Setting_IsExtendedFn, set_homing_cycle, get_int, NULL }, +#endif +#ifdef ENABLE_SAFETY_DOOR_INPUT_PIN + { Setting_ParkingPulloutIncrement, Group_SafetyDoor, "Parking pull-out distance", "mm", Format_Decimal, "###0.0", NULL, NULL, Setting_IsExtended, &settings.parking.pullout_increment, NULL, NULL }, + { Setting_ParkingPulloutRate, Group_SafetyDoor, "Parking pull-out rate", "mm/min", Format_Decimal, "###0.0", NULL, NULL, Setting_IsExtended, &settings.parking.pullout_rate, NULL, NULL }, + { Setting_ParkingTarget, Group_SafetyDoor, "Parking target", "mm", Format_Decimal, "-###0.0", "-100000", NULL, Setting_IsExtended, &settings.parking.target, NULL, NULL }, + { Setting_ParkingFastRate, Group_SafetyDoor, "Parking fast rate", "mm/min", Format_Decimal, "###0.0", NULL, NULL, Setting_IsExtended, &settings.parking.rate, NULL, NULL }, + { Setting_RestoreOverrides, Group_General, "Restore overrides", NULL, Format_Bool, NULL, NULL, NULL, Setting_IsExtendedFn, set_restore_overrides, get_int, NULL }, + { Setting_DoorOptions, Group_SafetyDoor, "Safety door options", NULL, Format_Bitfield, "Ignore when idle,Keep coolant state on open", NULL, NULL, Setting_IsExtendedFn, set_door_options, get_int, NULL }, +#endif + { Setting_SleepEnable, Group_General, "Sleep enable", NULL, Format_Bool, NULL, NULL, NULL, Setting_IsExtendedFn, set_sleep_enable, get_int, NULL }, + { Setting_HoldActions, Group_General, "Feed hold actions", NULL, Format_Bitfield, "Disable laser during hold,Restore spindle and coolant state on resume", NULL, NULL, Setting_IsExtendedFn, set_hold_actions, get_int, NULL }, + { Setting_ForceInitAlarm, Group_General, "Force init alarm", NULL, Format_Bool, NULL, NULL, NULL, Setting_IsExtendedFn, set_force_initialization_alarm, get_int, NULL }, + { Setting_ProbingFeedOverride, Group_Probing, "Probing feed override", NULL, Format_Bool, NULL, NULL, NULL, Setting_IsExtendedFn, set_probe_allow_feed_override, get_int, NULL }, +#ifdef ENABLE_SPINDLE_LINEARIZATION + { Setting_LinearSpindlePiece1, Group_Spindle, "Spindle linearisation, first point", NULL, Format_String, "x30", NULL, "30", Setting_IsExtendedFn, set_linear_piece, get_linear_piece, NULL }, + { Setting_LinearSpindlePiece2, Group_Spindle, "Spindle linearisation, second point", NULL, Format_String, "x30", NULL, "30", Setting_IsExtendedFn, set_linear_piece, get_linear_piece, NULL }, + { Setting_LinearSpindlePiece3, Group_Spindle, "Spindle linearisation, third point", NULL, Format_String, "x30", NULL, "30", Setting_IsExtendedFn, set_linear_piece, get_linear_piece, NULL }, + { Setting_LinearSpindlePiece4, Group_Spindle, "Spindle linearisation, fourth point", NULL, Format_String, "x30", NULL, "30", Setting_IsExtendedFn, set_linear_piece, get_linear_piece, NULL }, +#endif + { Setting_SpindlePGain, Group_Spindle_ClosedLoop, "Spindle P-gain", NULL, Format_Decimal, "###0.000", NULL, NULL, Setting_IsExtended, &settings.spindle.pid.p_gain, NULL, NULL }, + { Setting_SpindleIGain, Group_Spindle_ClosedLoop, "Spindle I-gain", NULL, Format_Decimal, "###0.000", NULL, NULL, Setting_IsExtended, &settings.spindle.pid.i_gain, NULL, NULL }, + { Setting_SpindleDGain, Group_Spindle_ClosedLoop, "Spindle D-gain", NULL, Format_Decimal, "###0.000", NULL, NULL, Setting_IsExtended, &settings.spindle.pid.d_gain, NULL, NULL }, + { Setting_SpindleMaxError, Group_Spindle_ClosedLoop, "Spindle PID max error", NULL, Format_Decimal, "###0.000", NULL, NULL, Setting_IsExtended, &settings.spindle.pid.max_error, NULL, NULL }, + { Setting_SpindleIMaxError, Group_Spindle_ClosedLoop, "Spindle PID max I error", NULL, Format_Decimal, "###0.000", NULL, NULL, Setting_IsExtended, &settings.spindle.pid.i_max_error, NULL, NULL }, + { Setting_PositionPGain, Group_Spindle_Sync, "Spindle sync P-gain", NULL, Format_Decimal, "###0.000", NULL, NULL, Setting_IsExtended, &settings.position.pid.p_gain, NULL, NULL }, + { Setting_PositionIGain, Group_Spindle_Sync, "Spindle sync I-gain", NULL, Format_Decimal, "###0.000", NULL, NULL, Setting_IsExtended, &settings.position.pid.i_gain, NULL, NULL }, + { Setting_PositionDGain, Group_Spindle_Sync, "Spindle sync D-gain", NULL, Format_Decimal, "###0.000", NULL, NULL, Setting_IsExtended, &settings.position.pid.d_gain, NULL, NULL }, + { Setting_PositionIMaxError, Group_Spindle_Sync, "Spindle sync PID max I error", NULL, Format_Decimal, "###0.000", NULL, NULL, Setting_IsExtended, &settings.position.pid.i_max_error, NULL, NULL }, + { Setting_AxisStepsPerMM, Group_Axis0, "?-axis travel resolution", "step/mm", Format_Decimal, "#####0.000", NULL, NULL, Setting_IsLegacyFn, set_axis_setting, get_float, NULL }, + { Setting_AxisMaxRate, Group_Axis0, "?-axis maximum rate", "mm/min", Format_Decimal, "#####0.000", NULL, NULL, Setting_IsLegacyFn, set_axis_setting, get_float, NULL }, + { Setting_AxisAcceleration, Group_Axis0, "?-axis acceleration", "mm/sec^2", Format_Decimal, "#####0.000", NULL, NULL, Setting_IsLegacyFn, set_axis_setting, get_float, NULL }, + { Setting_AxisMaxTravel, Group_Axis0, "?-axis maximum travel", "mm", Format_Decimal, "#####0.000", NULL, NULL, Setting_IsLegacyFn, set_axis_setting, get_float, NULL }, +#ifdef ENABLE_BACKLASH_COMPENSATION + { Setting_AxisBacklash, Group_Axis0, "?-axis backlash compensation", "mm", Format_Decimal, "#####0.000", NULL, NULL, Setting_IsExtendedFn, set_axis_setting, get_float, NULL, NULL }, +#endif + { Setting_AxisAutoSquareOffset, Group_Axis0, "?-axis dual axis offset", "mm", Format_Decimal, "-0.000", "-2", "2", Setting_IsExtendedFn, set_axis_setting, get_float, is_setting_available }, + { Setting_SpindleAtSpeedTolerance, Group_Spindle, "Spindle at speed tolerance", "percent", Format_Decimal, "##0.0", NULL, NULL, Setting_IsExtended, &settings.spindle.at_speed_tolerance, NULL, is_setting_available }, + { Setting_ToolChangeMode, Group_Toolchange, "Tool change mode", NULL, Format_RadioButtons, "Normal,Manual touch off,Manual touch off @ G59.3,Automatic touch off @ G59.3,Ignore M6", NULL, NULL, Setting_IsExtendedFn, set_tool_change_mode, get_int, NULL }, + { Setting_ToolChangeProbingDistance, Group_Toolchange, "Tool change probing distance", "mm", Format_Decimal, "#####0.0", NULL, NULL, Setting_IsExtendedFn, set_tool_change_probing_distance, get_float, NULL }, + { Setting_ToolChangeFeedRate, Group_Toolchange, "Tool change locate feed rate", "mm/min", Format_Decimal, "#####0.0", NULL, NULL, Setting_IsExtended, &settings.tool_change.feed_rate, NULL, NULL }, + { Setting_ToolChangeSeekRate, Group_Toolchange, "Tool change search seek rate", "mm/min", Format_Decimal, "#####0.0", NULL, NULL, Setting_IsExtended, &settings.tool_change.seek_rate, NULL, NULL }, + { Setting_ToolChangePulloffRate, Group_Toolchange, "Tool change probe pull-off rate", "mm/min", Format_Decimal, "#####0.0", NULL, NULL, Setting_IsExtended, &settings.tool_change.pulloff_rate, NULL, NULL }, + { Setting_DualAxisLengthFailPercent, Group_Limits_DualAxis, "Dual axis length fail", "percent", Format_Decimal, "##0.0", "0", "100", Setting_IsExtended, &settings.homing.dual_axis.fail_length_percent, NULL, NULL }, + { Setting_DualAxisLengthFailMin, Group_Limits_DualAxis, "Dual axis length fail min", "mm/min", Format_Decimal, "#####0.000", NULL, NULL, Setting_IsExtended, &settings.homing.dual_axis.fail_distance_min, NULL, NULL }, + { Setting_DualAxisLengthFailMax, Group_Limits_DualAxis, "Dual axis length fail max", "mm/min", Format_Decimal, "#####0.000", NULL, NULL, Setting_IsExtended, &settings.homing.dual_axis.fail_distance_max, NULL, NULL } +}; + +static setting_details_t details = { + .groups = setting_group_detail, + .n_groups = sizeof(setting_group_detail) / sizeof(setting_group_detail_t), + .settings = setting_detail, + .n_settings = sizeof(setting_detail) / sizeof(setting_detail_t), + .save = settings_write_global +}; + +setting_details_t *settings_get_details (void) +{ + details.on_get_settings = grbl.on_get_settings; + + return &details; +} + +static status_code_t set_probe_invert (setting_id_t id, uint_fast16_t int_value) +{ + if(!hal.probe.configure) + return Status_SettingDisabled; + + settings.probe.invert_probe_pin = int_value != 0; + hal.probe.configure(false, false); + + return Status_OK; +} + +static status_code_t set_report_mask (setting_id_t id, uint_fast16_t int_value) +{ +#if COMPATIBILITY_LEVEL <= 1 + settings.status_report.mask = int_value; +#else + int_value &= 0b11; + settings.status_report.mask = (settings.status_report.mask & ~0b11) | int_value; +#endif + + return Status_OK; +} + +static status_code_t set_report_inches (setting_id_t id, uint_fast16_t int_value) +{ + settings.flags.report_inches = int_value != 0; + report_init(); + system_flag_wco_change(); // Make sure WCO is immediately updated. + + return Status_OK; +} + +static status_code_t set_control_invert (setting_id_t id, uint_fast16_t int_value) +{ + settings.control_invert.mask = int_value & hal.signals_cap.mask; + + return Status_OK; +} + +static status_code_t set_spindle_invert (setting_id_t id, uint_fast16_t int_value) +{ + settings.spindle.invert.mask = int_value; + if(settings.spindle.invert.pwm && !hal.driver_cap.spindle_pwm_invert) { + settings.spindle.invert.pwm = Off; + return Status_SettingDisabled; + } + + return Status_OK; +} + +static status_code_t set_control_disable_pullup (setting_id_t id, uint_fast16_t int_value) +{ + settings.control_disable_pullup.mask = int_value & hal.signals_cap.mask; + + return Status_OK; +} + +static status_code_t set_probe_disable_pullup (setting_id_t id, uint_fast16_t int_value) +{ + if(!hal.probe.configure) + return Status_SettingDisabled; + + settings.probe.disable_probe_pullup = int_value != 0; + + return Status_OK; +} + +static status_code_t set_soft_limits_enable (setting_id_t id, uint_fast16_t int_value) +{ + if (int_value && !settings.homing.flags.enabled) + return Status_SoftLimitError; + + settings.limits.flags.soft_enabled = int_value != 0; + + return Status_OK; +} + +static status_code_t set_hard_limits_enable (setting_id_t id, uint_fast16_t int_value) +{ + settings.limits.flags.hard_enabled = bit_istrue(int_value, bit(0)); +#if COMPATIBILITY_LEVEL <= 1 + settings.limits.flags.check_at_init = bit_istrue(int_value, bit(1)); +#endif + hal.limits.enable(settings.limits.flags.hard_enabled, false); // Change immediately. NOTE: Nice to have but could be problematic later. + + return Status_OK; +} + +static status_code_t set_jog_soft_limited (setting_id_t id, uint_fast16_t int_value) +{ + if (int_value && !settings.homing.flags.enabled) + return Status_SoftLimitError; + + settings.limits.flags.jog_soft_limited = int_value != 0; + + return Status_OK; +} + +static status_code_t set_homing_enable (setting_id_t id, uint_fast16_t int_value) +{ + if (bit_istrue(int_value, bit(0))) { +#if COMPATIBILITY_LEVEL > 1 + settings.homing.flags.enabled = On; +#else + settings.homing.flags.value = int_value & 0x0F; + settings.limits.flags.two_switches = bit_istrue(int_value, bit(4)); + settings.homing.flags.manual = bit_istrue(int_value, bit(5)); + settings.homing.flags.override_locks = bit_istrue(int_value, bit(6)); + settings.homing.flags.keep_on_reset = bit_istrue(int_value, bit(7)); +#endif + } else { + settings.homing.flags.value = 0; + settings.limits.flags.soft_enabled = Off; // Force disable soft-limits. + settings.limits.flags.jog_soft_limited = Off; + } + + return Status_OK; +} + +static status_code_t set_enable_legacy_rt_commands (setting_id_t id, uint_fast16_t int_value) +{ + settings.flags.legacy_rt_commands = int_value != 0; + + return Status_OK; +} + +static status_code_t set_homing_cycle (setting_id_t id, uint_fast16_t int_value) +{ + settings.homing.cycle[id - Setting_HomingCycle_1].mask = int_value; + limits_set_homing_axes(); + + return Status_OK; +} + +static status_code_t set_mode (setting_id_t id, uint_fast16_t int_value) +{ + switch((machine_mode_t)int_value) { + + case Mode_Standard: + settings.flags.disable_laser_during_hold = 0; + gc_state.modal.diameter_mode = false; + break; + + case Mode_Laser: + if(!hal.driver_cap.variable_spindle) + return Status_SettingDisabledLaser; + if(settings.mode != Mode_Laser) + settings.flags.disable_laser_during_hold = DEFAULT_DISABLE_LASER_DURING_HOLD; + gc_state.modal.diameter_mode = false; + break; + + case Mode_Lathe: + settings.flags.disable_laser_during_hold = 0; + break; + + default: // Mode_Standard + return Status_InvalidStatement; + } + + settings.mode = (machine_mode_t)int_value; + + return Status_OK; +} + +#ifdef ENABLE_SAFETY_DOOR_INPUT_PIN + +static status_code_t set_parking_enable (setting_id_t id, uint_fast16_t int_value) +{ + settings.parking.flags.value = bit_istrue(int_value, bit(0)) ? (int_value & 0x07) : 0; + + return Status_OK; +} + +static status_code_t set_restore_overrides (setting_id_t id, uint_fast16_t int_value) +{ + settings.flags.restore_overrides = int_value != 0;; + + return Status_OK; +} + +static status_code_t set_door_options (setting_id_t id, uint_fast16_t int_value) +{ + settings.flags.safety_door_ignore_when_idle = bit_istrue(int_value, bit(0)); + settings.flags.keep_coolant_state_on_door_open = bit_istrue(int_value, bit(1)); + + return Status_OK; +} + +#endif + +static status_code_t set_sleep_enable (setting_id_t id, uint_fast16_t int_value) +{ + settings.flags.sleep_enable = int_value != 0; + + return Status_OK; +} + +static status_code_t set_hold_actions (setting_id_t id, uint_fast16_t int_value) +{ + settings.flags.disable_laser_during_hold = bit_istrue(int_value, bit(0)); + settings.flags.restore_after_feed_hold = bit_istrue(int_value, bit(1)); + + return Status_OK; +} + +static status_code_t set_force_initialization_alarm (setting_id_t id, uint_fast16_t int_value) +{ + settings.flags.force_initialization_alarm = int_value != 0; + + return Status_OK; +} + +static status_code_t set_probe_allow_feed_override (setting_id_t id, uint_fast16_t int_value) +{ + settings.probe.allow_feed_override = int_value != 0; + + return Status_OK; +} + +static status_code_t set_tool_change_mode (setting_id_t id, uint_fast16_t int_value) +{ + if(!hal.driver_cap.atc && hal.stream.suspend_read && int_value <= ToolChange_Ignore) { +#if COMPATIBILITY_LEVEL > 1 + if((toolchange_mode_t)int_value == ToolChange_Manual_G59_3 || (toolchange_mode_t)int_value == ToolChange_SemiAutomatic) + return Status_InvalidStatement; +#endif + settings.tool_change.mode = (toolchange_mode_t)int_value; + tc_init(); + } else + return Status_InvalidStatement; + + return Status_OK; +} + +static status_code_t set_tool_change_probing_distance (setting_id_t id, float value) +{ + if(hal.driver_cap.atc) + return Status_InvalidStatement; + + settings.tool_change.probing_distance = value; + + return Status_OK; +} + +#ifdef ENABLE_SPINDLE_LINEARIZATION + +static status_code_t set_linear_piece (setting_id_t id, char *svalue) +{ + uint32_t idx = id - Setting_LinearSpindlePiece1; + float rpm, start, end; + + if(svalue[0] == '0' && svalue[1] == '\0') { + settings.spindle.pwm_piece[idx].rpm = NAN; + settings.spindle.pwm_piece[idx].start = + settings.spindle.pwm_piece[idx].end = 0.0f; + } else if(sscanf(svalue, "%f,%f,%f", &rpm, &start, &end) == 3) { + settings.spindle.pwm_piece[idx].rpm = rpm; + settings.spindle.pwm_piece[idx].start = start; + settings.spindle.pwm_piece[idx].end = end; + } else + return Status_InvalidStatement; + + return Status_OK; +} + +static char *get_linear_piece (setting_id_t id) +{ + static char buf[20]; + + uint32_t idx = id - Setting_LinearSpindlePiece1; + + if(isnan(settings.spindle.pwm_piece[idx].rpm)) + strcpy(buf, ftoa(settings.spindle.pwm_piece[idx].rpm, N_DECIMAL_RPMVALUE)); + else { + sprintf(buf, "$%d=%f,%f,%f" ASCII_EOL, (setting_id_t)(Setting_LinearSpindlePiece1 + idx), settings.spindle.pwm_piece[idx].rpm, settings.spindle.pwm_piece[idx].start, settings.spindle.pwm_piece[idx].end); + hal.stream.write(buf); + } + + return buf; +} + +#endif + +inline static setting_id_t normalize_id (setting_id_t id) +{ + if((id > Setting_AxisSettingsBase && id <= Setting_AxisSettingsMax) || + (id > Setting_AxisSettingsBase2 && id <= Setting_AxisSettingsMax2)) + id -= id % AXIS_SETTINGS_INCREMENT; + else if(id > Setting_EncoderSettingsBase && id <= Setting_EncoderSettingsMax) + id = (setting_id_t)(Setting_EncoderSettingsBase + (id % ENCODER_SETTINGS_INCREMENT)); + + return id; +} + +setting_id_t settings_get_axis_base (setting_id_t id, uint_fast8_t *idx) +{ + setting_id_t base = normalize_id(id); + *idx = id - base; + + return *idx < N_AXIS ? base : Setting_SettingsMax; +} + +static status_code_t set_axis_setting (setting_id_t setting, float value) +{ + uint_fast8_t idx; + status_code_t status = Status_OK; + + switch(settings_get_axis_base(setting, &idx)) { + + case Setting_AxisStepsPerMM: + if (hal.max_step_rate && value * settings.axis[idx].max_rate > (float)hal.max_step_rate * 60.0f) + status = Status_MaxStepRateExceeded; + else + settings.axis[idx].steps_per_mm = value; + break; + + case Setting_AxisMaxRate: + if (hal.max_step_rate && value * settings.axis[idx].steps_per_mm > (float)hal.max_step_rate * 60.0f) + status = Status_MaxStepRateExceeded; + else + settings.axis[idx].max_rate = value; + break; + + case Setting_AxisAcceleration: + settings.axis[idx].acceleration = value * 60.0f * 60.0f; // Convert to mm/min^2 for grbl internal use. + break; + + case Setting_AxisMaxTravel: + settings.axis[idx].max_travel = -value; // Store as negative for grbl internal use. + break; + + case Setting_AxisBacklash: +#ifdef ENABLE_BACKLASH_COMPENSATION + settings.axis[idx].backlash = value; +#else + status = Status_SettingDisabled; +#endif + break; + + case Setting_AxisAutoSquareOffset: + if(hal.stepper.get_auto_squared && bit_istrue(hal.stepper.get_auto_squared().mask, bit(idx))) + settings.axis[idx].dual_axis_offset = value; + else + status = Status_SettingDisabled; + break; + + default: + status = Status_SettingDisabled; + break; + } + + return status; +} + +static float get_float (setting_id_t setting) +{ + float value = 0.0f; + + if (setting >= Setting_AxisSettingsBase && setting <= Setting_AxisSettingsMax) { + + uint_fast8_t idx; + + switch(settings_get_axis_base(setting, &idx)) { + + case Setting_AxisStepsPerMM: + value = settings.axis[idx].steps_per_mm; + break; + + case Setting_AxisMaxRate: + value = settings.axis[idx].max_rate; + break; + + case Setting_AxisAcceleration: + value = settings.axis[idx].acceleration / (60.0f * 60.0f); // Convert to mm/min^2 for grbl internal use. + break; + + case Setting_AxisMaxTravel: + value = -settings.axis[idx].max_travel; // Store as negative for grbl internal use. + break; + +#ifdef ENABLE_BACKLASH_COMPENSATION + case Setting_AxisBacklash: + value = settings.axis[idx].backlash; + break; +#endif + + case Setting_AxisAutoSquareOffset: + value = settings.axis[idx].dual_axis_offset; + break; + + default: // for stopping compiler warning + break; + } + } else switch(setting) { + + case Setting_ToolChangeProbingDistance: + value = settings.tool_change.probing_distance; + break; + + default: + break; + } + + return value; +} + +static uint32_t get_int (setting_id_t id) +{ + uint32_t value = 0; + + switch(id) { + + case Setting_Mode: + value = settings.mode; + break; + + case Setting_InvertProbePin: + value = settings.probe.invert_probe_pin; + break; + + case Setting_StatusReportMask: + value = settings.status_report.mask; + break; + + case Setting_ReportInches: + value = settings.flags.report_inches; + break; + + case Setting_ControlInvertMask: + value = settings.control_invert.mask; + break; + + case Setting_SpindleInvertMask: + value = settings.spindle.invert.mask; + break; + + case Setting_ControlPullUpDisableMask: + value = settings.control_disable_pullup.mask; + break; + + case Setting_ProbePullUpDisable: + value = settings.probe.disable_probe_pullup; + break; + + case Setting_SoftLimitsEnable: + value = settings.limits.flags.soft_enabled; + break; + + case Setting_HardLimitsEnable: + value = ((settings.limits.flags.hard_enabled & bit(0)) ? bit(0) | (settings.limits.flags.check_at_init ? bit(1) : 0) : 0); + break; + + case Setting_JogSoftLimited: + value = settings.limits.flags.jog_soft_limited; + break; + + case Setting_HomingEnable: + value = (settings.homing.flags.value & 0x0F) | + (settings.limits.flags.two_switches ? bit(4) : 0) | + (settings.homing.flags.manual ? bit(5) : 0) | + (settings.homing.flags.override_locks ? bit(6) : 0) | + (settings.homing.flags.keep_on_reset ? bit(7) : 0); + break; + + case Setting_EnableLegacyRTCommands: + value = settings.flags.legacy_rt_commands; + break; + + case Setting_ParkingEnable: + value = settings.parking.flags.value; + break; + + case Setting_HomingCycle_1: + case Setting_HomingCycle_2: + case Setting_HomingCycle_3: + case Setting_HomingCycle_4: + case Setting_HomingCycle_5: + case Setting_HomingCycle_6: + value = settings.homing.cycle[id - Setting_HomingCycle_1].mask; + break; + + case Setting_RestoreOverrides: + value = settings.flags.restore_overrides; + break; + + case Setting_DoorOptions: + value = settings.flags.safety_door_ignore_when_idle | (settings.flags.keep_coolant_state_on_door_open << 1) ; + break; + + case Setting_SleepEnable: + value = settings.flags.sleep_enable; + break; + + case Setting_HoldActions: + value = (settings.flags.disable_laser_during_hold ? bit(0) : 0) | (settings.flags.restore_after_feed_hold ? bit(1) : 0); + break; + + case Setting_ForceInitAlarm: + value = settings.flags.force_initialization_alarm; + break; + + case Setting_ProbingFeedOverride: + value = settings.probe.allow_feed_override; + break; + + case Setting_ToolChangeMode: + value = settings.tool_change.mode; + break; + + default: + break; + } + + return value; +} + +inline static uint8_t get_decimal_places (const char *format) +{ + char *dp = format == NULL ? NULL : strchr(format, '.'); + + return dp ? strchr(format, '\0') - dp - 1 : 1; +} + +char *setting_get_value (const setting_detail_t *setting, uint_fast16_t offset) +{ + char *value = NULL; + setting_id_t id = (setting_id_t)(setting->id + offset); + + switch(setting->type) { + + case Setting_NonCore: + case Setting_IsExtended: + case Setting_IsLegacy: + case Setting_IsExpanded: + switch(setting->datatype) { + + case Format_Decimal: + value = ftoa(*((float *)(setting->value)), get_decimal_places(setting->format)); + break; + + case Format_Int8: + case Format_Bool: + case Format_Bitfield: + case Format_XBitfield: + case Format_AxisMask: + case Format_RadioButtons: + value = uitoa(*((uint8_t *)(setting->value))); + break; + + case Format_Int16: + value = uitoa(*((uint16_t *)(setting->value))); + break; + + case Format_Integer: + value = uitoa(*((uint32_t *)(setting->value))); + break; + + case Format_String: + case Format_Password: + case Format_IPv4: + value = ((char *)(setting->value)); + break; + + default: + break; + } + break; + + case Setting_NonCoreFn: + case Setting_IsExtendedFn: + case Setting_IsLegacyFn: + case Setting_IsExpandedFn: + switch(setting->datatype) { + + case Format_Decimal: + value = ftoa(((setting_get_float_ptr)(setting->get_value))(id), get_decimal_places(setting->format)); + break; + + case Format_String: + case Format_Password: + case Format_IPv4: + value = ((setting_get_string_ptr)(setting->get_value))(id); + break; + + default: + value = uitoa(((setting_get_int_ptr)(setting->get_value))(id)); + break; + } + break; + } + + return value; +} + +static bool is_setting_available (const setting_detail_t *setting) +{ + bool available = false; // settings_is_group_available(setting->group); + + switch(normalize_id(setting->id)) { + + case Setting_SpindlePWMBehaviour: + available = hal.driver_cap.variable_spindle; + break; + + case Setting_SpindlePPR: + available = hal.driver_cap.spindle_sync || hal.driver_cap.spindle_pid; + break; + + case Setting_SpindleAtSpeedTolerance: + available = hal.driver_cap.spindle_at_speed; + break; + + case Setting_RpmMax: + case Setting_RpmMin: + case Setting_PWMFreq: + case Setting_PWMOffValue: + case Setting_PWMMinValue: + case Setting_PWMMaxValue: + available = hal.driver_cap.variable_spindle; + break; + + case Setting_AxisAutoSquareOffset: + available = hal.stepper.get_auto_squared != NULL; + break; + + default: + break; + } + + return available; +} + +// Write build info to persistent storage +void settings_write_build_info (char *line) +{ + if(hal.nvs.type != NVS_None) + hal.nvs.memcpy_to_nvs(NVS_ADDR_BUILD_INFO, (uint8_t *)line, sizeof(stored_line_t), true); +} + +// Read build info from persistent storage. +bool settings_read_build_info(char *line) +{ + if (!(hal.nvs.type != NVS_None && hal.nvs.memcpy_from_nvs((uint8_t *)line, NVS_ADDR_BUILD_INFO, sizeof(stored_line_t), true) == NVS_TransferResult_OK)) { + // Reset line with default value + line[0] = 0; // Empty line + settings_write_build_info(line); + return false; + } + return true; +} + +// Write startup line to persistent storage +void settings_write_startup_line (uint8_t idx, char *line) +{ + assert(idx < N_STARTUP_LINE); + +#ifdef FORCE_BUFFER_SYNC_DURING_NVS_WRITE + protocol_buffer_synchronize(); // A startup line may contain a motion and be executing. +#endif + + if(hal.nvs.type != NVS_None) + hal.nvs.memcpy_to_nvs(NVS_ADDR_STARTUP_BLOCK + idx * (sizeof(stored_line_t) + NVS_CRC_BYTES), (uint8_t *)line, sizeof(stored_line_t), true); +} + +// Read startup line to persistent storage. +bool settings_read_startup_line (uint8_t idx, char *line) +{ + assert(idx < N_STARTUP_LINE); + + if (!(hal.nvs.type != NVS_None && hal.nvs.memcpy_from_nvs((uint8_t *)line, NVS_ADDR_STARTUP_BLOCK + idx * (sizeof(stored_line_t) + NVS_CRC_BYTES), sizeof(stored_line_t), true) == NVS_TransferResult_OK)) { + // Reset line with default value + *line = '\0'; // Empty line + settings_write_startup_line(idx, line); + return false; + } + return true; +} + +// Write selected coordinate data to persistent storage. +void settings_write_coord_data (coord_system_id_t id, float (*coord_data)[N_AXIS]) +{ + assert(id <= N_CoordinateSystems); + +#ifdef FORCE_BUFFER_SYNC_DURING_NVS_WRITE + protocol_buffer_synchronize(); +#endif + + if(hal.nvs.type != NVS_None) + hal.nvs.memcpy_to_nvs(NVS_ADDR_PARAMETERS + id * (sizeof(coord_data_t) + NVS_CRC_BYTES), (uint8_t *)coord_data, sizeof(coord_data_t), true); +} + +// Read selected coordinate data from persistent storage. +bool settings_read_coord_data (coord_system_id_t id, float (*coord_data)[N_AXIS]) +{ + assert(id <= N_CoordinateSystems); + + if (!(hal.nvs.type != NVS_None && hal.nvs.memcpy_from_nvs((uint8_t *)coord_data, NVS_ADDR_PARAMETERS + id * (sizeof(coord_data_t) + NVS_CRC_BYTES), sizeof(coord_data_t), true) == NVS_TransferResult_OK)) { + // Reset with default zero vector + memset(coord_data, 0, sizeof(coord_data_t)); + settings_write_coord_data(id, coord_data); + return false; + } + return true; +} + +// Write selected tool data to persistent storage. +bool settings_write_tool_data (tool_data_t *tool_data) +{ +#ifdef N_TOOLS + assert(tool_data->tool > 0 && tool_data->tool <= N_TOOLS); // NOTE: idx 0 is a non-persistent entry for tools not in tool table + + if(hal.nvs.type != NVS_None) + hal.nvs.memcpy_to_nvs(NVS_ADDR_TOOL_TABLE + (tool_data->tool - 1) * (sizeof(tool_data_t) + NVS_CRC_BYTES), (uint8_t *)tool_data, sizeof(tool_data_t), true); + + return true; +#else + return false; +#endif +} + +// Read selected tool data from persistent storage. +bool settings_read_tool_data (uint32_t tool, tool_data_t *tool_data) +{ +#ifdef N_TOOLS + assert(tool > 0 && tool <= N_TOOLS); // NOTE: idx 0 is a non-persistent entry for tools not in tool table + + if (!(hal.nvs.type != NVS_None && hal.nvs.memcpy_from_nvs((uint8_t *)tool_data, NVS_ADDR_TOOL_TABLE + (tool - 1) * (sizeof(tool_data_t) + NVS_CRC_BYTES), sizeof(tool_data_t), true) == NVS_TransferResult_OK && tool_data->tool == tool)) { + memset(tool_data, 0, sizeof(tool_data_t)); + tool_data->tool = tool; + } + + return tool_data->tool == tool; +#else + return false; +#endif +} + +// Read Grbl global settings from persistent storage. +// Checks version-byte of non-volatile storage and global settings copy. +bool read_global_settings () +{ + bool ok = hal.nvs.type != NVS_None && SETTINGS_VERSION == hal.nvs.get_byte(0) && hal.nvs.memcpy_from_nvs((uint8_t *)&settings, NVS_ADDR_GLOBAL, sizeof(settings_t), true) == NVS_TransferResult_OK; + + return ok && settings.version == SETTINGS_VERSION; +} + + +// Write Grbl global settings and version number to persistent storage +void settings_write_global (void) +{ + if(hal.nvs.type != NVS_None) { + hal.nvs.put_byte(0, SETTINGS_VERSION); + hal.nvs.memcpy_to_nvs(NVS_ADDR_GLOBAL, (uint8_t *)&settings, sizeof(settings_t), true); + } +} + + +// Restore Grbl global settings to defaults and write to persistent storage +void settings_restore (settings_restore_t restore) +{ + uint_fast8_t idx; + stored_line_t empty_line; + + memset(empty_line, 0xFF, sizeof(stored_line_t)); + *empty_line = '\0'; + + if (restore.defaults) { + memcpy(&settings, &defaults, sizeof(settings_t)); + + settings.control_invert.mask &= hal.signals_cap.mask; + settings.spindle.invert.ccw &= hal.driver_cap.spindle_dir; + settings.spindle.invert.pwm &= hal.driver_cap.spindle_pwm_invert; + + settings_write_global(); + } + + if (restore.parameters) { + float coord_data[N_AXIS]; + + memset(coord_data, 0, sizeof(coord_data)); + for (idx = 0; idx <= N_WorkCoordinateSystems; idx++) + settings_write_coord_data((coord_system_id_t)idx, &coord_data); + + settings_write_coord_data(CoordinateSystem_G92, &coord_data); // Clear G92 offsets + +#ifdef N_TOOLS + tool_data_t tool_data; + memset(&tool_data, 0, sizeof(tool_data_t)); + for (idx = 1; idx <= N_TOOLS; idx++) { + tool_data.tool = idx; + settings_write_tool_data(&tool_data); + } +#endif + } + + if (restore.startup_lines) { + for (idx = 0; idx < N_STARTUP_LINE; idx++) + settings_write_startup_line(idx, empty_line); + } + + if (restore.build_info) { + settings_write_build_info(empty_line); + settings_write_build_info(BUILD_INFO); + } + + setting_details_t *details = settings_get_details(); + + while(details->on_get_settings) { + details = details->on_get_settings(); + if(details->restore) + details->restore(); + }; + + nvs_buffer_sync_physical(); +} + +inline static bool is_available (const setting_detail_t *setting) +{ + return setting->is_available == NULL || setting->is_available(setting); +} + +bool settings_is_group_available (setting_group_t group) +{ + bool available = false; + + switch(group) { + + case Group_Probing: + available = hal.probe.get_state != NULL; + break; + + case Group_Encoders: + case Group_Encoder0: + available = hal.encoder.get_n_encoders && hal.encoder.get_n_encoders() > 0; + break; + + case Group_Spindle_Sync: + available = hal.driver_cap.spindle_sync; + break; + + case Group_Spindle_ClosedLoop: + available = hal.driver_cap.spindle_pid; + break; + + case Group_Limits_DualAxis: + available = hal.stepper.get_auto_squared != NULL; + break; + + case Group_General: + case Group_Homing: + case Group_Jogging: + case Group_Limits: + case Group_ControlSignals: + case Group_Spindle: + case Group_Axis: + case Group_XAxis: + case Group_YAxis: + case Group_ZAxis: + #ifdef A_AXIS + case Group_AAxis: + #endif + #ifdef B_AXIS + case Group_BAxis: + #endif + #ifdef C_AXIS + case Group_CAxis: + #endif + available = true; + break; + + default: + { + uint_fast16_t idx; + setting_details_t *details = settings_get_details(); + do { + if(details->settings) { + for(idx = 0; idx < details->n_settings; idx++) { + if(details->settings[idx].group == group && is_available(&details->settings[idx])) { + available = true; + break; + } + } + } + details = !available && details->on_get_settings ? details->on_get_settings() : NULL; + } while(details); + } + break; + } + + return available; +} + +setting_group_t settings_normalize_group (setting_group_t group) +{ + return (group > Group_Axis0 && group < Group_Axis0 + N_AXIS) ? Group_Axis0 : group; +} + +/* +setting_group_t settings_get_parent_group (setting_group_t group) +{ + uint_fast16_t idx; + setting_details_t *settings = settings_get_details(); + + for(idx = 0; idx < settings->n_groups; idx++) { + if(settings->groups[idx].id == group) { + group = settings->groups[idx].parent; + break; + } + } + + return group; +} +*/ + +bool settings_iterator (const setting_detail_t *setting, setting_output_ptr callback, void *data) +{ + bool ok = false; + + switch(setting->id) { + + case Setting_AxisStepsPerMM: + case Setting_AxisMaxRate: + case Setting_AxisAcceleration: + case Setting_AxisMaxTravel: + case Setting_AxisStepperCurrent: + case Setting_AxisMicroSteps: + case Setting_AxisBacklash: + case Setting_AxisAutoSquareOffset: + case Setting_AxisExtended0: + case Setting_AxisExtended1: + case Setting_AxisExtended2: + case Setting_AxisExtended3: + case Setting_AxisExtended4: + case Setting_AxisExtended5: + case Setting_AxisExtended6: + case Setting_AxisExtended7: + case Setting_AxisExtended8: + case Setting_AxisExtended9: + { + uint_fast8_t axis_idx = 0; + for(axis_idx = 0; axis_idx < N_AXIS; axis_idx++) { + if(callback(setting, axis_idx, data)) + ok = true; + } + } + break; + + case Setting_EncoderModeBase: + case Setting_EncoderCPRBase: + case Setting_EncoderCPDBase: + case Setting_EncoderDblClickWindowBase: + { + uint_fast8_t encoder_idx = 0, n_encoders = hal.encoder.get_n_encoders(); + for(encoder_idx = 0; encoder_idx < n_encoders; encoder_idx++) { + if(callback(setting, encoder_idx * ENCODER_SETTINGS_INCREMENT, data)) + ok = true; + } + } + break; + + default: + ok = callback(setting, 0, data); + break; + } + + return ok; +} + +const setting_detail_t *setting_get_details (setting_id_t id, setting_details_t **set) +{ + uint_fast16_t idx, offset = id - normalize_id(id); + setting_details_t *details = settings_get_details(); + + id -= offset; + + do { + for(idx = 0; idx < details->n_settings; idx++) { + if(details->settings[idx].id == id && is_available(&details->settings[idx])) { + if(offset && offset >= (details->settings[idx].group == Group_Encoder0 ? hal.encoder.get_n_encoders() : N_AXIS)) + return NULL; + if(set) + *set = details; + return &details->settings[idx]; + } + } + details = details->on_get_settings ? details->on_get_settings() : NULL; + } while(details); + + return NULL; +} + +static status_code_t validate_value (const setting_detail_t *setting, float value) +{ + float val; + uint_fast8_t set_idx = 0; + + if(setting->min_value) { + if(!read_float((char *)setting->min_value, &set_idx, &val)) + return Status_BadNumberFormat; + + if(value < val) + return Status_SettingValueOutOfRange; + + } else if(value < 0.0f) + return Status_NegativeValue; + + if (setting->max_value) { + set_idx = 0; + + if(!read_float((char *)setting->max_value, &set_idx, &val)) + return Status_BadNumberFormat; + + if(value > val) + return Status_SettingValueOutOfRange; + } + + return Status_OK; +} + +static uint32_t strnumentries (const char *s, const char delimiter) +{ + if(s == NULL || *s == '\0') + return 0; + + char *p = (char *)s; + uint32_t entries = 1; + + while((p = strchr(p, delimiter))) { + p++; + entries++; + } + + return entries; +} + +setting_datatype_t setting_datatype_to_external (setting_datatype_t datatype) +{ + switch(datatype) { + + case Format_Int8: + case Format_Int16: + datatype = Format_Integer; + break; + + default: + break; + } + + return datatype; +} + +bool setting_is_list (const setting_detail_t *setting) +{ + return setting->datatype == Format_Bitfield || setting->datatype == Format_XBitfield || setting->datatype == Format_RadioButtons; +} + +static char *remove_element (char *s, uint_fast8_t entry) +{ + if(entry) { + while(*s && entry) { + if(*s == ',' && --entry == 0) + *s = '\0'; + else + s++; + } + } + + if(entry == 0) { + char *s2 = s + 1; + if(*s2 == ',') + s2++; + else while(*s2 && *s2 != ',') + s2++; + while(*s2) + *s++ = *s2++; + *s = '\0'; + } + + return s; +} + +static void setting_remove_element (setting_id_t id, uint_fast8_t pos) +{ + const setting_detail_t *setting = setting_get_details(id, NULL); + + if(setting && setting_is_list(setting)) { + remove_element((char *)setting->format, pos); + if(setting->min_value) + remove_element((char *)setting->min_value, pos); + } +} + +inline static bool setting_is_string (setting_datatype_t datatype) +{ + return datatype == Format_String || datatype == Format_Password || datatype == Format_IPv4; +} + +inline static bool setting_is_core (setting_type_t type) +{ + return !(type == Setting_NonCore || type == Setting_NonCoreFn); +} +/* +static uint32_t get_mask (const char *bits) +{ + uint32_t mask = 0; + uint_fast8_t set_idx = 0; + float value; + + while(read_float((char *)bits, &set_idx, &value)) { + mask |= (1 << (uint8_t)value); + if(bits[set_idx] == ',') + set_idx++; + } + + return mask; +} +*/ +status_code_t setting_validate_me (const setting_detail_t *setting, float value, char *svalue) +{ + status_code_t status = Status_OK; + + switch(setting->datatype) { + + case Format_Bool: + if(!(value == 0.0f || value == 1.0f)) + status = Status_SettingValueOutOfRange; + break; + + case Format_Bitfield: + case Format_XBitfield:; + if(!(isintf(value) && /* (setting->min_value + ? (((uint32_t)value & ~get_mask(setting->min_value)) == 0) + : */ ((uint32_t)value < (1UL << strnumentries(setting->format, ','))))) //) + status = Status_SettingValueOutOfRange; + break; + + case Format_RadioButtons: + if(!(isintf(value) && (uint32_t)value < strnumentries(setting->format, ','))) + status = Status_SettingValueOutOfRange; + break; + + case Format_AxisMask: + if(!(isintf(value) && (uint32_t)value < (1 << N_AXIS))) + status = Status_SettingValueOutOfRange; + break; + + case Format_Int8: + case Format_Int16: + case Format_Integer: + case Format_Decimal: + status = validate_value(setting, value); + if(setting->datatype == Format_Integer && status == Status_OK && !isintf(value)) + status = Status_BadNumberFormat; + break; + + case Format_String: + case Format_Password: + { + uint_fast16_t len = strlen(svalue); + status = validate_value(setting, (float)len); + } + break; + + case Format_IPv4: + // handled by driver or plugin, dependent on network library + break; + } + + return status; +} + +status_code_t setting_validate (setting_id_t id, float value, char *svalue) +{ + const setting_detail_t *setting = setting_get_details(id, NULL); + + // If no details available setting could nevertheless be a valid setting id. + return setting == NULL ? Status_OK : setting_validate_me(setting, value, svalue); +} + +// A helper method to set settings from command line +status_code_t settings_store_setting (setting_id_t id, char *svalue) +{ + uint_fast8_t set_idx = 0; + float value = NAN; + status_code_t status = Status_OK; + setting_details_t *set; + const setting_detail_t *setting = setting_get_details(id, &set); + + if(setting == NULL) + return Status_SettingDisabled; + + // Trim leading spaces + while(*svalue == ' ') + svalue++; + + if(!setting_is_string(setting->datatype) && !read_float(svalue, &set_idx, &value) && setting_is_core(setting->type)) + return Status_BadNumberFormat; + + if((status = setting_validate_me(setting, value, svalue)) != Status_OK) { + if(setting == Setting_PulseMicroseconds && status == Status_SettingValueOutOfRange) + status = Status_SettingStepPulseMin; + + return status; + } + + switch(setting->type) { + + case Setting_NonCore: + case Setting_IsExtended: + case Setting_IsLegacy: + case Setting_IsExpanded: + switch(setting->datatype) { + + case Format_Decimal: + *((float *)(setting->value)) = value; + break; + + case Format_String: + case Format_Password: + strcpy(((char *)(setting->value)), svalue); + break; + + case Format_AxisMask: + *((uint8_t *)(setting->value)) = (uint8_t)truncf(value) & AXES_BITMASK; + break; + + case Format_Int8: + case Format_Bool: + case Format_Bitfield: + case Format_XBitfield: + case Format_RadioButtons: + *((uint8_t *)(setting->value)) = (uint8_t)truncf(value); + break; + + case Format_Int16: + *((uint16_t *)(setting->value)) = (uint16_t)truncf(value); + break; + + case Format_Integer: + *((uint32_t *)(setting->value)) = (uint32_t)truncf(value); + break; + + default: + status = Status_BadNumberFormat; + break; + } + break; + + case Setting_NonCoreFn: + case Setting_IsExtendedFn: + case Setting_IsLegacyFn: + case Setting_IsExpandedFn: + switch(setting->datatype) { + + case Format_Decimal: + status = ((setting_set_float_ptr)(setting->value))(id, value); + break; + + case Format_String: + case Format_Password: + case Format_IPv4: + status = ((setting_set_string_ptr)(setting->value))(id, svalue); + break; + + default: + status = ((setting_set_int_ptr)(setting->value))(id, (uint_fast16_t)truncf(value)); + break; + } + break; + } + + if(status == Status_OK) { + if(set->save) + set->save(); +#ifdef ENABLE_BACKLASH_COMPENSATION + mc_backlash_init(); +#endif + if(set->on_changed) + set->on_changed(&settings); + } + + return status; +} + +// Initialize the config subsystem +void settings_init (void) +{ + if(!read_global_settings()) { + settings_restore_t settings = settings_all; + settings.defaults = 1; // Ensure global settings get restored + if(hal.nvs.type != NVS_None) + grbl.report.status_message(Status_SettingReadFail); + settings_restore(settings); // Force restore all non-volatile storage data. + report_init(); +#if COMPATIBILITY_LEVEL <= 1 + report_grbl_settings(true, NULL); +#else + report_grbl_settings(false, NULL); +#endif + } else { + memset(&tool_table, 0, sizeof(tool_data_t)); // First entry is for tools not in tool table +#ifdef N_TOOLS + uint_fast8_t idx; + for (idx = 1; idx <= N_TOOLS; idx++) + settings_read_tool_data(idx, &tool_table[idx]); +#endif + report_init(); +#ifdef ENABLE_BACKLASH_COMPENSATION + mc_backlash_init(); +#endif + hal.settings_changed(&settings); + if(hal.probe.configure) // Initialize probe invert mask. + hal.probe.configure(false, false); + } + + if(!hal.driver_cap.spindle_pwm_invert) + setting_remove_element(Setting_SpindleInvertMask, 2); + + if(!hal.signals_cap.motor_fault) + setting_remove_element(Setting_ControlInvertMask, 8); +/* TODO + if(!hal.signals_cap.probe_disconnected) + setting_remove_element(Setting_ControlInvertMask, 7); + + if(!hal.signals_cap.e_stop) + setting_remove_element(Setting_ControlInvertMask, 6); + + if(!hal.signals_cap.stop_disable) + setting_remove_element(Setting_ControlInvertMask, 5); + + if(!hal.signals_cap.block_delete) + setting_remove_element(Setting_ControlInvertMask, 4); + + if(!hal.signals_cap.safety_door_ajar) + setting_remove_element(Setting_ControlInvertMask, 3); +*/ + + setting_details_t *details = settings_get_details(); + + while(details->on_get_settings) { + details = details->on_get_settings(); + if(details->load) + details->load(); + if(details->on_changed) + details->on_changed(&settings); + }; + + settings_get_details()->on_changed = hal.settings_changed; +} diff --git a/settings.h b/settings.h new file mode 100644 index 0000000..f62a601 --- /dev/null +++ b/settings.h @@ -0,0 +1,721 @@ +/* + settings.h - non-volatile storage configuration handling + + Part of grblHAL + + Copyright (c) 2017-2021 Terje Io + Copyright (c) 2011-2016 Sungeun K. Jeon for Gnea Research LLC + Copyright (c) 2009-2011 Simen Svale Skogsrud + + 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 . +*/ + +#ifndef _SETTINGS_H_ +#define _SETTINGS_H_ + +#include "config.h" +#include "system.h" +#include "plugins.h" + +// Version of the persistent storage data. Will be used to migrate existing data from older versions of Grbl +// when firmware is upgraded. Always stored in byte 0 of non-volatile storage +#define SETTINGS_VERSION 19 // NOTE: Check settings_reset() when moving to next version. + +// Define axis settings numbering scheme. Starts at Setting_AxisSettingsBase, every INCREMENT, over N_SETTINGS. +#define AXIS_SETTINGS_INCREMENT 10 // Must be greater than the number of axis settings. + +// Define encoder settings numbering scheme. Starts at Setting_EncoderSettingsBase, every INCREMENT, over N_SETTINGS. +// Not referenced by the core. +#define ENCODER_N_SETTINGS_MAX 5 // NOTE: This is the maximum number of encoders allowed. +#define ENCODER_SETTINGS_INCREMENT 10 + +typedef enum { + Setting_PulseMicroseconds = 0, + Setting_StepperIdleLockTime = 1, + Setting_StepInvertMask = 2, + Setting_DirInvertMask = 3, + Setting_InvertStepperEnable = 4, + Setting_LimitPinsInvertMask = 5, + Setting_InvertProbePin = 6, + Setting_SpindlePWMBehaviour = 7, + Setting_StatusReportMask = 10, + Setting_JunctionDeviation = 11, + Setting_ArcTolerance = 12, + Setting_ReportInches = 13, + Setting_ControlInvertMask = 14, + Setting_CoolantInvertMask = 15, + Setting_SpindleInvertMask = 16, + Setting_ControlPullUpDisableMask = 17, + Setting_LimitPullUpDisableMask = 18, + Setting_ProbePullUpDisable = 19, + Setting_SoftLimitsEnable = 20, + Setting_HardLimitsEnable = 21, + Setting_HomingEnable = 22, + Setting_HomingDirMask = 23, + Setting_HomingFeedRate = 24, + Setting_HomingSeekRate = 25, + Setting_HomingDebounceDelay = 26, + Setting_HomingPulloff = 27, + Setting_G73Retract = 28, + Setting_PulseDelayMicroseconds = 29, + Setting_RpmMax = 30, + Setting_RpmMin = 31, + Setting_Mode = 32, + Setting_PWMFreq = 33, + Setting_PWMOffValue = 34, + Setting_PWMMinValue = 35, + Setting_PWMMaxValue = 36, + Setting_StepperDeenergizeMask = 37, + Setting_SpindlePPR = 38, + Setting_EnableLegacyRTCommands = 39, + Setting_JogSoftLimited = 40, + Setting_ParkingEnable = 41, + Setting_ParkingAxis = 42, + + Setting_HomingLocateCycles = 43, + Setting_HomingCycle_1 = 44, + Setting_HomingCycle_2 = 45, + Setting_HomingCycle_3 = 46, + Setting_HomingCycle_4 = 47, + Setting_HomingCycle_5 = 48, + Setting_HomingCycle_6 = 49, +// Optional driver implemented settings for jogging + Setting_JogStepSpeed = 50, + Setting_JogSlowSpeed = 51, + Setting_JogFastSpeed = 52, + Setting_JogStepDistance = 53, + Setting_JogSlowDistance = 54, + Setting_JogFastDistance = 55, +// + Setting_ParkingPulloutIncrement = 56, + Setting_ParkingPulloutRate = 57, + Setting_ParkingTarget = 58, + Setting_ParkingFastRate = 59, + + Setting_RestoreOverrides = 60, + Setting_DoorOptions = 61, + Setting_SleepEnable = 62, + Setting_HoldActions = 63, + Setting_ForceInitAlarm = 64, + Setting_ProbingFeedOverride = 65, +// Optional driver implemented settings for piecewise linear spindle PWM algorithm + Setting_LinearSpindlePiece1 = 66, + Setting_LinearSpindlePiece2 = 67, + Setting_LinearSpindlePiece3 = 68, + Setting_LinearSpindlePiece4 = 69, +// + +// Optional driver implemented settings for additional streams + Setting_NetworkServices = 70, + Setting_BlueToothDeviceName = 71, + Setting_BlueToothServiceName = 72, + Setting_WifiMode = 73, + Setting_WiFi_STA_SSID = 74, + Setting_WiFi_STA_Password = 75, + Setting_WiFi_AP_SSID = 76, + Setting_WiFi_AP_Password = 77, + Setting_Wifi_AP_Country = 78, + Setting_Wifi_AP_Channel = 79, +// + +// Optional settings for closed loop spindle speed control + Setting_SpindlePGain = 80, + Setting_SpindleIGain = 81, + Setting_SpindleDGain = 82, + Setting_SpindleDeadband = 83, + Setting_SpindleMaxError = 84, + Setting_SpindleIMaxError = 85, + Setting_SpindleDMaxError = 86, + +// Optional settings for closed loop spindle synchronized motion + Setting_PositionPGain = 90, + Setting_PositionIGain = 91, + Setting_PositionDGain = 92, + Setting_PositionDeadband = 93, + Setting_PositionMaxError = 94, + Setting_PositionIMaxError = 95, + Setting_PositionDMaxError = 96, +// + +// Reserving settings in the range 100 - 299 for axis settings. + Setting_AxisSettingsBase = 100, // Reserved for core settings + Setting_AxisSettingsMax = Setting_AxisSettingsBase + AXIS_SETTINGS_INCREMENT * 7 + N_AXIS, + Setting_AxisSettingsBase2 = 200, // Reserved for driver/plugin settings + Setting_AxisSettingsMax2 = Setting_AxisSettingsBase2 + AXIS_SETTINGS_INCREMENT * 9 + N_AXIS, +// + +// Optional driver implemented settings + + // Normally used for Ethernet or WiFi Station + Setting_Hostname = 300, + Setting_IpMode = 301, + Setting_IpAddress = 302, + Setting_Gateway = 303, + Setting_NetMask = 304, + Setting_TelnetPort = 305, + Setting_HttpPort = 306, + Setting_WebSocketPort = 307, + + // Normally used for WiFi Access Point + Setting_Hostname2 = 310, + Setting_IpMode2 = 311, + Setting_IpAddress2 = 312, + Setting_Gateway2 = 313, + Setting_NetMask2 = 314, + Setting_TelnetPort2 = 315, + Setting_HttpPort2 = 316, + Setting_WebSocketPort2 = 317, + + Setting_Hostname3 = 320, + Setting_IpMode3 = 321, + Setting_IpAddress3 = 322, + Setting_Gateway3 = 323, + Setting_NetMask3 = 324, + Setting_TelnetPort3 = 325, + Setting_HttpPort3 = 326, + Setting_WebSocketPort3 = 327, + + Setting_AdminPassword = 330, + Setting_UserPassword = 331, + + Setting_TrinamicDriver = 338, + Setting_TrinamicHoming = 339, + + Setting_SpindleAtSpeedTolerance = 340, + Setting_ToolChangeMode = 341, + Setting_ToolChangeProbingDistance = 342, + Setting_ToolChangeFeedRate = 343, + Setting_ToolChangeSeekRate = 344, + Setting_ToolChangePulloffRate = 345, + + Setting_DualAxisLengthFailPercent = 347, + Setting_DualAxisLengthFailMin = 348, + Setting_DualAxisLengthFailMax = 349, + + Setting_THC_Mode = 350, + Setting_THC_Delay = 351, + Setting_THC_Threshold = 352, + Setting_THC_PGain = 353, + Setting_THC_IGain = 354, + Setting_THC_DGain = 355, + Setting_THC_VADThreshold = 356, + Setting_THC_VoidOverride = 357, + Setting_Arc_FailTimeout = 358, + Setting_Arc_RetryDelay = 359, + Setting_Arc_MaxRetries = 360, + Setting_Arc_VoltageScale = 361, + Setting_Arc_VoltageOffset = 362, + Setting_Arc_HeightPerVolt = 363, + Setting_Arc_OkHighVoltage = 364, + Setting_Arc_OkLowVoltage = 365, + + Settings_IoPort_InvertIn = 370, + Settings_IoPort_Pullup_Disable = 371, + Settings_IoPort_InvertOut = 372, + Settings_IoPort_OD_Enable = 373, + Settings_ModBus_BaudRate = 374, + Settings_ModBus_RXTimeout = 375, + + Setting_EncoderSettingsBase = 400, // NOTE: Reserving settings values >= 400 for encoder settings. Up to 449. + Setting_EncoderSettingsMax = 449, + + // Reserved for user plugins - do NOT use for public plugins + Setting_UserDefined_0 = 450, + Setting_UserDefined_1 = 451, + Setting_UserDefined_2 = 452, + Setting_UserDefined_3 = 453, + Setting_UserDefined_4 = 454, + Setting_UserDefined_5 = 455, + Setting_UserDefined_6 = 456, + Setting_UserDefined_7 = 457, + Setting_UserDefined_8 = 458, + Setting_UserDefined_9 = 459, + + Setting_SettingsMax, + Setting_SettingsAll = Setting_SettingsMax, + + // Calculated base values for core stepper settings + Setting_AxisStepsPerMM = Setting_AxisSettingsBase, + Setting_AxisMaxRate = Setting_AxisSettingsBase + AXIS_SETTINGS_INCREMENT, + Setting_AxisAcceleration = Setting_AxisSettingsBase + 2 * AXIS_SETTINGS_INCREMENT, + Setting_AxisMaxTravel = Setting_AxisSettingsBase + 3 * AXIS_SETTINGS_INCREMENT, + Setting_AxisStepperCurrent = Setting_AxisSettingsBase + 4 * AXIS_SETTINGS_INCREMENT, + Setting_AxisMicroSteps = Setting_AxisSettingsBase + 5 * AXIS_SETTINGS_INCREMENT, + Setting_AxisBacklash = Setting_AxisSettingsBase + 6 * AXIS_SETTINGS_INCREMENT, + Setting_AxisAutoSquareOffset = Setting_AxisSettingsBase + 7 * AXIS_SETTINGS_INCREMENT, + + // Calculated base values for driver/plugin stepper settings + Setting_AxisExtended0 = Setting_AxisSettingsBase2, + Setting_AxisExtended1 = Setting_AxisSettingsBase2 + AXIS_SETTINGS_INCREMENT, + Setting_AxisExtended2 = Setting_AxisSettingsBase2 + 2 * AXIS_SETTINGS_INCREMENT, + Setting_AxisExtended3 = Setting_AxisSettingsBase2 + 3 * AXIS_SETTINGS_INCREMENT, + Setting_AxisExtended4 = Setting_AxisSettingsBase2 + 4 * AXIS_SETTINGS_INCREMENT, + Setting_AxisExtended5 = Setting_AxisSettingsBase2 + 5 * AXIS_SETTINGS_INCREMENT, + Setting_AxisExtended6 = Setting_AxisSettingsBase2 + 6 * AXIS_SETTINGS_INCREMENT, + Setting_AxisExtended7 = Setting_AxisSettingsBase2 + 7 * AXIS_SETTINGS_INCREMENT, + Setting_AxisExtended8 = Setting_AxisSettingsBase2 + 8 * AXIS_SETTINGS_INCREMENT, + Setting_AxisExtended9 = Setting_AxisSettingsBase2 + 9 * AXIS_SETTINGS_INCREMENT, + + // Calculated base values for encoder settings + Setting_EncoderModeBase = Setting_EncoderSettingsBase + Setting_EncoderMode, + Setting_EncoderCPRBase = Setting_EncoderSettingsBase + Setting_EncoderCPR, + Setting_EncoderCPDBase = Setting_EncoderSettingsBase + Setting_EncoderCPD, + Setting_EncoderDblClickWindowBase = Setting_EncoderSettingsBase + Setting_EncoderDblClickWindow +} setting_id_t; + +typedef union { + uint8_t mask; + struct { + uint8_t defaults :1, + parameters :1, + startup_lines :1, + build_info :1, + driver_parameters :1, + unassigned :3; + + }; +} settings_restore_t; + +extern const settings_restore_t settings_all; + +typedef char stored_line_t[MAX_STORED_LINE_LENGTH]; + +typedef union { + uint16_t value; + struct { + uint16_t report_inches :1, + restore_overrides :1, + safety_door_ignore_when_idle :1, + sleep_enable :1, + disable_laser_during_hold :1, + force_initialization_alarm :1, + legacy_rt_commands :1, + restore_after_feed_hold :1, + keep_coolant_state_on_door_open :1, + unassigned :7; + }; +} settingflags_t; + +typedef union { + uint8_t value; + struct { + uint8_t invert_probe_pin :1, + disable_probe_pullup :1, + invert_connected_pin :1, + disable_connected_pullup :1, + allow_feed_override :1, + enable_protection :1, + unassigned :2; + }; +} probeflags_t; + +typedef union { + uint16_t mask; + struct { + uint16_t machine_position :1, + buffer_state :1, + line_numbers :1, + feed_speed :1, + pin_state :1, + work_coord_offset :1, + overrides :1, + probe_coordinates :1, + sync_on_wco_change :1, + parser_state :1, + alarm_substate :1, + run_substate :1, + unassigned :4; + }; +} reportmask_t; + +typedef union { + uint8_t value; + struct { + uint8_t enabled :1, + deactivate_upon_init :1, + enable_override_control :1, + unassigned :5; + }; +} parking_setting_flags_t; + +typedef struct { + parking_setting_flags_t flags; + uint8_t axis; // Define which axis that performs the parking motion + float target; // Parking axis target. In mm, as machine coordinate [-max_travel,0]. + float rate; // Parking fast rate after pull-out in mm/min. + float pullout_rate; // Pull-out/plunge slow feed rate in mm/min. + float pullout_increment; // Spindle pull-out and plunge distance in mm. Incremental distance. +} parking_settings_t; + +typedef struct { + float p_gain; + float i_gain; + float d_gain; + float p_max_error; + float i_max_error; + float d_max_error; + float deadband; + float max_error; +} pid_values_t; + +typedef enum { + SpindleAction_None = 0, + SpindleAction_DisableWithZeroSPeed, + SpindleAction_EnableWithAllSPeeds, +} spindle_action_t; + +typedef union { + uint8_t value; + uint8_t mask; + struct { + uint8_t pwm_action :2, + unassigned :6; + }; +} spindle_settings_flags_t; + +typedef struct { + float rpm_max; + float rpm_min; + float pwm_freq; + float pwm_period; + float pwm_off_value; + float pwm_min_value; + float pwm_max_value; + float at_speed_tolerance; + pwm_piece_t pwm_piece[SPINDLE_NPWM_PIECES]; + pid_values_t pid; + uint16_t ppr; // Spindle encoder pulses per revolution + spindle_state_t invert; + spindle_settings_flags_t flags; +} spindle_settings_t; + +typedef struct { + pid_values_t pid; +} position_pid_t; // Used for synchronized motion + +typedef union { + uint8_t value; + struct { + uint8_t enabled :1, + single_axis_commands :1, + init_lock :1, + force_set_origin :1, + manual :1, + override_locks :1, + keep_on_reset :1, + unassigned :1; + }; +} homing_settings_flags_t; + +typedef struct { + float fail_length_percent; // DUAL_AXIS_HOMING_FAIL_AXIS_LENGTH_PERCENT + float fail_distance_max; // DUAL_AXIS_HOMING_FAIL_DISTANCE_MAX + float fail_distance_min; // DUAL_AXIS_HOMING_FAIL_DISTANCE_MIN +} homing_dual_axis_t; + +typedef struct { + float feed_rate; + float seek_rate; + float pulloff; + axes_signals_t dir_mask; + uint8_t locate_cycles; + uint16_t debounce_delay; + homing_settings_flags_t flags; + axes_signals_t cycle[N_AXIS]; + homing_dual_axis_t dual_axis; +} homing_settings_t; + +typedef struct { + axes_signals_t step_invert; + axes_signals_t dir_invert; + axes_signals_t enable_invert; + axes_signals_t deenergize; + float pulse_microseconds; + float pulse_delay_microseconds; + uint16_t idle_lock_time; // If value = 255, steppers do not disable. +} stepper_settings_t; + +typedef struct { + float steps_per_mm; + float max_rate; + float acceleration; + float max_travel; + float dual_axis_offset; +#ifdef ENABLE_BACKLASH_COMPENSATION + float backlash; +#endif +} axis_settings_t; + +typedef union { + uint8_t value; + struct { + uint8_t hard_enabled :1, + soft_enabled :1, + check_at_init :1, + jog_soft_limited :1, + two_switches :1, + unassigned :3; + }; +} limit_settings_flags_t; + +typedef struct { + limit_settings_flags_t flags; + axes_signals_t invert; + axes_signals_t disable_pullup; +} limit_settings_t; + +typedef union { + uint8_t value; + uint8_t mask; + struct { + uint8_t bit0 :1, + bit1 :1, + bit2 :1, + bit3 :1, + bit4 :1, + bit5 :1, + bit6 :1, + bit7 :1; + }; +} ioport_bus_t; + +typedef struct { + ioport_bus_t invert_in; + ioport_bus_t pullup_disable_in; + ioport_bus_t invert_out; + ioport_bus_t od_enable_out; +} ioport_signals_t; + +typedef enum { + Mode_Standard = 0, + Mode_Laser, + Mode_Lathe +} machine_mode_t; + +typedef enum { + ToolChange_Disabled = 0, + ToolChange_Manual, + ToolChange_Manual_G59_3, + ToolChange_SemiAutomatic, + ToolChange_Ignore +} toolchange_mode_t; + +typedef struct { + float feed_rate; + float seek_rate; + float pulloff_rate; + float probing_distance; + toolchange_mode_t mode; +} tool_change_settings_t; + +// Global persistent settings (Stored from byte persistent storage_ADDR_GLOBAL onwards) +typedef struct { + // Settings struct version + uint32_t version; + float junction_deviation; + float arc_tolerance; + float g73_retract; + machine_mode_t mode; + tool_change_settings_t tool_change; + axis_settings_t axis[N_AXIS]; + control_signals_t control_invert; + control_signals_t control_disable_pullup; + coolant_state_t coolant_invert; + spindle_settings_t spindle; + stepper_settings_t steppers; + reportmask_t status_report; // Mask to indicate desired report data. + settingflags_t flags; // Contains default boolean settings + probeflags_t probe; + homing_settings_t homing; + limit_settings_t limits; + parking_settings_t parking; + position_pid_t position; // Used for synchronized motion + ioport_signals_t ioport; +} settings_t; + +typedef enum { + Group_Root = 0, + Group_General, + Group_ControlSignals, + Group_Limits, + Group_Limits_DualAxis, + Group_Coolant, + Group_Spindle, + Group_Spindle_Sync, + Group_Spindle_ClosedLoop, + Group_Toolchange, + Group_Plasma, + Group_Homing, + Group_Probing, + Group_SafetyDoor, + Group_Jogging, + Group_Networking, + Group_Networking_Wifi, + Group_Bluetooth, + Group_AuxPorts, + Group_ModBus, + Group_Encoders, + Group_Encoder0, + Group_Encoder1, + Group_Encoder2, + Group_Encoder3, + Group_Encoder4, + Group_UserSettings, + Group_Stepper, + Group_MotorDriver, + Group_Axis, +// NOTE: axis groups MUST be sequential AND last + Group_Axis0, + Group_XAxis = Group_Axis0, + Group_YAxis, + Group_ZAxis, +#ifdef A_AXIS + Group_AAxis, +#endif +#ifdef B_AXIS + Group_BAxis, +#endif +#ifdef C_AXIS + Group_CAxis, +#endif + Group_All = Group_Root +} setting_group_t; + +typedef enum { + Format_Bool = 0, + Format_Bitfield, + Format_XBitfield, + Format_RadioButtons, + Format_AxisMask, + Format_Integer, // 32 bit + Format_Decimal, + Format_String, + Format_Password, + Format_IPv4, + // For internal use only + Format_Int8, + Format_Int16, +} setting_datatype_t; + +typedef struct { + setting_group_t parent; + setting_group_t id; + const char *name; +} setting_group_detail_t; + +typedef enum { + Setting_NonCore = 0, + Setting_NonCoreFn, + Setting_IsExtended, + Setting_IsExtendedFn, + Setting_IsLegacy, + Setting_IsLegacyFn, + Setting_IsExpanded, + Setting_IsExpandedFn +} setting_type_t; + +typedef union { + uint32_t ivalue; + float fvalue; +} setting_limit_t; + +typedef struct setting_detail { + setting_id_t id; + setting_group_t group; + const char *name; + const char *unit; + setting_datatype_t datatype; + const char *format; + const char *min_value; + const char *max_value; + setting_type_t type; + void *value; + void *get_value; + bool (*is_available)(const struct setting_detail *setting); +} setting_detail_t; + +typedef status_code_t (*setting_set_int_ptr)(setting_id_t id, uint_fast16_t value); +typedef status_code_t (*setting_set_float_ptr)(setting_id_t id, float value); +typedef status_code_t (*setting_set_string_ptr)(setting_id_t id, char *value); +typedef uint32_t (*setting_get_int_ptr)(setting_id_t id); +typedef float (*setting_get_float_ptr)(setting_id_t id); +typedef char *(*setting_get_string_ptr)(setting_id_t id); +typedef bool (*setting_output_ptr)(const setting_detail_t *setting, uint_fast16_t offset, void *data); + +typedef void (*settings_changed_ptr)(settings_t *settings); +typedef void (*driver_settings_load_ptr)(void); +typedef void (*driver_settings_save_ptr)(void); +typedef void (*driver_settings_restore_ptr)(void); + +typedef struct setting_details { + const uint8_t n_groups; + const setting_group_detail_t *groups; + const uint16_t n_settings; + const setting_detail_t *settings; + struct setting_details *(*on_get_settings)(void); + settings_changed_ptr on_changed; + driver_settings_save_ptr save; + driver_settings_load_ptr load; + driver_settings_restore_ptr restore; +} setting_details_t; + +extern settings_t settings; + +// Initialize the configuration subsystem (load settings from persistent storage) +void settings_init(); + +// Write Grbl global settings and version number to persistent storage +void settings_write_global(void); + +// Helper function to clear and restore persistent storage defaults +void settings_restore(settings_restore_t restore_flags); + +// A helper method to set new settings from command line +status_code_t settings_store_setting(setting_id_t setting, char *svalue); + +// Writes the protocol line variable as a startup line in persistent storage +void settings_write_startup_line(uint8_t idx, char *line); + +// Reads an persistent storage startup line to the protocol line variable +bool settings_read_startup_line(uint8_t idx, char *line); + +// Writes build info user-defined string +void settings_write_build_info(char *line); + +// Reads build info user-defined string +bool settings_read_build_info(char *line); + +// Writes selected coordinate data to persistent storage +void settings_write_coord_data(coord_system_id_t id, float (*coord_data)[N_AXIS]); + +// Reads selected coordinate data from persistent storage +bool settings_read_coord_data(coord_system_id_t id, float (*coord_data)[N_AXIS]); + +// Writes selected tool data to persistent storage +bool settings_write_tool_data (tool_data_t *tool_data); + +// Read selected tool data from persistent storage +bool settings_read_tool_data (uint32_t tool, tool_data_t *tool_data); + +setting_details_t *settings_get_details (void); +bool settings_is_group_available (setting_group_t group); +bool settings_iterator (const setting_detail_t *setting, setting_output_ptr callback, void *data); +const setting_detail_t *setting_get_details (setting_id_t id, setting_details_t **set); +setting_datatype_t setting_datatype_to_external (setting_datatype_t datatype); +setting_group_t settings_normalize_group (setting_group_t group); +char *setting_get_value (const setting_detail_t *setting, uint_fast16_t offset); +setting_id_t settings_get_axis_base (setting_id_t id, uint_fast8_t *idx); +bool setting_is_list (const setting_detail_t *setting); + +#endif diff --git a/sleep.c b/sleep.c new file mode 100644 index 0000000..9041d5b --- /dev/null +++ b/sleep.c @@ -0,0 +1,88 @@ +/* + sleep.c - determines and executes sleep procedures + + Part of grblHAL + + Copyright (c) 2018-2021 Terje Io + Copyright (c) 2016 Sungeun K. Jeon + + 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 . +*/ + +#include "hal.h" +#include "state_machine.h" + +volatile bool slumber; + +static void fall_asleep() +{ + slumber = false; +} + +// Starts sleep timer if running conditions are satified. When elapsed, sleep mode is executed. +static void sleep_execute() +{ + // Enable sleep timeout + slumber = true; + hal.delay_ms((uint32_t)(SLEEP_DURATION * 1000.0f * 60.0f), fall_asleep); + + // Fetch current number of buffered characters in input stream buffer. + uint16_t rx_initial = hal.stream.get_rx_buffer_available(); + + do { + // Monitor for any new input stream data or external events (queries, buttons, alarms) to exit. + if ((hal.stream.get_rx_buffer_available() != rx_initial) || sys.rt_exec_state || sys.rt_exec_alarm ) { + // Disable sleep timeout and return to normal operation. + hal.delay_ms(0, NULL); + return; + } + } while(slumber); + + // If reached, sleep counter has expired. Execute sleep procedures. + // Notify user that Grbl has timed out and will be parking. + // To exit sleep, resume or reset. Either way, the job will not be recoverable. + grbl.report.feedback_message(Message_SleepMode); + system_set_exec_state_flag(EXEC_SLEEP); +} + + +// Checks running conditions for sleep. If satisfied, enables sleep timeout and executes +// sleep mode upon elapse. +// NOTE: Sleep procedures can be blocking, since Grbl isn't receiving any commands, nor moving. +// Hence, make sure any valid running state that executes the sleep timer is not one that is moving. +void sleep_check() +{ + // The sleep execution feature will continue only if the machine is in an IDLE or HOLD state and + // has any powered components enabled. + // NOTE: With overrides or in laser mode, modal spindle and coolant state are not guaranteed. Need + // to directly monitor and record running state during parking to ensure proper function. + if (!sys.steppers_deenergize && (gc_state.modal.spindle.value || gc_state.modal.coolant.value)) { + switch(state_get()) { + + case STATE_IDLE: + sleep_execute(); + break; + + case STATE_HOLD: + if(sys.holding_state == Hold_Complete) + sleep_execute(); + break; + + case STATE_SAFETY_DOOR: + if(sys.parking_state == Parking_DoorAjar) + sleep_execute(); + break; + } + } +} diff --git a/sleep.h b/sleep.h new file mode 100644 index 0000000..2cfca6b --- /dev/null +++ b/sleep.h @@ -0,0 +1,29 @@ +/* + sleep.h - Sleep methods header file + + Part of grblHAL + + Copyright (c) 2016 Sungeun K. Jeon + + 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 . +*/ + +#ifndef _SLEEP_H_ +#define _SLEEP_H_ + +// Checks running conditions for sleep. If satisfied, enables sleep countdown and executes +// sleep mode upon elapse. +void sleep_check(); + +#endif diff --git a/spindle_control.c b/spindle_control.c new file mode 100644 index 0000000..e003167 --- /dev/null +++ b/spindle_control.c @@ -0,0 +1,225 @@ +/* + spindle_control.c - spindle control methods + + Part of grblHAL + + Copyright (c) 2017-2021 Terje Io + Copyright (c) 2012-2015 Sungeun K. Jeon + Copyright (c) 2009-2011 Simen Svale Skogsrud + + 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 . +*/ + +#include + +#include "hal.h" +#include "protocol.h" +#include "state_machine.h" + +// Set spindle speed override +// NOTE: Unlike motion overrides, spindle overrides do not require a planner reinitialization. +void spindle_set_override (uint_fast8_t speed_override) +{ + if(sys.override.control.spindle_rpm_disable) + return; + + speed_override = max(min(speed_override, MAX_SPINDLE_RPM_OVERRIDE), MIN_SPINDLE_RPM_OVERRIDE); + + if ((uint8_t)speed_override != sys.override.spindle_rpm) { + sys.override.spindle_rpm = (uint8_t)speed_override; + if(state_get() == STATE_IDLE) + spindle_set_state(gc_state.modal.spindle, gc_state.spindle.rpm); + else + sys.step_control.update_spindle_rpm = On; + sys.report.overrides = On; // Set to report change immediately + } +} + +// Immediately sets spindle running state with direction and spindle rpm, if enabled. +// Called by g-code parser spindle_sync(), parking retract and restore, g-code program end, +// sleep, and spindle stop override. +bool spindle_set_state (spindle_state_t state, float rpm) +{ + if (!ABORTED) { // Block during abort. + + if (!state.on) { // Halt or set spindle direction and rpm. + sys.spindle_rpm = rpm = 0.0f; + hal.spindle.set_state((spindle_state_t){0}, 0.0f); + } else { + // NOTE: Assumes all calls to this function is when Grbl is not moving or must remain off. + // TODO: alarm/interlock if going from CW to CCW directly in non-laser mode? + if (settings.mode == Mode_Laser && state.ccw) + rpm = 0.0f; // TODO: May need to be rpm_min*(100/MAX_SPINDLE_RPM_OVERRIDE); + + hal.spindle.set_state(state, spindle_set_rpm(rpm, sys.override.spindle_rpm)); + } + sys.report.spindle = On; // Set to report change immediately + + st_rpm_changed(rpm); + } + + return !ABORTED; +} + +// G-code parser entry-point for setting spindle state. Forces a planner buffer sync and bails +// if an abort or check-mode is active. +bool spindle_sync (spindle_state_t state, float rpm) +{ + bool ok = true; + bool at_speed = state_get() == STATE_CHECK_MODE || !state.on || !hal.driver_cap.spindle_at_speed || settings.spindle.at_speed_tolerance <= 0.0f; + + if (state_get() != STATE_CHECK_MODE) { + // Empty planner buffer to ensure spindle is set when programmed. + if((ok = protocol_buffer_synchronize()) && spindle_set_state(state, rpm) && !at_speed) { + float delay = 0.0f; + while(!(at_speed = hal.spindle.get_state().at_speed)) { + delay_sec(0.1f, DelayMode_Dwell); + delay += 0.1f; + if(ABORTED) + break; + if(delay >= SAFETY_DOOR_SPINDLE_DELAY) { + system_raise_alarm(Alarm_Spindle); + break; + } + } + } + } + + return ok && at_speed; +} + +// Restore spindle running state with direction, enable, spindle RPM and appropriate delay. +bool spindle_restore (spindle_state_t state, float rpm) +{ + bool ok = true; + + if(settings.mode == Mode_Laser) // When in laser mode, ignore spindle spin-up delay. Set to turn on laser when cycle starts. + sys.step_control.update_spindle_rpm = On; + else { // TODO: add check for current spindle state matches restore state? + spindle_set_state(state, rpm); + if(state.on) { + if((ok = !hal.driver_cap.spindle_at_speed)) + delay_sec(SAFETY_DOOR_SPINDLE_DELAY, DelayMode_SysSuspend); + else if((ok == (settings.spindle.at_speed_tolerance <= 0.0f))) { + float delay = 0.0f; + while(!(ok = hal.spindle.get_state().at_speed)) { + delay_sec(0.1f, DelayMode_SysSuspend); + delay += 0.1f; + if(ABORTED) + break; + if(delay >= SAFETY_DOOR_SPINDLE_DELAY) { + system_raise_alarm(Alarm_Spindle); + break; + } + } + } + } + } + + return ok; +} + +// Calculate and set programmed RPM according to override and max/min limits +float spindle_set_rpm (float rpm, uint8_t override_pct) +{ + if(override_pct != 100) + rpm *= 0.01f * (float)override_pct; // Scale RPM by override value. + + // Apply RPM limits + if (rpm <= 0.0f) + rpm = 0.0f; + else if (rpm > settings.spindle.rpm_max) + rpm = settings.spindle.rpm_max; + else if (rpm < settings.spindle.rpm_min) + rpm = settings.spindle.rpm_min; + + sys.spindle_rpm = rpm; + + return rpm; +} + +// +// The following functions are not called by the core, may be called by driver code. +// + +// calculate inverted pwm value if configured +static inline uint_fast16_t invert_pwm (spindle_pwm_t *pwm_data, uint_fast16_t pwm_value) +{ + return pwm_data->invert_pwm ? pwm_data->period - pwm_value - 1 : pwm_value; +} + +// Precompute PWM values for faster conversion. +// Returns false if no PWM range possible, driver should revert to simple on/off spindle control if so. +bool spindle_precompute_pwm_values (spindle_pwm_t *pwm_data, uint32_t clock_hz) +{ + if(settings.spindle.rpm_max > settings.spindle.rpm_min) { + pwm_data->period = (uint_fast16_t)((float)clock_hz / settings.spindle.pwm_freq); + if(settings.spindle.pwm_off_value == 0.0f) + pwm_data->off_value = pwm_data->invert_pwm ? pwm_data->period : 0; + else + pwm_data->off_value = invert_pwm(pwm_data, (uint_fast16_t)(pwm_data->period * settings.spindle.pwm_off_value / 100.0f)); + pwm_data->min_value = (uint_fast16_t)(pwm_data->period * settings.spindle.pwm_min_value / 100.0f); + pwm_data->max_value = (uint_fast16_t)(pwm_data->period * settings.spindle.pwm_max_value / 100.0f) + pwm_data->offset; + pwm_data->pwm_gradient = (float)(pwm_data->max_value - pwm_data->min_value) / (settings.spindle.rpm_max - settings.spindle.rpm_min); + pwm_data->always_on = settings.spindle.pwm_off_value != 0.0f; + } + +#ifdef ENABLE_SPINDLE_LINEARIZATION + uint_fast8_t idx; + + pwm_data->n_pieces = 0; + + for(idx = 0; idx < SPINDLE_NPWM_PIECES; idx++) { + if(!isnan(settings.spindle.pwm_piece[idx].rpm) && settings.spindle.pwm_piece[idx].start != 0.0f) + memcpy(&pwm_data->piece[pwm_data->n_pieces++], &settings.spindle.pwm_piece[idx], sizeof(pwm_piece_t)); + } +#endif + + return settings.spindle.rpm_max > settings.spindle.rpm_min; +} + +// Spindle RPM to PWM conversion. +uint_fast16_t spindle_compute_pwm_value (spindle_pwm_t *pwm_data, float rpm, bool pid_limit) +{ + uint_fast16_t pwm_value; + + if(rpm > settings.spindle.rpm_min) { + #ifdef ENABLE_SPINDLE_LINEARIZATION + // Compute intermediate PWM value with linear spindle speed model via piecewise linear fit model. + uint_fast8_t idx = pwm_data->n_pieces; + + if(idx) { + do { + idx--; + if(idx == 0 || rpm > pwm_data->piece[idx].rpm) { + pwm_value = floorf(pwm_data->piece[idx].start * rpm - pwm_data->piece[idx].end); + break; + } + } while(idx); + } else + #endif + // Compute intermediate PWM value with linear spindle speed model. + pwm_value = (uint_fast16_t)floorf((rpm - settings.spindle.rpm_min) * pwm_data->pwm_gradient) + pwm_data->min_value; + + if(pwm_value >= (pid_limit ? pwm_data->period : pwm_data->max_value)) + pwm_value = pid_limit ? pwm_data->period - 1 : pwm_data->max_value; + else if(pwm_value < pwm_data->min_value) + pwm_value = pwm_data->min_value; + + pwm_value = invert_pwm(pwm_data, pwm_value); + } else + pwm_value = rpm == 0.0f ? pwm_data->off_value : invert_pwm(pwm_data, pwm_data->min_value); + + return pwm_value; +} diff --git a/spindle_control.h b/spindle_control.h new file mode 100644 index 0000000..731f89f --- /dev/null +++ b/spindle_control.h @@ -0,0 +1,109 @@ +/* + spindle_control.h - spindle control methods + + Part of grblHAL + + Copyright (c) 2017-2021 Terje Io + Copyright (c) 2012-2015 Sungeun K. Jeon + Copyright (c) 2009-2011 Simen Svale Skogsrud + + 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 . +*/ + +#ifndef _SPINDLE_CONTROL_H_ +#define _SPINDLE_CONTROL_H_ + +typedef union { + uint8_t value; + uint8_t mask; + struct { + uint8_t on :1, + ccw :1, + pwm :1, // NOTE: only used for PWM inversion setting + reserved3 :1, + reserved4 :1, + encoder_error :1, + at_speed :1, + synchronized :1; + }; +} spindle_state_t; + +typedef struct { + float rpm; + float start; + float end; +} pwm_piece_t; + +// Precalculated values that may be set/used by HAL driver to speed up RPM to PWM conversions if variable spindle is supported +typedef struct { + uint_fast16_t period; + uint_fast16_t off_value; // NOTE: this value holds the inverted version if software PWM inversion is enabled by the driver. + uint_fast16_t min_value; + uint_fast16_t max_value; + float pwm_gradient; + bool invert_pwm; // NOTE: set (by driver) when inversion is done in code + bool always_on; + int_fast16_t offset; + uint_fast16_t n_pieces; + pwm_piece_t piece[SPINDLE_NPWM_PIECES]; +} spindle_pwm_t; + +// Used when HAL driver supports spindle synchronization +typedef struct { + float rpm; + float rpm_low_limit; + float rpm_high_limit; + float angular_position; // Number of revolutions since last reset + float rpm_programmed; + uint32_t index_count; + uint32_t pulse_count; + uint32_t error_count; + spindle_state_t state_programmed; +} spindle_data_t; + +typedef enum { + SpindleData_Counters, + SpindleData_RPM, + SpindleData_AngularPosition +} spindle_data_request_t; + +void spindle_set_override (uint_fast8_t speed_override); + +// Called by g-code parser when setting spindle state and requires a buffer sync. +// Immediately sets spindle running state with direction and spindle RPM, if enabled. +// Called by spindle_sync() after sync and parking motion/spindle stop override during restore. + +// Called by g-code parser when setting spindle state and requires a buffer sync. +bool spindle_sync (spindle_state_t state, float rpm); + +// Sets spindle running state with direction, enable, and spindle RPM. +bool spindle_set_state (spindle_state_t state, float rpm); + +// Spindle speed calculation and limit handling +float spindle_set_rpm (float rpm, uint8_t speed_override); + +// Restore spindle running state with direction, enable, spindle RPM and appropriate delay. +bool spindle_restore (spindle_state_t state, float rpm); + +// +// The following functions are not called by the core, may be called by driver code. +// + +// Precompute PWM values for faster conversion. +bool spindle_precompute_pwm_values (spindle_pwm_t *pwm_data, uint32_t clock_hz); + +// Spindle speed to PWM conversion. +uint_fast16_t spindle_compute_pwm_value (spindle_pwm_t *pwm_data, float rpm, bool pid_limit); + +#endif diff --git a/spindle_sync.h b/spindle_sync.h new file mode 100644 index 0000000..83dfc50 --- /dev/null +++ b/spindle_sync.h @@ -0,0 +1,78 @@ +/* + spindle_sync.h - An embedded CNC Controller with rs274/ngc (g-code) support + + Spindle sync data strucures + + NOTE: not referenced in the core grbl code + + Part of grblHAL + + Copyright (c) 2020-2021 Terje Io + + Grbl is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + Grbl is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with Grbl. If not, see . +*/ + +#ifndef _SPINDLE_SYNC_H_ +#define _SPINDLE_SYNC_H_ + +#include "pid.h" + +// Free running timer log data. +// The free running timer is used to timestamp pulse events from the encoder. +typedef struct { + volatile uint32_t last_index; // Timer value at last encoder index pulse + volatile uint32_t last_pulse; // Timer value at last encoder pulse + volatile uint32_t pulse_length; // Last timer tics between spindle encoder pulse interrupts. +} spindle_encoder_timer_t; + +// Pulse counter timer log data. +// This counter is used to "prescale" the encoder pulses in order to +// reduce pulse interrupt frequency. This allows the use of high PPR encoders +// without overloading the MCU while still making use of the better resolution. +// NOTE: if a 16bit counter is used then it is important than proper casting +// is performed in order to handle counter overflow correctly. +typedef struct { + volatile uint32_t last_count; // Counter value at last encoder pulse interrupt + volatile uint32_t last_index; // Counter value at last encoder index interrupt + volatile uint32_t index_count; + volatile uint32_t pulse_count; +} spindle_encoder_counter_t; + +typedef struct { + uint32_t ppr; // Encoder pulses per revolution + float rpm_factor; // Inverse of event timer tics per RPM + float pulse_distance; // Encoder pulse distance in fraction of one revolution + uint32_t maximum_tt; // Maximum timer tics since last spindle encoder pulse before RPM = 0 is returned + spindle_encoder_timer_t timer; // Event timestamps + spindle_encoder_counter_t counter; // Encoder event counts + uint32_t error_count; // Incremented when actual PPR count differs from ppr setting + uint32_t tics_per_irq; // Counts per interrupt generated (prescaler value) + volatile bool spin_lock; +} spindle_encoder_t; + +typedef struct { + float prev_pos; // Target position of previous segment + float steps_per_mm; // Steps per mm for current block + float programmed_rate; // Programmed feed in mm/rev for current block + int32_t min_cycles_per_tick; // Minimum cycles per tick for PID loop + uint_fast8_t segment_id; // Used for detecing start of new segment + pidf_t pid; // PID data for position + stepper_pulse_start_ptr stepper_pulse_start_normal; // Driver pulse function to restore after spindle sync move is completed +#ifdef PID_LOG + int32_t log[PID_LOG]; + int32_t pos[PID_LOG]; +#endif +} spindle_sync_t; + +#endif diff --git a/state_machine.c b/state_machine.c new file mode 100644 index 0000000..b4d125e --- /dev/null +++ b/state_machine.c @@ -0,0 +1,681 @@ +/* + state_machine.c - An embedded CNC Controller with rs274/ngc (g-code) support + + Main state machine + + Part of grblHAL + + Copyright (c) 2018-2021 Terje Io + Copyright (c) 2011-2016 Sungeun K. Jeon for Gnea Research LLC + Copyright (c) 2009-2011 Simen Svale Skogsrud + + 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 . +*/ + +#include +//#include + +#include "hal.h" +#include "motion_control.h" +#include "state_machine.h" +#include "override.h" + +static void state_idle (uint_fast16_t new_state); +static void state_cycle (uint_fast16_t rt_exec); +static void state_await_hold (uint_fast16_t rt_exec); +static void state_noop (uint_fast16_t rt_exec); +static void state_await_motion_cancel (uint_fast16_t rt_exec); +static void state_await_resume (uint_fast16_t rt_exec); +static void state_await_toolchanged (uint_fast16_t rt_exec); +static void state_await_waypoint_retract (uint_fast16_t rt_exec); +static void state_restore (uint_fast16_t rt_exec); +static void state_await_resumed (uint_fast16_t rt_exec); +static void state_await_restore (uint_fast16_t rt_exec); + +static void (* volatile stateHandler)(uint_fast16_t rt_exec) = state_idle; + +static float restore_spindle_rpm; +static planner_cond_t restore_condition; +static sys_state_t pending_state = STATE_IDLE, sys_state = STATE_IDLE; + +typedef struct { + float target[N_AXIS]; + float restore_target[N_AXIS]; + float retract_waypoint; + bool retracting; + bool restart_retract; + bool active; + plan_line_data_t plan_data; +} parking_data_t; + +// Declare and initialize parking local variables +static parking_data_t park; + +static void state_restore_conditions (planner_cond_t *condition, float rpm) +{ + if(!settings.parking.flags.enabled || !park.restart_retract) { + + spindle_restore(condition->spindle, rpm); + + // Block if safety door re-opened during prior restore actions. + if (gc_state.modal.coolant.value != hal.coolant.get_state().value) { + // NOTE: Laser mode will honor this delay. An exhaust system is often controlled by this pin. + coolant_set_state(condition->coolant); + delay_sec(SAFETY_DOOR_COOLANT_DELAY, DelayMode_SysSuspend); + } + + sys.override.spindle_stop.value = 0; // Clear spindle stop override states + } +} + +bool initiate_hold (uint_fast16_t new_state) +{ + if(settings.parking.flags.enabled) { + memset(&park.plan_data, 0, sizeof(plan_line_data_t)); + park.plan_data.condition.system_motion = On; + park.plan_data.condition.no_feed_override = On; + park.plan_data.line_number = PARKING_MOTION_LINE_NUMBER; + } + + plan_block_t *block = plan_get_current_block(); + + if (block == NULL) { + restore_condition.spindle = gc_state.modal.spindle; + restore_condition.coolant.mask = gc_state.modal.coolant.mask | hal.coolant.get_state().mask; + restore_spindle_rpm = gc_state.spindle.rpm; + } else { + restore_condition = block->condition; + restore_spindle_rpm = block->spindle.rpm; + } + + if(settings.mode == Mode_Laser && settings.flags.disable_laser_during_hold) + enqueue_accessory_override(CMD_OVERRIDE_SPINDLE_STOP); + + if(sys_state & (STATE_CYCLE|STATE_JOG)) { + st_update_plan_block_parameters(); // Notify stepper module to recompute for hold deceleration. + sys.step_control.execute_hold = On; // Initiate suspend state with active flag. + stateHandler = state_await_hold; + } + + if(new_state == STATE_HOLD) + sys.holding_state = Hold_Pending; + else { + sys.parking_state = Parking_Retracting; + park.active = false; + } + + sys.suspend = true; + pending_state = sys_state == STATE_JOG ? new_state : STATE_IDLE; + + return sys_state == STATE_CYCLE; +} + +bool state_door_reopened (void) +{ + return settings.parking.flags.enabled && park.restart_retract; +} + +void state_update (uint_fast16_t rt_exec) +{ + if((rt_exec & EXEC_SAFETY_DOOR) && sys_state != STATE_SAFETY_DOOR) + state_set(STATE_SAFETY_DOOR); + else + stateHandler(rt_exec); +} + +sys_state_t state_get (void) +{ + return sys_state; +} + +void state_set (uint_fast16_t new_state) +{ + if(new_state != sys_state) { + + switch(new_state) { // Set up new state and handler + + case STATE_IDLE: + sys.suspend = false; // Break suspend state. + sys.step_control.flags = 0; // Restore step control to normal operation. + sys.parking_state = Parking_DoorClosed; + sys.holding_state = Hold_NotHolding; + sys_state = pending_state = new_state; + stateHandler = state_idle; + break; + + case STATE_CYCLE: + if(sys_state == STATE_IDLE) { + // Start cycle only if queued motions exist in planner buffer and the motion is not canceled. + plan_block_t *block; + if ((block = plan_get_current_block())) { + sys_state = new_state; + sys.steppers_deenergize = false; // Cancel stepper deenergize if pending. + st_prep_buffer(); // Initialize step segment buffer before beginning cycle. + if(block->condition.spindle.synchronized) { + + if(hal.spindle.reset_data) + hal.spindle.reset_data(); + + uint32_t index = hal.spindle.get_data(SpindleData_Counters)->index_count + 2; + + while(index != hal.spindle.get_data(SpindleData_Counters)->index_count); // check for abort in this loop? + + } + st_wake_up(); + stateHandler = state_cycle; + } + } + break; + + case STATE_JOG: + if(sys_state == STATE_TOOL_CHANGE) + pending_state = STATE_TOOL_CHANGE; + sys_state = new_state; + stateHandler = state_cycle; + break; + + case STATE_TOOL_CHANGE: + sys_state = new_state; + stateHandler = state_await_toolchanged; + break; + + case STATE_HOLD: + if(sys.override.control.sync && sys.override.control.feed_hold_disable) + sys.flags.feed_hold_pending = On; + if(!((sys_state & STATE_JOG) || sys.override.control.feed_hold_disable)) { + if(!initiate_hold(new_state)) { + sys.holding_state = Hold_Complete; + stateHandler = state_await_resume; + } + sys_state = new_state; + sys.flags.feed_hold_pending = Off; + } + break; + + case STATE_SAFETY_DOOR: + if((sys_state & (STATE_ALARM|STATE_ESTOP|STATE_SLEEP|STATE_CHECK_MODE))) + return; + grbl.report.feedback_message(Message_SafetyDoorAjar); + // no break + case STATE_SLEEP: + sys.parking_state = Parking_Retracting; + if(!initiate_hold(new_state)) { + if(pending_state != new_state) { + sys_state = new_state; + state_await_hold(EXEC_CYCLE_COMPLETE); // "Simulate" a cycle stop + } + } else + sys_state = new_state; + break; + + case STATE_ALARM: + case STATE_ESTOP: + case STATE_HOMING: + case STATE_CHECK_MODE: + sys_state = new_state; + sys.suspend = false; + stateHandler = state_noop; + break; + } + + if(!(sys_state & (STATE_ALARM|STATE_ESTOP))) + sys.alarm = Alarm_None; + + if(grbl.on_state_change) + grbl.on_state_change(new_state); + } +} + +// Suspend manager. Controls spindle overrides in hold states. +void state_suspend_manager (void) +{ + if(stateHandler != state_await_resume || !gc_state.modal.spindle.on) + return; + + if (sys.override.spindle_stop.value) { + + // Handles beginning of spindle stop + if (sys.override.spindle_stop.initiate) { + sys.override.spindle_stop.value = 0; // Clear stop override state + spindle_set_state((spindle_state_t){0}, 0.0f); // De-energize + sys.override.spindle_stop.enabled = On; // Set stop override state to enabled, if de-energized. + } + + // Handles restoring of spindle state + if (sys.override.spindle_stop.restore) { + grbl.report.feedback_message(Message_SpindleRestore); + if (settings.mode == Mode_Laser) // When in laser mode, ignore spindle spin-up delay. Set to turn on laser when cycle starts. + sys.step_control.update_spindle_rpm = On; + else + spindle_set_state(restore_condition.spindle, restore_spindle_rpm); + sys.override.spindle_stop.value = 0; // Clear stop override state + } + + } else if (sys.step_control.update_spindle_rpm && hal.spindle.get_state().on) { + // Handles spindle state during hold. NOTE: Spindle speed overrides may be altered during hold state. + spindle_set_state(restore_condition.spindle, restore_spindle_rpm); + sys.step_control.update_spindle_rpm = Off; + } +} + +static void state_idle (uint_fast16_t rt_exec) +{ + if((rt_exec & EXEC_CYCLE_START)) + state_set(STATE_CYCLE); + + if(rt_exec & EXEC_FEED_HOLD) + state_set(STATE_HOLD); + + if ((rt_exec & EXEC_TOOL_CHANGE)) { + hal.stream.suspend_read(true); // Block reading from input stream until tool change state is acknowledged + state_set(STATE_TOOL_CHANGE); + } + + if (rt_exec & EXEC_SLEEP) + state_set(STATE_SLEEP); +} + +static void state_cycle (uint_fast16_t rt_exec) +{ + if (rt_exec == EXEC_CYCLE_START) + return; // no need to perform other tests... + + if ((rt_exec & EXEC_TOOL_CHANGE)) + hal.stream.suspend_read(true); // Block reading from input stream until tool change state is acknowledged + + if (rt_exec & EXEC_CYCLE_COMPLETE) + state_set(gc_state.tool_change ? STATE_TOOL_CHANGE : STATE_IDLE); + + if (rt_exec & EXEC_MOTION_CANCEL) { + st_update_plan_block_parameters(); // Notify stepper module to recompute for hold deceleration. + sys.suspend = true; + sys.step_control.execute_hold = On; // Initiate suspend state with active flag. + stateHandler = state_await_motion_cancel; + } + + if ((rt_exec & EXEC_FEED_HOLD)) + state_set(STATE_HOLD); +} + +static void state_await_toolchanged (uint_fast16_t rt_exec) +{ + if (rt_exec & EXEC_CYCLE_START) { + if(!gc_state.tool_change) { + if(hal.stream.suspend_read) + hal.stream.suspend_read(false); // Tool change complete, restore "normal" stream input. + sys.report.tool = On; + } + pending_state = gc_state.tool_change ? STATE_TOOL_CHANGE : STATE_IDLE; + state_set(STATE_IDLE); + state_set(STATE_CYCLE); + // Force a status report to let the sender know tool change is completed. + system_set_exec_state_flag(EXEC_STATUS_REPORT); + } +} + +static void state_await_motion_cancel (uint_fast16_t rt_exec) +{ + if (rt_exec & EXEC_CYCLE_COMPLETE) { + if(sys_state == STATE_JOG) { + sys.step_control.flags = 0; + plan_reset(); + st_reset(); + sync_position(); +#ifdef ENABLE_BACKLASH_COMPENSATION + mc_sync_backlash_position(); +#endif + sys.suspend = false; + } + state_set(pending_state); + if(gc_state.tool_change) + state_set(STATE_TOOL_CHANGE); + } +} + +static void state_await_hold (uint_fast16_t rt_exec) +{ + if (rt_exec & EXEC_CYCLE_COMPLETE) { + + bool handler_changed = false; + + plan_cycle_reinitialize(); + sys.step_control.flags = 0; + + if(sys.alarm_pending) { + system_set_exec_alarm(sys.alarm_pending); + sys.alarm_pending = Alarm_None; + } + + switch (sys_state) { + + case STATE_TOOL_CHANGE: + hal.spindle.set_state((spindle_state_t){0}, 0.0f); // De-energize + hal.coolant.set_state((coolant_state_t){0}); // De-energize + break; + + // Resume door state when parking motion has retracted and door has been closed. + case STATE_SLEEP: + case STATE_SAFETY_DOOR: + // Parking manager. Handles de/re-energizing, switch state checks, and parking motions for + // the safety door and sleep states. + + // Handles retraction motions and de-energizing. + // Ensure any prior spindle stop override is disabled at start of safety door routine. + sys.override.spindle_stop.value = 0; + + // Parking requires parking axis homed, the current location not exceeding the??? + // parking target location, and laser mode disabled. + if(settings.parking.flags.enabled && !sys.override.control.parking_disable && settings.mode != Mode_Laser) { + + // Get current position and store as restore location. + if (!park.active) { + park.active = true; + system_convert_array_steps_to_mpos(park.restore_target, sys.position); + } + + // Execute slow pull-out parking retract motion. + // NOTE: State is will remain DOOR, until the de-energizing and retract is complete. + if (bit_istrue(sys.homed.mask, bit(settings.parking.axis)) && (park.restore_target[settings.parking.axis] < settings.parking.target)) { + + handler_changed = true; + stateHandler = state_await_waypoint_retract; + + // Copy restore location to park target and calculate spindle retract waypoint. + memcpy(park.target, park.restore_target, sizeof(park.target)); + park.retract_waypoint = settings.parking.pullout_increment + park.target[settings.parking.axis]; + park.retract_waypoint = min(park.retract_waypoint, settings.parking.target); + + // Retract spindle by pullout distance. Ensure retraction motion moves away from + // the workpiece and waypoint motion doesn't exceed the parking target location. + if (park.target[settings.parking.axis] < park.retract_waypoint) { + park.target[settings.parking.axis] = park.retract_waypoint; + park.plan_data.feed_rate = settings.parking.pullout_rate; + park.plan_data.condition.coolant = restore_condition.coolant; // Retain coolant state + park.plan_data.condition.spindle = restore_condition.spindle; // Retain spindle state + park.plan_data.spindle.rpm = restore_spindle_rpm; + if(!(park.retracting = mc_parking_motion(park.target, &park.plan_data))) + stateHandler(EXEC_CYCLE_COMPLETE); + } else + stateHandler(EXEC_CYCLE_COMPLETE); + } else { + // Parking motion not possible. Just disable the spindle and coolant. + // NOTE: Laser mode does not start a parking motion to ensure the laser stops immediately. + hal.spindle.set_state((spindle_state_t){0}, 0.0f); // De-energize + if(!settings.flags.keep_coolant_state_on_door_open) + hal.coolant.set_state((coolant_state_t){0}); // De-energize + sys.parking_state = Parking_DoorAjar; + } + } else { + hal.spindle.set_state((spindle_state_t){0}, 0.0f); // De-energize + if(!settings.flags.keep_coolant_state_on_door_open) + hal.coolant.set_state((coolant_state_t){0}); // De-energize + sys.parking_state = Parking_DoorAjar; + } + break; + + default: + break; + } + + if(!handler_changed) { + sys.holding_state = Hold_Complete; + stateHandler = state_await_resume; + } + } +} + +static void state_await_resume (uint_fast16_t rt_exec) +{ + if((rt_exec & EXEC_CYCLE_COMPLETE) && settings.parking.flags.enabled) { + if(sys.step_control.execute_sys_motion) + sys.step_control.execute_sys_motion = Off; + sys.parking_state = Parking_DoorAjar; + } + + if ((rt_exec & EXEC_CYCLE_START) && !(sys_state == STATE_SAFETY_DOOR && hal.control.get_state().safety_door_ajar)) { + + bool handler_changed = false; + + if(sys_state == STATE_HOLD && !sys.override.spindle_stop.value) + sys.override.spindle_stop.restore_cycle = On; + + switch (sys_state) { + + case STATE_TOOL_CHANGE: + break; + + case STATE_SLEEP: + break; + + // Resume door state when parking motion has retracted and door has been closed. + case STATE_SAFETY_DOOR: + + if(settings.parking.flags.enabled) { + + park.restart_retract = false; + sys.parking_state = Parking_Resuming; + + // Execute fast restore motion to the pull-out position. Parking requires homing enabled. + // NOTE: State is will remain DOOR, until the de-energizing and retract is complete. + if (park.retracting) { + handler_changed = true; + stateHandler = state_restore; + // Check to ensure the motion doesn't move below pull-out position. + if (park.target[settings.parking.axis] <= settings.parking.target) { + float target[N_AXIS]; + memcpy(target, park.target, sizeof(target)); + target[settings.parking.axis] = park.retract_waypoint; + park.plan_data.feed_rate = settings.parking.rate; + if(!mc_parking_motion(target, &park.plan_data)) + stateHandler(EXEC_CYCLE_COMPLETE); + else + st_parking_setup_buffer(); + } else // tell next handler to proceed with final step immediately + stateHandler(EXEC_CYCLE_COMPLETE); + } + } else + // Delayed Tasks: Restart spindle and coolant, delay to power-up, then resume cycle. + // Block if safety door re-opened during prior restore actions. + state_restore_conditions(&restore_condition, restore_spindle_rpm); + break; + + default: + if (!settings.flags.restore_after_feed_hold) { + if(!hal.spindle.get_state().on) { + gc_state.spindle.rpm = 0.0f; + gc_state.modal.spindle.on = gc_state.modal.spindle.ccw = Off; + } + sys.override.spindle_stop.value = 0; // Clear spindle stop override states + } else { + handler_changed = true; + stateHandler = state_await_restore; + stateHandler(0); + } + break; + } + + // Restart cycle if there is no further processing to take place + if(!(handler_changed || sys_state == STATE_SLEEP)) { + state_set(STATE_IDLE); + state_set(STATE_CYCLE); + } + } + + if (rt_exec & EXEC_SLEEP) + state_set(STATE_SLEEP); +} + +static void state_await_restore (uint_fast16_t rt_exec) +{ + static bool restart = false; + + if(rt_exec == 0) { + + restart = true; + + if (restore_condition.spindle.on != hal.spindle.get_state().on) { + grbl.report.feedback_message(Message_SpindleRestore); + spindle_restore(restore_condition.spindle, restore_spindle_rpm); + } + + if (restore_condition.coolant.value != hal.coolant.get_state().value) { + // NOTE: Laser mode will honor this delay. An exhaust system is often controlled by this pin. + coolant_set_state(restore_condition.coolant); + delay_sec(SAFETY_DOOR_COOLANT_DELAY, DelayMode_SysSuspend); + } + + sys.override.spindle_stop.value = 0; // Clear spindle stop override states + + grbl.report.feedback_message(Message_None); + + if(restart) { + state_set(STATE_IDLE); + state_set(STATE_CYCLE); + } + } + + if(rt_exec & EXEC_FEED_HOLD) { + restart = false; + stateHandler = state_await_resume; + } + +} + +static void restart_retract (void) +{ + grbl.report.feedback_message(Message_SafetyDoorAjar); + + stateHandler = state_await_hold; + + park.restart_retract = true; + sys.parking_state = Parking_Retracting; + + if (sys.step_control.execute_sys_motion) { + st_update_plan_block_parameters(); // Notify stepper module to recompute for hold deceleration. + sys.step_control.execute_hold = On; + sys.step_control.execute_sys_motion = On; + } else // else NO_MOTION is active. + stateHandler(EXEC_CYCLE_COMPLETE); +} + +static void state_await_waypoint_cancel (uint_fast16_t rt_exec) +{ + if (rt_exec & EXEC_SAFETY_DOOR) + restart_retract(); + + else if (rt_exec & EXEC_CYCLE_COMPLETE) { + sys.parking_state = Parking_Cancel; + sys.step_control.execute_hold = Off; + state_restore(rt_exec); + } +} + +static void state_await_waypoint_retract (uint_fast16_t rt_exec) +{ + if (rt_exec & EXEC_CYCLE_COMPLETE) { + + if(sys.step_control.execute_sys_motion) + sys.step_control.execute_sys_motion = Off; + + // NOTE: Clear accessory state after retract and after an aborted restore motion. + park.plan_data.condition.spindle.value = 0; + park.plan_data.spindle.rpm = 0.0f; + hal.spindle.set_state(park.plan_data.condition.spindle, 0.0f); // De-energize + + if(!settings.flags.keep_coolant_state_on_door_open) { + park.plan_data.condition.coolant.value = 0; + hal.coolant.set_state(park.plan_data.condition.coolant); // De-energize + } + + stateHandler = state_await_resume; + + // Execute fast parking retract motion to parking target location. + if (park.target[settings.parking.axis] < settings.parking.target) { + float target[N_AXIS]; + memcpy(target, park.target, sizeof(target)); + target[settings.parking.axis] = settings.parking.target; + park.plan_data.feed_rate = settings.parking.rate; + if(mc_parking_motion(target, &park.plan_data)) + park.retracting = true; + else + stateHandler(EXEC_CYCLE_COMPLETE); + } else + stateHandler(EXEC_CYCLE_COMPLETE); + + } else if (rt_exec & EXEC_CYCLE_START) { + + stateHandler = state_await_waypoint_cancel; + + if (sys.step_control.execute_sys_motion) { + st_update_plan_block_parameters(); // Notify stepper module to recompute for hold deceleration. + sys.step_control.execute_hold = On; + sys.step_control.execute_sys_motion = On; + } else // else NO_MOTION is active. + stateHandler(EXEC_CYCLE_COMPLETE); + } +} + +static void state_restore (uint_fast16_t rt_exec) +{ + if (rt_exec & EXEC_SAFETY_DOOR) + restart_retract(); + + else if (rt_exec & EXEC_CYCLE_COMPLETE) { + + if(sys.step_control.execute_sys_motion) + sys.step_control.execute_sys_motion = Off; + + stateHandler = state_await_resumed; + + // Delayed Tasks: Restart spindle and coolant, delay to power-up, then resume cycle. + // Block if safety door re-opened during prior restore actions. + if(sys.parking_state != Parking_Cancel) + state_restore_conditions(&restore_condition, restore_spindle_rpm); + + park.restart_retract = false; + sys.parking_state = Parking_Resuming; + + // Execute slow plunge motion from pull-out position to resume position. + + // Regardless if the retract parking motion was a valid/safe motion or not, the + // restore parking motion should logically be valid, either by returning to the + // original position through valid machine space or by not moving at all. + park.plan_data.feed_rate = settings.parking.pullout_rate; + park.plan_data.condition.coolant = restore_condition.coolant; + park.plan_data.condition.spindle = restore_condition.spindle; + park.plan_data.spindle.rpm = restore_spindle_rpm; + if(!mc_parking_motion(park.restore_target, &park.plan_data)) + stateHandler(EXEC_CYCLE_COMPLETE); // No motion, proceed to next step + } +} + +static void state_await_resumed (uint_fast16_t rt_exec) +{ + if (rt_exec & EXEC_SAFETY_DOOR) + restart_retract(); + + else if (rt_exec & EXEC_CYCLE_COMPLETE) { + if(sys.step_control.execute_sys_motion) { + sys.step_control.flags = 0; + st_parking_restore_buffer(); // Restore step segment buffer to normal run state. + } + state_set(STATE_IDLE); + state_set(STATE_CYCLE); + } +} + +static void state_noop (uint_fast16_t rt_exec) +{ + // Do nothing - state change requests are handled elsewhere or ignored. +} diff --git a/state_machine.h b/state_machine.h new file mode 100644 index 0000000..fb02f33 --- /dev/null +++ b/state_machine.h @@ -0,0 +1,33 @@ +/* + state_machine.h - An embedded CNC Controller with rs274/ngc (g-code) support + + Main state machine + + Part of grblHAL + + Copyright (c) 2018-2021 Terje Io + + Grbl is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + Grbl is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with Grbl. If not, see . +*/ + +#ifndef _STATE_MACHINE_H_ +#define _STATE_MACHINE_H_ + +sys_state_t state_get (void); +void state_set (uint_fast16_t state); +void state_update (uint_fast16_t rt_exec); +bool state_door_reopened (void); +void state_suspend_manager (void); + +#endif diff --git a/stepper.c b/stepper.c new file mode 100644 index 0000000..775696b --- /dev/null +++ b/stepper.c @@ -0,0 +1,1040 @@ +/* + stepper.c - stepper motor driver: executes motion plans using stepper motors + + Part of grblHAL + + Copyright (c) 2016-2021 Terje Io + Copyright (c) 2011-2016 Sungeun K. Jeon for Gnea Research LLC + Copyright (c) 2009-2011 Simen Svale Skogsrud + + 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 . +*/ + +#include +#include +#include + +#include "hal.h" +#include "protocol.h" +#include "state_machine.h" + +//#include "debug.h" + +#ifndef ACCELERATION_TICKS_PER_SECOND +#define ACCELERATION_TICKS_PER_SECOND 100 +#endif + +// Some useful constants. +#define DT_SEGMENT (1.0f/(ACCELERATION_TICKS_PER_SECOND*60.0f)) // min/segment +#define REQ_MM_INCREMENT_SCALAR 1.25f + +typedef enum { + Ramp_Accel, + Ramp_Cruise, + Ramp_Decel, + Ramp_DecelOverride +} ramp_type_t; + +typedef union { + uint8_t flags; + struct { + uint8_t velocity_profile :1, + hold_partial_block :1, + parking :1, + decel_override :1, + unassigned :4; + }; +} prep_flags_t; + +// Holds the planner block Bresenham algorithm execution data for the segments in the segment +// buffer. Normally, this buffer is partially in-use, but, for the worst case scenario, it will +// never exceed the number of accessible stepper buffer segments (SEGMENT_BUFFER_SIZE-1). +// NOTE: This data is copied from the prepped planner blocks so that the planner blocks may be +// discarded when entirely consumed and completed by the segment buffer. Also, AMASS alters this +// data for its own use. +static st_block_t st_block_buffer[SEGMENT_BUFFER_SIZE - 1]; + +// Primary stepper segment ring buffer. Contains small, short line segments for the stepper +// algorithm to execute, which are "checked-out" incrementally from the first block in the +// planner buffer. Once "checked-out", the steps in the segments buffer cannot be modified by +// the planner, where the remaining planner block steps still can. +static segment_t segment_buffer[SEGMENT_BUFFER_SIZE]; + +// Stepper ISR data struct. Contains the running data for the main stepper ISR. +static stepper_t st; + +#ifdef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING +typedef struct { + uint32_t level_1; + uint32_t level_2; + uint32_t level_3; +} amass_t; + +static amass_t amass; +#endif + +// Message to be output by foreground process +static char *message = NULL; // TODO: do we need a queue for this? + +// Stepper timer ticks per minute +static float cycles_per_min; + +// Step segment ring buffer indices +static volatile segment_t *segment_buffer_tail; +static segment_t *segment_buffer_head, *segment_next_head; + +// Pointers for the step segment being prepped from the planner buffer. Accessed only by the +// main program. Pointers may be planning segments or planner blocks ahead of what being executed. +static plan_block_t *pl_block; // Pointer to the planner block being prepped +static st_block_t *st_prep_block; // Pointer to the stepper block data being prepped +static st_block_t st_hold_block; // Copy of stepper block data for block put on hold during parking + +// Segment preparation data struct. Contains all the necessary information to compute new segments +// based on the current executing planner block. +typedef struct { + prep_flags_t recalculate; + + float dt_remainder; + uint32_t steps_remaining; + float steps_per_mm; + float req_mm_increment; + + st_block_t *last_st_block; + uint32_t last_steps_remaining; + float last_steps_per_mm; + float last_dt_remainder; + + ramp_type_t ramp_type; // Current segment ramp state + float mm_complete; // End of velocity profile from end of current planner block in (mm). + // NOTE: This value must coincide with a step(no mantissa) when converted. + float current_speed; // Current speed at the end of the segment buffer (mm/min) + float maximum_speed; // Maximum speed of executing block. Not always nominal speed. (mm/min) + float exit_speed; // Exit speed of executing block (mm/min) + float accelerate_until; // Acceleration ramp end measured from end of block (mm) + float decelerate_after; // Deceleration ramp start measured from end of block (mm) + float target_position; // + float target_feed; // + float inv_feedrate; // Used by PWM laser mode to speed up segment calculations. + float current_spindle_rpm; +} st_prep_t; + +static st_prep_t prep; + + +/* BLOCK VELOCITY PROFILE DEFINITION + __________________________ + /| |\ _________________ ^ + / | | \ /| |\ | + / | | \ / | | \ s + / | | | | | \ p + / | | | | | \ e + +-----+------------------------+---+--+---------------+----+ e + | BLOCK 1 ^ BLOCK 2 | d + | + time -----> EXAMPLE: Block 2 entry speed is at max junction velocity + + The planner block buffer is planned assuming constant acceleration velocity profiles and are + continuously joined at block junctions as shown above. However, the planner only actively computes + the block entry speeds for an optimal velocity plan, but does not compute the block internal + velocity profiles. These velocity profiles are computed ad-hoc as they are executed by the + stepper algorithm and consists of only 7 possible types of profiles: cruise-only, cruise- + deceleration, acceleration-cruise, acceleration-only, deceleration-only, full-trapezoid, and + triangle(no cruise). + + maximum_speed (< nominal_speed) -> + + +--------+ <- maximum_speed (= nominal_speed) /|\ + / \ / | \ + current_speed -> + \ / | + <- exit_speed + | + <- exit_speed / | | + +-------------+ current_speed -> +----+--+ + time --> ^ ^ ^ ^ + | | | | + decelerate_after(in mm) decelerate_after(in mm) + ^ ^ ^ ^ + | | | | + accelerate_until(in mm) accelerate_until(in mm) + + The step segment buffer computes the executing block velocity profile and tracks the critical + parameters for the stepper algorithm to accurately trace the profile. These critical parameters + are shown and defined in the above illustration. +*/ + +// + +// Output message in sync with motion, called by foreground process. +static void output_message (sys_state_t state) +{ + if(message) { + report_message(message, Message_Plain); + free(message); + message = NULL; + } +} + +// Callback from delay to deenergize steppers after movement, might been cancelled +void st_deenergize () +{ + if(sys.steppers_deenergize) { + hal.stepper.enable(settings.steppers.deenergize); + sys.steppers_deenergize = false; + } +} + + +// Stepper state initialization. Cycle should only start if the st.cycle_start flag is +// enabled. Startup init and limits call this function but shouldn't start the cycle. +void st_wake_up () +{ + if(sys.steppers_deenergize) { + sys.steppers_deenergize = false; +// hal.delay_ms(0, st_deenergize); // Cancel any pending steppers deenergize + } + + // Initialize stepper data to ensure first ISR call does not step and + // cancel any pending steppers deenergize + //st.exec_block = NULL; + + hal.stepper.wake_up(); +} + + +// Stepper shutdown +ISR_CODE void st_go_idle () +{ + // Disable Stepper Driver Interrupt. Allow Stepper Port Reset Interrupt to finish, if active. + + sys_state_t state = state_get(); + + hal.stepper.go_idle(false); + + // Set stepper driver idle state, disabled or enabled, depending on settings and circumstances. + if (((settings.steppers.idle_lock_time != 255) || sys.rt_exec_alarm || state == STATE_SLEEP) && state != STATE_HOMING) { + // Force stepper dwell to lock axes for a defined amount of time to ensure the axes come to a complete + // stop and not drift from residual inertial forces at the end of the last movement. + sys.steppers_deenergize = true; + hal.delay_ms(settings.steppers.idle_lock_time, st_deenergize); + } else + hal.stepper.enable(settings.steppers.idle_lock_time == 255 ? (axes_signals_t){AXES_BITMASK} : settings.steppers.deenergize); +} + + +/* "The Stepper Driver Interrupt" - This timer interrupt is the workhorse of Grbl. Grbl employs + the venerable Bresenham line algorithm to manage and exactly synchronize multi-axis moves. + Unlike the popular DDA algorithm, the Bresenham algorithm is not susceptible to numerical + round-off errors and only requires fast integer counters, meaning low computational overhead + and maximizing the microcontrollers capabilities. However, the downside of the Bresenham algorithm + is, for certain multi-axis motions, the non-dominant axes may suffer from un-smooth step + pulse trains, or aliasing, which can lead to strange audible noises or shaking. This is + particularly noticeable or may cause motion issues at low step frequencies (0-5kHz), but + is usually not a physical problem at higher frequencies, although audible. + To improve Bresenham multi-axis performance, Grbl uses what we call an Adaptive Multi-Axis + Step Smoothing (AMASS) algorithm, which does what the name implies. At lower step frequencies, + AMASS artificially increases the Bresenham resolution without effecting the algorithm's + innate exactness. AMASS adapts its resolution levels automatically depending on the step + frequency to be executed, meaning that for even lower step frequencies the step smoothing + level increases. Algorithmically, AMASS is acheived by a simple bit-shifting of the Bresenham + step count for each AMASS level. For example, for a Level 1 step smoothing, we bit shift + the Bresenham step event count, effectively multiplying it by 2, while the axis step counts + remain the same, and then double the stepper ISR frequency. In effect, we are allowing the + non-dominant Bresenham axes step in the intermediate ISR tick, while the dominant axis is + stepping every two ISR ticks, rather than every ISR tick in the traditional sense. At AMASS + Level 2, we simply bit-shift again, so the non-dominant Bresenham axes can step within any + of the four ISR ticks, the dominant axis steps every four ISR ticks, and quadruple the + stepper ISR frequency. And so on. This, in effect, virtually eliminates multi-axis aliasing + issues with the Bresenham algorithm and does not significantly alter Grbl's performance, but + in fact, more efficiently utilizes unused CPU cycles overall throughout all configurations. + AMASS retains the Bresenham algorithm exactness by requiring that it always executes a full + Bresenham step, regardless of AMASS Level. Meaning that for an AMASS Level 2, all four + intermediate steps must be completed such that baseline Bresenham (Level 0) count is always + retained. Similarly, AMASS Level 3 means all eight intermediate steps must be executed. + Although the AMASS Levels are in reality arbitrary, where the baseline Bresenham counts can + be multiplied by any integer value, multiplication by powers of two are simply used to ease + CPU overhead with bitshift integer operations. + This interrupt is simple and dumb by design. All the computational heavy-lifting, as in + determining accelerations, is performed elsewhere. This interrupt pops pre-computed segments, + defined as constant velocity over n number of steps, from the step segment buffer and then + executes them by pulsing the stepper pins appropriately via the Bresenham algorithm. This + ISR is supported by The Stepper Port Reset Interrupt which it uses to reset the stepper port + after each pulse. The bresenham line tracer algorithm controls all stepper outputs + simultaneously with these two interrupts. + + NOTE: This interrupt must be as efficient as possible and complete before the next ISR tick, + which for Grbl must be less than 33.3usec (@30kHz ISR rate). Oscilloscope measured time in + ISR is 5usec typical and 25usec maximum, well below requirement. + NOTE: This ISR expects at least one step to be executed per segment. +*/ +ISR_CODE void stepper_driver_interrupt_handler (void) +{ +#ifdef ENABLE_BACKLASH_COMPENSATION + static bool backlash_motion; +#endif + + // Start a step pulse when there is a block to execute. + if(st.exec_block) { + + hal.stepper.pulse_start(&st); + + st.new_block = st.dir_change = false; + + if (st.step_count == 0) // Segment is complete. Discard current segment. + st.exec_segment = NULL; + } + + // If there is no step segment, attempt to pop one from the stepper buffer + if (st.exec_segment == NULL) { + // Anything in the buffer? If so, load and initialize next step segment. + if (segment_buffer_tail != segment_buffer_head) { + + // Initialize new step segment and load number of steps to execute + st.exec_segment = (segment_t *)segment_buffer_tail; + + // Initialize step segment timing per step and load number of steps to execute. + hal.stepper.cycles_per_tick(st.exec_segment->cycles_per_tick); + st.step_count = st.exec_segment->n_step; // NOTE: Can sometimes be zero when moving slow. + + // If the new segment starts a new planner block, initialize stepper variables and counters. + if (st.exec_block != st.exec_segment->exec_block) { + + if((st.dir_change = st.exec_block == NULL || st.dir_outbits.value != st.exec_segment->exec_block->direction_bits.value)) + st.dir_outbits = st.exec_segment->exec_block->direction_bits; + st.exec_block = st.exec_segment->exec_block; + st.step_event_count = st.exec_block->step_event_count; + st.new_block = true; +#ifdef ENABLE_BACKLASH_COMPENSATION + backlash_motion = st.exec_block->backlash_motion; +#endif + + if(st.exec_block->overrides.sync) + sys.override.control = st.exec_block->overrides; + + // Execute output commands to be syncronized with motion + while(st.exec_block->output_commands) { + output_command_t *cmd = st.exec_block->output_commands; + cmd->is_executed = true; + if(cmd->is_digital) + hal.port.digital_out(cmd->port, cmd->value != 0.0f); + else + hal.port.analog_out(cmd->port, cmd->value); + st.exec_block->output_commands = cmd->next; + } + + // Enqueue any message to be printed (by foreground process) + if(st.exec_block->message) { + if(message == NULL) { + message = st.exec_block->message; + protocol_enqueue_rt_command(output_message); + } else + free(st.exec_block->message); // + st.exec_block->message = NULL; + } + + // Initialize Bresenham line and distance counters + st.counter_x = st.counter_y = st.counter_z + #ifdef A_AXIS + = st.counter_a + #endif + #ifdef B_AXIS + = st.counter_b + #endif + #ifdef C_AXIS + = st.counter_c + #endif + = st.step_event_count >> 1; + + #ifndef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING + memcpy(st.steps, st.exec_block->steps, sizeof(st.steps)); + #endif + } + + #ifdef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING + // With AMASS enabled, adjust Bresenham axis increment counters according to AMASS level. + st.amass_level = st.exec_segment->amass_level; + st.steps[X_AXIS] = st.exec_block->steps[X_AXIS] >> st.amass_level; + st.steps[Y_AXIS] = st.exec_block->steps[Y_AXIS] >> st.amass_level; + st.steps[Z_AXIS] = st.exec_block->steps[Z_AXIS] >> st.amass_level; + #ifdef A_AXIS + st.steps[A_AXIS] = st.exec_block->steps[A_AXIS] >> st.amass_level; + #endif + #ifdef B_AXIS + st.steps[B_AXIS] = st.exec_block->steps[B_AXIS] >> st.amass_level; + #endif + #ifdef C_AXIS + st.steps[C_AXIS] = st.exec_block->steps[C_AXIS] >> st.amass_level; + #endif + #endif + + if(st.exec_segment->update_rpm) { + #ifdef SPINDLE_PWM_DIRECT + hal.spindle.update_pwm(st.exec_segment->spindle_pwm); + #else + hal.spindle.update_rpm(st.exec_segment->spindle_rpm); + #endif + } + } else { + // Segment buffer empty. Shutdown. + st_go_idle(); + // Ensure pwm is set properly upon completion of rate-controlled motion. + if (st.exec_block->dynamic_rpm && settings.mode == Mode_Laser) + hal.spindle.set_state((spindle_state_t){0}, 0.0f); + + st.exec_block = NULL; + system_set_exec_state_flag(EXEC_CYCLE_COMPLETE); // Flag main program for cycle complete + + return; // Nothing to do but exit. + } + } + + // Check probing state. + // Monitors probe pin state and records the system position when detected. + // NOTE: This function must be extremely efficient as to not bog down the stepper ISR. + if (sys.probing_state == Probing_Active && hal.probe.get_state().triggered) { + sys.probing_state = Probing_Off; + memcpy(sys.probe_position, sys.position, sizeof(sys.position)); + bit_true(sys.rt_exec_state, EXEC_MOTION_CANCEL); + } + + register axes_signals_t step_outbits = (axes_signals_t){0}; + + // Execute step displacement profile by Bresenham line algorithm + + st.counter_x += st.steps[X_AXIS]; + if (st.counter_x > st.step_event_count) { + step_outbits.x = On; + st.counter_x -= st.step_event_count; +#ifdef ENABLE_BACKLASH_COMPENSATION + if(!backlash_motion) +#endif + sys.position[X_AXIS] = sys.position[X_AXIS] + (st.dir_outbits.x ? -1 : 1); + } + + st.counter_y += st.steps[Y_AXIS]; + if (st.counter_y > st.step_event_count) { + step_outbits.y = On; + st.counter_y -= st.step_event_count; +#ifdef ENABLE_BACKLASH_COMPENSATION + if(!backlash_motion) +#endif + sys.position[Y_AXIS] = sys.position[Y_AXIS] + (st.dir_outbits.y ? -1 : 1); + } + + st.counter_z += st.steps[Z_AXIS]; + if (st.counter_z > st.step_event_count) { + step_outbits.z = On; + st.counter_z -= st.step_event_count; +#ifdef ENABLE_BACKLASH_COMPENSATION + if(!backlash_motion) +#endif + sys.position[Z_AXIS] = sys.position[Z_AXIS] + (st.dir_outbits.z ? -1 : 1); + } + + #ifdef A_AXIS + st.counter_a += st.steps[A_AXIS]; + if (st.counter_a > st.step_event_count) { + step_outbits.a = On; + st.counter_a -= st.step_event_count; +#ifdef ENABLE_BACKLASH_COMPENSATION + if(!backlash_motion) +#endif + sys.position[A_AXIS] = sys.position[A_AXIS] + (st.dir_outbits.a ? -1 : 1); + } + #endif + + #ifdef B_AXIS + st.counter_b += st.steps[B_AXIS]; + if (st.counter_b > st.step_event_count) { + step_outbits.b = On; + st.counter_b -= st.step_event_count; +#ifdef ENABLE_BACKLASH_COMPENSATION + if(!backlash_motion) +#endif + sys.position[B_AXIS] = sys.position[B_AXIS] + (st.dir_outbits.b ? -1 : 1); + } + #endif + + #ifdef C_AXIS + st.counter_c += st.steps[C_AXIS]; + if (st.counter_c > st.step_event_count) { + step_outbits.c = On; + st.counter_c -= st.step_event_count; +#ifdef ENABLE_BACKLASH_COMPENSATION + if(!backlash_motion) +#endif + sys.position[C_AXIS] = sys.position[C_AXIS] + (st.dir_outbits.c ? -1 : 1); + } + #endif + + st.step_outbits.value = step_outbits.value; + + // During a homing cycle, lock out and prevent desired axes from moving. + if (state_get() == STATE_HOMING) + st.step_outbits.value &= sys.homing_axis_lock.mask; + + if (st.step_count == 0 || --st.step_count == 0) { + // Segment is complete. Advance segment tail pointer. + segment_buffer_tail = segment_buffer_tail->next; + } +} + +// Reset and clear stepper subsystem variables +void st_reset () +{ + if(hal.probe.configure) + hal.probe.configure(false, false); + + if(message) { + free(message); + message = NULL; + } + + // Initialize stepper driver idle state, clear step and direction port pins. + st_go_idle(); + // hal.stepper.go_idle(true); + + // NOTE: buffer indices starts from 1 for simpler driver coding! + + // Set up stepper block ringbuffer as circular linked list and add id + uint_fast8_t idx; + for(idx = 0 ; idx <= SEGMENT_BUFFER_SIZE - 2 ; idx++) { + st_block_buffer[idx].next = &st_block_buffer[idx == SEGMENT_BUFFER_SIZE - 2 ? 0 : idx + 1]; + st_block_buffer[idx].id = idx + 1; + } + + // Set up segments ringbuffer as circular linked list, add id and clear AMASS level + for(idx = 0 ; idx <= SEGMENT_BUFFER_SIZE - 1 ; idx++) { + segment_buffer[idx].next = &segment_buffer[idx == SEGMENT_BUFFER_SIZE - 1 ? 0 : idx + 1]; + segment_buffer[idx].id = idx + 1; + segment_buffer[idx].amass_level = 0; + } + + st_prep_block = &st_block_buffer[0]; + + // Initialize stepper algorithm variables. + pl_block = NULL; // Planner block pointer used by segment buffer + segment_buffer_tail = segment_buffer_head = &segment_buffer[0]; // empty = tail + segment_next_head = segment_buffer_head->next; + + memset(&prep, 0, sizeof(st_prep_t)); + memset(&st, 0, sizeof(stepper_t)); + +#ifdef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING + // TODO: move to driver? + // AMASS_LEVEL0: Normal operation. No AMASS. No upper cutoff frequency. Starts at LEVEL1 cutoff frequency. + // Defined as step timer frequency / Cutoff frequency in Hz + amass.level_1 = hal.f_step_timer / 8000; + amass.level_2 = hal.f_step_timer / 4000; + amass.level_3 = hal.f_step_timer / 2000; +#endif + + cycles_per_min = (float)hal.f_step_timer * 60.0f; +} + +// Called by spindle_set_state() to inform about RPM changes. +// Used by st_prep_buffer() to determine if spindle needs update when dynamic RPM is called for. +void st_rpm_changed (float rpm) +{ + prep.current_spindle_rpm = rpm; +} + +// Called by planner_recalculate() when the executing block is updated by the new plan. +void st_update_plan_block_parameters () +{ + if (pl_block != NULL) { // Ignore if at start of a new block. + prep.recalculate.velocity_profile = On; + pl_block->entry_speed_sqr = prep.current_speed * prep.current_speed; // Update entry speed. + pl_block = NULL; // Flag st_prep_segment() to load and check active velocity profile. + } +} + +// Changes the run state of the step segment buffer to execute the special parking motion. +void st_parking_setup_buffer() +{ + // Store step execution data of partially completed block, if necessary. + if (prep.recalculate.hold_partial_block && !prep.recalculate.parking) { + prep.last_st_block = st_prep_block; + memcpy(&st_hold_block, st_prep_block, sizeof(st_block_t)); + prep.last_steps_remaining = prep.steps_remaining; + prep.last_dt_remainder = prep.dt_remainder; + prep.last_steps_per_mm = prep.steps_per_mm; + } + // Set flags to execute a parking motion + prep.recalculate.parking = On; + prep.recalculate.velocity_profile = Off; + pl_block = NULL; // Always reset parking motion to reload new block. +} + + +// Restores the step segment buffer to the normal run state after a parking motion. +void st_parking_restore_buffer() +{ + // Restore step execution data and flags of partially completed block, if necessary. + if (prep.recalculate.hold_partial_block) { + memcpy(prep.last_st_block, &st_hold_block, sizeof(st_block_t)); + st_prep_block = prep.last_st_block; + prep.steps_remaining = prep.last_steps_remaining; + prep.dt_remainder = prep.last_dt_remainder; + prep.steps_per_mm = prep.last_steps_per_mm; + prep.recalculate.flags = 0; + prep.recalculate.hold_partial_block = prep.recalculate.velocity_profile = On; + prep.req_mm_increment = REQ_MM_INCREMENT_SCALAR / prep.steps_per_mm; // Recompute this value. + } else + prep.recalculate.flags = 0; + + pl_block = NULL; // Set to reload next block. +} + +/* Prepares step segment buffer. Continuously called from main program. + + The segment buffer is an intermediary buffer interface between the execution of steps + by the stepper algorithm and the velocity profiles generated by the planner. The stepper + algorithm only executes steps within the segment buffer and is filled by the main program + when steps are "checked-out" from the first block in the planner buffer. This keeps the + step execution and planning optimization processes atomic and protected from each other. + The number of steps "checked-out" from the planner buffer and the number of segments in + the segment buffer is sized and computed such that no operation in the main program takes + longer than the time it takes the stepper algorithm to empty it before refilling it. + Currently, the segment buffer conservatively holds roughly up to 40-50 msec of steps. + NOTE: Computation units are in steps, millimeters, and minutes. +*/ +void st_prep_buffer() +{ + // Block step prep buffer, while in a suspend state and there is no suspend motion to execute. + if (sys.step_control.end_motion) + return; + + while (segment_buffer_tail != segment_next_head) { // Check if we need to fill the buffer. + + // Determine if we need to load a new planner block or if the block needs to be recomputed. + if (pl_block == NULL) { + + // Query planner for a queued block + + pl_block = sys.step_control.execute_sys_motion ? plan_get_system_motion_block() : plan_get_current_block(); + + if (pl_block == NULL) + return; // No planner blocks. Exit. + + // Check if we need to only recompute the velocity profile or load a new block. + if (prep.recalculate.velocity_profile) { + if(settings.parking.flags.enabled) { + if (prep.recalculate.parking) + prep.recalculate.velocity_profile = Off; + else + prep.recalculate.flags = 0; + } else + prep.recalculate.flags = 0; + } else { + + // Prepare and copy Bresenham algorithm segment data from the new planner block, so that + // when the segment buffer completes the planner block, it may be discarded when the + // segment buffer finishes the prepped block, but the stepper ISR is still executing it. + + st_prep_block = st_prep_block->next; + + uint_fast8_t idx = N_AXIS; + #ifndef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING + do { + idx--; + st_prep_block->steps[idx] = (pl_block->steps[idx] << 1); + } while(idx); + st_prep_block->step_event_count = (pl_block->step_event_count << 1); + #else + // With AMASS enabled, simply bit-shift multiply all Bresenham data by the max AMASS + // level, such that we never divide beyond the original data anywhere in the algorithm. + // If the original data is divided, we can lose a step from integer roundoff. + do { + idx--; + st_prep_block->steps[idx] = pl_block->steps[idx] << MAX_AMASS_LEVEL; + } while(idx); + st_prep_block->step_event_count = pl_block->step_event_count << MAX_AMASS_LEVEL; + #endif + + st_prep_block->direction_bits = pl_block->direction_bits; + st_prep_block->programmed_rate = pl_block->programmed_rate; + st_prep_block->millimeters = pl_block->millimeters; + st_prep_block->steps_per_mm = (float)pl_block->step_event_count / pl_block->millimeters; + st_prep_block->output_commands = pl_block->output_commands; + st_prep_block->overrides = pl_block->overrides; + st_prep_block->backlash_motion = pl_block->condition.backlash_motion; + st_prep_block->message = pl_block->message; + pl_block->message= NULL; + + // Initialize segment buffer data for generating the segments. + prep.steps_per_mm = st_prep_block->steps_per_mm; + prep.steps_remaining = pl_block->step_event_count; + prep.req_mm_increment = REQ_MM_INCREMENT_SCALAR / prep.steps_per_mm; + prep.dt_remainder = prep.target_position = 0.0f; // Reset for new segment block + + if (sys.step_control.execute_hold || prep.recalculate.decel_override) { + // New block loaded mid-hold. Override planner block entry speed to enforce deceleration. + prep.current_speed = prep.exit_speed; + pl_block->entry_speed_sqr = prep.exit_speed * prep.exit_speed; + prep.recalculate.decel_override = Off; + } else + prep.current_speed = sqrtf(pl_block->entry_speed_sqr); + + // Setup laser mode variables. RPM rate adjusted motions will always complete a motion with the + // spindle off. + if ((st_prep_block->dynamic_rpm = pl_block->condition.is_rpm_rate_adjusted)) + // Pre-compute inverse programmed rate to speed up RPM updating per step segment. + prep.inv_feedrate = pl_block->condition.is_laser_ppi_mode ? 1.0f : 1.0f / pl_block->programmed_rate; + else + st_prep_block->dynamic_rpm = pl_block->condition.is_rpm_pos_adjusted; + } + + /* --------------------------------------------------------------------------------- + Compute the velocity profile of a new planner block based on its entry and exit + speeds, or recompute the profile of a partially-completed planner block if the + planner has updated it. For a commanded forced-deceleration, such as from a feed + hold, override the planner velocities and decelerate to the target exit speed. + */ + prep.mm_complete = 0.0f; // Default velocity profile complete at 0.0mm from end of block. + float inv_2_accel = 0.5f / pl_block->acceleration; + + if (sys.step_control.execute_hold) { // [Forced Deceleration to Zero Velocity] + // Compute velocity profile parameters for a feed hold in-progress. This profile overrides + // the planner block profile, enforcing a deceleration to zero speed. + prep.ramp_type = Ramp_Decel; + // Compute decelerate distance relative to end of block. + float decel_dist = pl_block->millimeters - inv_2_accel * pl_block->entry_speed_sqr; + if (decel_dist < 0.0f) { + // Deceleration through entire planner block. End of feed hold is not in this block. + prep.exit_speed = sqrtf(pl_block->entry_speed_sqr - 2.0f * pl_block->acceleration * pl_block->millimeters); + } else { + prep.mm_complete = decel_dist; // End of feed hold. + prep.exit_speed = 0.0f; + } + } else { // [Normal Operation] + // Compute or recompute velocity profile parameters of the prepped planner block. + prep.ramp_type = Ramp_Accel; // Initialize as acceleration ramp. + prep.accelerate_until = pl_block->millimeters; + + float exit_speed_sqr; + if (sys.step_control.execute_sys_motion) + prep.exit_speed = exit_speed_sqr = 0.0f; // Enforce stop at end of system motion. + else { + exit_speed_sqr = plan_get_exec_block_exit_speed_sqr(); + prep.exit_speed = sqrtf(exit_speed_sqr); + } + + float nominal_speed = plan_compute_profile_nominal_speed(pl_block); + float nominal_speed_sqr = nominal_speed * nominal_speed; + float intersect_distance = 0.5f * (pl_block->millimeters + inv_2_accel * (pl_block->entry_speed_sqr - exit_speed_sqr)); + + prep.target_feed = nominal_speed; + + if (pl_block->entry_speed_sqr > nominal_speed_sqr) { // Only occurs during override reductions. + + prep.accelerate_until = pl_block->millimeters - inv_2_accel * (pl_block->entry_speed_sqr - nominal_speed_sqr); + + if (prep.accelerate_until <= 0.0f) { // Deceleration-only. + prep.ramp_type = Ramp_Decel; + // prep.decelerate_after = pl_block->millimeters; + // prep.maximum_speed = prep.current_speed; + + // Compute override block exit speed since it doesn't match the planner exit speed. + prep.exit_speed = sqrtf(pl_block->entry_speed_sqr - 2.0f * pl_block->acceleration * pl_block->millimeters); + prep.recalculate.decel_override = On; // Flag to load next block as deceleration override. + + // TODO: Determine correct handling of parameters in deceleration-only. + // Can be tricky since entry speed will be current speed, as in feed holds. + // Also, look into near-zero speed handling issues with this. + + } else { + // Decelerate to cruise or cruise-decelerate types. Guaranteed to intersect updated plan. + prep.decelerate_after = inv_2_accel * (nominal_speed_sqr - exit_speed_sqr); // Should always be >= 0.0 due to planner reinit. + prep.maximum_speed = nominal_speed; + prep.ramp_type = Ramp_DecelOverride; + } + } else if (intersect_distance > 0.0f) { + if (intersect_distance < pl_block->millimeters) { // Either trapezoid or triangle types + // NOTE: For acceleration-cruise and cruise-only types, following calculation will be 0.0. + prep.decelerate_after = inv_2_accel * (nominal_speed_sqr - exit_speed_sqr); + if (prep.decelerate_after < intersect_distance) { // Trapezoid type + prep.maximum_speed = nominal_speed; + if (pl_block->entry_speed_sqr == nominal_speed_sqr) { + // Cruise-deceleration or cruise-only type. + prep.ramp_type = Ramp_Cruise; + } else { + // Full-trapezoid or acceleration-cruise types + prep.accelerate_until -= inv_2_accel * (nominal_speed_sqr - pl_block->entry_speed_sqr); + } + } else { // Triangle type + prep.accelerate_until = prep.decelerate_after = intersect_distance; + prep.maximum_speed = sqrtf(2.0f * pl_block->acceleration * intersect_distance + exit_speed_sqr); + } + } else { // Deceleration-only type + prep.ramp_type = Ramp_Decel; + // prep.decelerate_after = pl_block->millimeters; + // prep.maximum_speed = prep.current_speed; + } + } else { // Acceleration-only type + prep.accelerate_until = 0.0f; + // prep.decelerate_after = 0.0f; + prep.maximum_speed = prep.exit_speed; + } + } + + if(state_get() != STATE_HOMING) + sys.step_control.update_spindle_rpm |= (settings.mode == Mode_Laser); // Force update whenever updating block in laser mode. + } + + // Initialize new segment + segment_t *prep_segment = segment_buffer_head; + + // Set new segment to point to the current segment data block. + prep_segment->exec_block = st_prep_block; + prep_segment->update_rpm = false; + + /*------------------------------------------------------------------------------------ + Compute the average velocity of this new segment by determining the total distance + traveled over the segment time DT_SEGMENT. The following code first attempts to create + a full segment based on the current ramp conditions. If the segment time is incomplete + when terminating at a ramp state change, the code will continue to loop through the + progressing ramp states to fill the remaining segment execution time. However, if + an incomplete segment terminates at the end of the velocity profile, the segment is + considered completed despite having a truncated execution time less than DT_SEGMENT. + The velocity profile is always assumed to progress through the ramp sequence: + acceleration ramp, cruising state, and deceleration ramp. Each ramp's travel distance + may range from zero to the length of the block. Velocity profiles can end either at + the end of planner block (typical) or mid-block at the end of a forced deceleration, + such as from a feed hold. + */ + float dt_max = DT_SEGMENT; // Maximum segment time + float dt = 0.0f; // Initialize segment time + float time_var = dt_max; // Time worker variable + float mm_var; // mm - Distance worker variable + float speed_var; // Speed worker variable + float mm_remaining = pl_block->millimeters; // New segment distance from end of block. + float minimum_mm = mm_remaining - prep.req_mm_increment; // Guarantee at least one step. + + if (minimum_mm < 0.0f) + minimum_mm = 0.0f; + + do { + + switch (prep.ramp_type) { + + case Ramp_DecelOverride: + speed_var = pl_block->acceleration * time_var; + if ((prep.current_speed - prep.maximum_speed) <= speed_var) { + // Cruise or cruise-deceleration types only for deceleration override. + mm_remaining = prep.accelerate_until; + time_var = 2.0f * (pl_block->millimeters - mm_remaining) / (prep.current_speed + prep.maximum_speed); + prep.ramp_type = Ramp_Cruise; + prep.current_speed = prep.maximum_speed; + } else {// Mid-deceleration override ramp. + mm_remaining -= time_var * (prep.current_speed - 0.5f * speed_var); + prep.current_speed -= speed_var; + } + break; + + case Ramp_Accel: + // NOTE: Acceleration ramp only computes during first do-while loop. + speed_var = pl_block->acceleration * time_var; + mm_remaining -= time_var * (prep.current_speed + 0.5f * speed_var); + if (mm_remaining < prep.accelerate_until) { // End of acceleration ramp. + // Acceleration-cruise, acceleration-deceleration ramp junction, or end of block. + mm_remaining = prep.accelerate_until; // NOTE: 0.0 at EOB + time_var = 2.0f * (pl_block->millimeters - mm_remaining) / (prep.current_speed + prep.maximum_speed); + prep.ramp_type = mm_remaining == prep.decelerate_after ? Ramp_Decel : Ramp_Cruise; + prep.current_speed = prep.maximum_speed; + } else // Acceleration only. + prep.current_speed += speed_var; + break; + + case Ramp_Cruise: + // NOTE: mm_var used to retain the last mm_remaining for incomplete segment time_var calculations. + // NOTE: If maximum_speed*time_var value is too low, round-off can cause mm_var to not change. To + // prevent this, simply enforce a minimum speed threshold in the planner. + mm_var = mm_remaining - prep.maximum_speed * time_var; + if (mm_var < prep.decelerate_after) { // End of cruise. + // Cruise-deceleration junction or end of block. + time_var = (mm_remaining - prep.decelerate_after) / prep.maximum_speed; + mm_remaining = prep.decelerate_after; // NOTE: 0.0 at EOB + prep.ramp_type = Ramp_Decel; + } else // Cruising only. + mm_remaining = mm_var; + break; + + default: // case Ramp_Decel: + // NOTE: mm_var used as a misc worker variable to prevent errors when near zero speed. + speed_var = pl_block->acceleration * time_var; // Used as delta speed (mm/min) + if (prep.current_speed > speed_var) { // Check if at or below zero speed. + // Compute distance from end of segment to end of block. + mm_var = mm_remaining - time_var * (prep.current_speed - 0.5f * speed_var); // (mm) + if (mm_var > prep.mm_complete) { // Typical case. In deceleration ramp. + mm_remaining = mm_var; + prep.current_speed -= speed_var; + break; // Segment complete. Exit switch-case statement. Continue do-while loop. + } + } + // Otherwise, at end of block or end of forced-deceleration. + time_var = 2.0f * (mm_remaining - prep.mm_complete) / (prep.current_speed + prep.exit_speed); + mm_remaining = prep.mm_complete; + prep.current_speed = prep.exit_speed; + } + + dt += time_var; // Add computed ramp time to total segment time. + + if (dt < dt_max) + time_var = dt_max - dt;// **Incomplete** At ramp junction. + else { + if (mm_remaining > minimum_mm) { // Check for very slow segments with zero steps. + // Increase segment time to ensure at least one step in segment. Override and loop + // through distance calculations until minimum_mm or mm_complete. + dt_max += DT_SEGMENT; + time_var = dt_max - dt; + } else + break; // **Complete** Exit loop. Segment execution time maxed. + } + + } while (mm_remaining > prep.mm_complete); // **Complete** Exit loop. Profile complete. + + /* ----------------------------------------------------------------------------------- + Compute spindle spindle speed for step segment + */ + + if (sys.step_control.update_spindle_rpm || st_prep_block->dynamic_rpm) { + float rpm; + if (pl_block->condition.spindle.on) { + // NOTE: Feed and rapid overrides are independent of PWM value and do not alter laser power/rate. + // If current_speed is zero, then may need to be rpm_min*(100/MAX_SPINDLE_RPM_OVERRIDE) + // but this would be instantaneous only and during a motion. May not matter at all. + rpm = spindle_set_rpm(pl_block->condition.is_rpm_rate_adjusted && !pl_block->condition.is_laser_ppi_mode + ? pl_block->spindle.rpm * prep.current_speed * prep.inv_feedrate + : pl_block->spindle.rpm, sys.override.spindle_rpm); + + if(pl_block->condition.is_rpm_pos_adjusted) { + float npos = (float)(pl_block->step_event_count - prep.steps_remaining) / (float)pl_block->step_event_count; + rpm += (spindle_set_rpm(pl_block->spindle.css.target_rpm, sys.override.spindle_rpm) - prep.current_spindle_rpm) * npos; + } + } else + sys.spindle_rpm = rpm = 0.0f; + + if(rpm != prep.current_spindle_rpm) { + #ifdef SPINDLE_PWM_DIRECT + prep.current_spindle_rpm = rpm; + prep_segment->spindle_pwm = hal.spindle.get_pwm(rpm); + #else + prep.current_spindle_rpm = prep_segment->spindle_rpm = rpm; + #endif + prep_segment->update_rpm = true; + sys.step_control.update_spindle_rpm = Off; + } + } + + /* ----------------------------------------------------------------------------------- + Compute segment step rate, steps to execute, and apply necessary rate corrections. + NOTE: Steps are computed by direct scalar conversion of the millimeter distance + remaining in the block, rather than incrementally tallying the steps executed per + segment. This helps in removing floating point round-off issues of several additions. + However, since floats have only 7.2 significant digits, long moves with extremely + high step counts can exceed the precision of floats, which can lead to lost steps. + Fortunately, this scenario is highly unlikely and unrealistic in CNC machines + supported by Grbl (i.e. exceeding 10 meters axis travel at 200 step/mm). + */ + float step_dist_remaining = prep.steps_per_mm * mm_remaining; // Convert mm_remaining to steps + uint32_t n_steps_remaining = (uint32_t)ceilf(step_dist_remaining); // Round-up current steps remaining + + prep_segment->n_step = (uint_fast16_t)(prep.steps_remaining - n_steps_remaining); // Compute number of steps to execute. + + // Bail if we are at the end of a feed hold and don't have a step to execute. + if (prep_segment->n_step == 0 && sys.step_control.execute_hold) { + // Less than one step to decelerate to zero speed, but already very close. AMASS + // requires full steps to execute. So, just bail. + sys.step_control.end_motion = On; + if (settings.parking.flags.enabled && !prep.recalculate.parking) + prep.recalculate.hold_partial_block = On; + return; // Segment not generated, but current step data still retained. + } + + // Compute segment step rate. Since steps are integers and mm distances traveled are not, + // the end of every segment can have a partial step of varying magnitudes that are not + // executed, because the stepper ISR requires whole steps due to the AMASS algorithm. To + // compensate, we track the time to execute the previous segment's partial step and simply + // apply it with the partial step distance to the current segment, so that it minutely + // adjusts the whole segment rate to keep step output exact. These rate adjustments are + // typically very small and do not adversely effect performance, but ensures that Grbl + // outputs the exact acceleration and velocity profiles as computed by the planner. + dt += prep.dt_remainder; // Apply previous segment partial step execute time + float inv_rate = dt / ((float)prep.steps_remaining - step_dist_remaining); // Compute adjusted step rate inverse + + // Compute timer ticks per step for the prepped segment. + uint32_t cycles = (uint32_t)ceilf(cycles_per_min * inv_rate); // (cycles/step) + + // Record end position of segment relative to block if spindle synchronized motion + if((prep_segment->spindle_sync = pl_block->condition.spindle.synchronized)) { + prep.target_position += dt * prep.target_feed; + prep_segment->cruising = prep.ramp_type == Ramp_Cruise; + prep_segment->target_position = prep.target_position; //st_prep_block->millimeters - pl_block->millimeters; + } + + #ifdef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING + // Compute step timing and multi-axis smoothing level. + // NOTE: AMASS overdrives the timer with each level, so only one prescalar is required. + if (cycles < amass.level_1) + prep_segment->amass_level = 0; + else { + prep_segment->amass_level = cycles < amass.level_2 ? 1 : (cycles < amass.level_3 ? 2 : 3); + cycles >>= prep_segment->amass_level; + prep_segment->n_step <<= prep_segment->amass_level; + } + #endif + + prep_segment->cycles_per_tick = cycles; + prep_segment->current_rate = prep.current_speed; + + // Segment complete! Increment segment pointers, so stepper ISR can immediately execute it. + segment_buffer_head = segment_next_head; + segment_next_head = segment_next_head->next; + + // Update the appropriate planner and segment data. + pl_block->millimeters = mm_remaining; + prep.steps_remaining = n_steps_remaining; + prep.dt_remainder = ((float)n_steps_remaining - step_dist_remaining) * inv_rate; + + // Check for exit conditions and flag to load next planner block. + if (mm_remaining <= prep.mm_complete) { + + // End of planner block or forced-termination. No more distance to be executed. + if (mm_remaining > 0.0f) { // At end of forced-termination. + // Reset prep parameters for resuming and then bail. Allow the stepper ISR to complete + // the segment queue, where realtime protocol will set new state upon receiving the + // cycle stop flag from the ISR. Prep_segment is blocked until then. + sys.step_control.end_motion = On; + if (settings.parking.flags.enabled && !prep.recalculate.parking) + prep.recalculate.hold_partial_block = On; + return; // Bail! + } else { // End of planner block + // The planner block is complete. All steps are set to be executed in the segment buffer. + if (sys.step_control.execute_sys_motion) { + sys.step_control.end_motion = On; + return; + } + pl_block = NULL; // Set pointer to indicate check and load next planner block. + plan_discard_current_block(); + } + } + } +} + + +// Called by realtime status reporting to fetch the current speed being executed. This value +// however is not exactly the current speed, but the speed computed in the last step segment +// in the segment buffer. It will always be behind by up to the number of segment blocks (-1) +// divided by the ACCELERATION TICKS PER SECOND in seconds. +float st_get_realtime_rate() +{ + return state_get() & (STATE_CYCLE|STATE_HOMING|STATE_HOLD|STATE_JOG|STATE_SAFETY_DOOR) ? prep.current_speed : 0.0f; +} diff --git a/stepper.h b/stepper.h new file mode 100644 index 0000000..e4e47a2 --- /dev/null +++ b/stepper.h @@ -0,0 +1,139 @@ +/* + stepper.h - stepper motor driver: executes motion plans of planner.c using the stepper motors + + Part of grblHAL + + Copyright (c) 2019-2020 Terje Io + Copyright (c) 2011-2016 Sungeun K. Jeon for Gnea Research LLC + Copyright (c) 2009-2011 Simen Svale Skogsrud + + 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 . +*/ + +#include "planner.h" + +#ifndef _STEPPER_H_ +#define _STEPPER_H_ + +#ifndef SEGMENT_BUFFER_SIZE +#define SEGMENT_BUFFER_SIZE 10 +#endif + +typedef enum { + SquaringMode_Both = 0, + SquaringMode_A, + SquaringMode_B, +} squaring_mode_t; + +// Holds the planner block Bresenham algorithm execution data for the segments in the segment buffer. +// NOTE: This data is copied from the prepped planner blocks so that the planner blocks may be +// discarded when entirely consumed and completed by the segment buffer. Also, AMASS alters this +// data for its own use. +typedef struct st_block { + uint_fast8_t id; // Id may be used by driver to track changes + struct st_block *next; // Pointer to next element in cirular list of blocks + uint32_t steps[N_AXIS]; + uint32_t step_event_count; + axes_signals_t direction_bits; + gc_override_flags_t overrides; // Block bitfield variable for overrides + float steps_per_mm; + float millimeters; + float programmed_rate; + char *message; // Message to be displayed when block is executed + output_command_t *output_commands; // Output commands (linked list) to be performed when block is executed + bool dynamic_rpm; // Tracks motions that require dynamic RPM adjustment + bool backlash_motion; +} st_block_t; + +typedef struct st_segment { + uint_fast8_t id; // Id may be used by driver to track changes + struct st_segment *next; // Pointer to next element in cirular list of segments + st_block_t *exec_block; // Pointer to the block data for the segment + uint32_t cycles_per_tick; // Step distance traveled per ISR tick, aka step rate. + float current_rate; + float target_position; // Target position of segment relative to block start, used by spindle sync code + uint_fast16_t n_step; // Number of step events to be executed for this segment +#ifdef SPINDLE_PWM_DIRECT + uint_fast16_t spindle_pwm; // Spindle PWM to be set at the start of segment execution +#else + float spindle_rpm; // Spindle RPM to be set at the start of the segment execution +#endif + bool update_rpm; // True if set spindle speed at the start of the segment execution + bool spindle_sync; // True if block is spindle synchronized + bool cruising; // True when in cruising part of profile, only set for spindle synced moves + uint_fast8_t amass_level; // Indicates AMASS level for the ISR to execute this segment +} segment_t; + +// Stepper ISR data struct. Contains the running data for the main stepper ISR. +typedef struct { + // Used by the bresenham line algorithm + uint32_t counter_x, // Counter variables for the bresenham line tracer + counter_y, + counter_z + #ifdef A_AXIS + , counter_a + #endif + #ifdef B_AXIS + , counter_b + #endif + #ifdef C_AXIS + , counter_c + #endif +; + bool new_block; // Set to true when a new block is started, might be used by driver for advanced functionality + bool dir_change; // Set to true on direction changes, might be used by driver for advanced functionality + axes_signals_t step_outbits; // The next stepping-bits to be output + axes_signals_t dir_outbits; // The next direction-bits to be output + uint32_t steps[N_AXIS]; + uint_fast8_t amass_level; // AMASS level for this segment +// uint_fast16_t spindle_pwm; + uint_fast16_t step_count; // Steps remaining in line segment motion + uint32_t step_event_count; + st_block_t *exec_block; // Pointer to the block data for the segment being executed + segment_t *exec_segment; // Pointer to the segment being executed +} stepper_t; + +// Initialize and setup the stepper motor subsystem +void stepper_init(); + +// Enable steppers, but cycle does not start unless called by motion control or realtime command. +void st_wake_up(); + +// Immediately disables steppers +void st_go_idle(); + +// Reset the stepper subsystem variables +void st_reset(); + +// Called by spindle_set_state() to inform about RPM changes. +void st_rpm_changed(float rpm); + +// Changes the run state of the step segment buffer to execute the special parking motion. +void st_parking_setup_buffer(); + +// Restores the step segment buffer to the normal run state after a parking motion. +void st_parking_restore_buffer(); + +// Reloads step segment buffer. Called continuously by realtime execution system. +void st_prep_buffer(); + +// Called by planner_recalculate() when the executing block is updated by the new plan. +void st_update_plan_block_parameters(); + +// Called by realtime status reporting if realtime rate reporting is enabled in config.h. +float st_get_realtime_rate(); + +void stepper_driver_interrupt_handler (void); + +#endif diff --git a/stream.h b/stream.h new file mode 100644 index 0000000..7e0af6e --- /dev/null +++ b/stream.h @@ -0,0 +1,107 @@ +/* + stream.h - some ASCII control character definitions and optional structures for stream buffers + + Part of grblHAL + + Copyright (c) 2019-2020 Terje Io + + Grbl is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + Grbl is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with Grbl. If not, see . +*/ + +#ifndef _STREAM_H_ +#define _STREAM_H_ + +#define ASCII_ETX 0x03 +#define ASCII_ACK 0x06 +#define ASCII_BS 0x08 +#define ASCII_TAB 0x09 +#define ASCII_LF 0x0A +#define ASCII_CR 0x0D +#define ASCII_XON 0x11 +#define ASCII_XOFF 0x13 +#define ASCII_NAK 0x15 +#define ASCII_EOF 0x1A +#define ASCII_CAN 0x18 +#define ASCII_EM 0x19 +#define ASCII_ESC 0x1B +#define ASCII_DEL 0x7F +#define ASCII_EOL "\r\n" + +#ifndef RX_BUFFER_SIZE +#define RX_BUFFER_SIZE 1024 // must be a power of 2 +#endif + +#ifndef TX_BUFFER_SIZE +#define TX_BUFFER_SIZE 512 // must be a power of 2 +#endif + +#ifndef BLOCK_TX_BUFFER_SIZE +#define BLOCK_TX_BUFFER_SIZE 256 +#endif + +// Serial baud rate +#ifndef BAUD_RATE +#define BAUD_RATE 115200 +#endif + +// Value to be returned from input stream when no data is available +#ifndef SERIAL_NO_DATA +#define SERIAL_NO_DATA -1 +#endif + +#define BUFCOUNT(head, tail, size) ((head >= tail) ? (head - tail) : (size - tail + head)) + +#include +#include +#include + +typedef enum { + StreamType_Serial = 0, + StreamType_MPG, + StreamType_Bluetooth, + StreamType_Telnet, + StreamType_WebSocket, + StreamType_SDCard, + StreamType_FlashFs, + StreamType_Redirected, + StreamType_Null +} stream_type_t; + +// These structures are not referenced in the core code, may be used by drivers + +typedef struct { + volatile uint_fast16_t head; + volatile uint_fast16_t tail; + bool overflow; +#ifdef SERIAL_RTS_HANDSHAKE + volatile bool rts_state; +#endif + bool backup; + char data[RX_BUFFER_SIZE]; +} stream_rx_buffer_t; + +typedef struct { + volatile uint_fast16_t head; + volatile uint_fast16_t tail; + char data[TX_BUFFER_SIZE]; +} stream_tx_buffer_t; + +typedef struct { + uint_fast16_t length; + uint_fast16_t max_length; + char *s; + char data[BLOCK_TX_BUFFER_SIZE]; +} stream_block_tx_buffer_t; + +#endif diff --git a/system.c b/system.c new file mode 100644 index 0000000..3eddbb9 --- /dev/null +++ b/system.c @@ -0,0 +1,824 @@ +/* + system.c - Handles system level commands and real-time processes + + Part of grblHAL + + Copyright (c) 2017-2021 Terje Io + Copyright (c) 2014-2016 Sungeun K. Jeon for Gnea Research LLC + + Grbl is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + Grbl is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with Grbl. If not, see . +*/ + +#include +#include + +#include "hal.h" +#include "motion_control.h" +#include "protocol.h" +#include "tool_change.h" +#include "state_machine.h" +#include "limits.h" +#ifdef KINEMATICS_API +#include "kinematics.h" +#endif + +static status_code_t jog (sys_state_t state, char *args); +static status_code_t enumerate_alarms (sys_state_t state, char *args); +static status_code_t enumerate_errors (sys_state_t state, char *args); +static status_code_t enumerate_groups (sys_state_t state, char *args); +static status_code_t enumerate_settings (sys_state_t state, char *args); +static status_code_t enumerate_all (sys_state_t state, char *args); +static status_code_t output_settings (sys_state_t state, char *args); +static status_code_t output_all_settings (sys_state_t state, char *args); +static status_code_t output_parser_state (sys_state_t state, char *args); +static status_code_t toggle_block_delete (sys_state_t state, char *args); +static status_code_t check_mode (sys_state_t state, char *args); +static status_code_t disable_lock (sys_state_t state, char *args); +static status_code_t output_help (sys_state_t state, char *args); +static status_code_t home (sys_state_t state, char *args); +static status_code_t home_x (sys_state_t state, char *args); +static status_code_t home_y (sys_state_t state, char *args); +static status_code_t home_z (sys_state_t state, char *args); +#ifdef A_AXIS +static status_code_t home_a (sys_state_t state, char *args); +#endif +#ifdef B_AXIS +static status_code_t home_b (sys_state_t state, char *args); +#endif +#ifdef C_AXIS +static status_code_t home_c (sys_state_t state, char *args); +#endif +static status_code_t enter_sleep (sys_state_t state, char *args); +static status_code_t set_tool_reference (sys_state_t state, char *args); +static status_code_t tool_probe_workpiece (sys_state_t state, char *args); +static status_code_t output_ngc_parameters (sys_state_t state, char *args); +static status_code_t build_info (sys_state_t state, char *args); +static status_code_t output_all_build_info (sys_state_t state, char *args); +static status_code_t settings_reset (sys_state_t state, char *args); +static status_code_t output_startup_lines (sys_state_t state, char *args); +static status_code_t set_startup_line0 (sys_state_t state, char *args); +static status_code_t set_startup_line1 (sys_state_t state, char *args); +#ifdef DEBUGOUT +static status_code_t output_memmap (sys_state_t state, char *args); +#endif + +// Pin change interrupt for pin-out commands, i.e. cycle start, feed hold, and reset. Sets +// only the realtime command execute variable to have the main program execute these when +// its ready. This works exactly like the character-based realtime commands when picked off +// directly from the incoming data stream. +ISR_CODE void control_interrupt_handler (control_signals_t signals) +{ + if(signals.deasserted) + return; // for now... + + if (signals.value) { + + sys.last_event.control.value = signals.value; + + if ((signals.reset || signals.e_stop || signals.motor_fault) && state_get() != STATE_ESTOP) + mc_reset(); + else { +#ifdef ENABLE_SAFETY_DOOR_INPUT_PIN + if (signals.safety_door_ajar) { + if(settings.flags.safety_door_ignore_when_idle) { + // Only stop the spindle (laser off) when idle or jogging, + // this to allow positioning the controlled point (spindle) when door is open. + // NOTE: at least for lasers there should be an external interlock blocking laser power. + if(state_get() != STATE_IDLE && state_get() != STATE_JOG) + system_set_exec_state_flag(EXEC_SAFETY_DOOR); + if(settings.mode == Mode_Laser) // Turn off spindle imeediately (laser) when in laser mode + hal.spindle.set_state((spindle_state_t){0}, 0.0f); + } else + system_set_exec_state_flag(EXEC_SAFETY_DOOR); + } +#endif + if (signals.probe_triggered) { + if(sys.probing_state == Probing_Off && (state_get() & (STATE_CYCLE|STATE_JOG))) { + system_set_exec_state_flag(EXEC_STOP); + sys.alarm_pending = Alarm_ProbeProtect; + } else + hal.probe.configure(false, false); + } else if (signals.probe_disconnected) { + if(sys.probing_state == Probing_Active && state_get() == STATE_CYCLE) { + system_set_exec_state_flag(EXEC_FEED_HOLD); + sys.alarm_pending = Alarm_ProbeProtect; + } + } else if (signals.feed_hold) + system_set_exec_state_flag(EXEC_FEED_HOLD); + else if (signals.cycle_start) + system_set_exec_state_flag(EXEC_CYCLE_START); + } + } +} + + +// Executes user startup script, if stored. +void system_execute_startup (void) +{ + if(hal.nvs.type != NVS_None) { + + char line[sizeof(stored_line_t)]; + uint_fast8_t n; + + for (n = 0; n < N_STARTUP_LINE; n++) { + if (!settings_read_startup_line(n, line)) + report_execute_startup_message(line, Status_SettingReadFail); + else if (*line != '\0') + report_execute_startup_message(line, gc_execute_block(line, NULL)); + } + } +} + +// Reset spindle encoder data +status_code_t spindle_reset_data (sys_state_t state, char *args) +{ + if(hal.spindle.reset_data) + hal.spindle.reset_data(); + + return hal.spindle.reset_data ? Status_OK : Status_InvalidStatement; +} + +status_code_t read_int (char *s, int32_t *value) +{ + uint_fast8_t counter = 0; + float parameter; + if(!read_float(s, &counter, ¶meter)) + return Status_BadNumberFormat; + + if(parameter - truncf(parameter) != 0.0f) + return Status_InvalidStatement; + + *value = (int32_t)parameter; + + return Status_OK; +} + +const sys_command_t sys_commands[] = { + {"G", true, output_parser_state}, + {"J", false, jog}, + {"#", true, output_ngc_parameters}, + {"$", false, output_settings}, + {"+", false, output_all_settings}, + {"B", true, toggle_block_delete}, + {"C", true, check_mode}, + {"X", false, disable_lock}, + {"H", false, home}, + {"HX", false, home_x}, + {"HY", false, home_y}, + {"HZ", false, home_z}, +#ifdef A_AXIS + {"HA", false, home_a}, +#endif +#ifdef B_AXIS + {"HB", false, home_b}, +#endif +#ifdef C_AXIS + {"HC", false, home_c}, +#endif + {"HELP", false, output_help}, + {"SLP", true, enter_sleep}, + {"TLR", true, set_tool_reference}, + {"TPW", true, tool_probe_workpiece}, + {"I", false, build_info}, + {"I+", true, output_all_build_info}, + {"RST", false, settings_reset}, + {"N", true, output_startup_lines}, + {"N0", false, set_startup_line0}, + {"N1", false, set_startup_line1}, + {"EA", true, enumerate_alarms}, + {"EE", true, enumerate_errors}, + {"EG", true, enumerate_groups}, + {"ES", true, enumerate_settings}, + {"E*", true, enumerate_all}, + {"RST", false, settings_reset}, + {"LEV", true, report_last_signals_event}, + {"LIM", true, report_current_limit_state}, + {"SD", false, report_spindle_data}, + {"SR", false, spindle_reset_data}, +#ifdef DEBUGOUT + {"Q", true, output_memmap}, +#endif +}; + +// Directs and executes one line of formatted input from protocol_process. While mostly +// incoming streaming g-code blocks, this also executes Grbl internal commands, such as +// settings, initiating the homing cycle, and toggling switch states. This differs from +// the realtime command module by being susceptible to when Grbl is ready to execute the +// next line during a cycle, so for switches like block delete, the switch only effects +// the lines that are processed afterward, not necessarily real-time during a cycle, +// since there are motions already stored in the buffer. However, this 'lag' should not +// be an issue, since these commands are not typically used during a cycle. + +// NOTE: Code calling system_execute_line() needs to provide a line buffer of at least LINE_BUFFER_SIZE +status_code_t system_execute_line (char *line) +{ + if(line[1] == '\0') { + report_grbl_help(); + return Status_OK; + } + + if(strlen(line) >= ((LINE_BUFFER_SIZE / 2) - 1)) + return Status_Overflow; + + sys_commands_t base = { + .n_commands = sizeof(sys_commands) / sizeof(sys_command_t), + .commands = sys_commands, + .on_get_commands = grbl.on_get_commands + }; + + status_code_t retval = Status_Unhandled; + char c, *org = line, *ucline = line, *lcline = line + (LINE_BUFFER_SIZE / 2); + + // Uppercase original and copy original out in the buffer + // TODO: create a common function for stripping down uppercase version? + do { + c = *org++; + if(c != ' ') // Remove spaces from uppercase version + *ucline++ = CAPS(c); + *lcline++ = c; + } while(c); + + lcline = line + (LINE_BUFFER_SIZE / 2); + + if(!strncmp(&line[1], "HELP", 4)) + return report_help(&line[5], &lcline[5]); + + char *args = strchr(line, '='), *lcargs = strchr(lcline, '='); + + if(args) { + *args++ = '\0'; + *lcargs++ = '\0'; + } + + uint_fast8_t idx; + sys_commands_t *cmd = &base; + + do { + for(idx = 0; idx < cmd->n_commands; idx++) { + if(!strcmp(&line[1], cmd->commands[idx].command)) { + if(!cmd->commands[idx].noargs || lcargs == NULL) { + if((retval = cmd->commands[idx].execute(state_get(), lcargs)) != Status_Unhandled) + break; + } + } + } + cmd = retval == Status_Unhandled && cmd->on_get_commands ? cmd->on_get_commands() : NULL; + } while(cmd); + + // Let user code have a peek at system commands before check for global setting + if(retval == Status_Unhandled && grbl.on_unknown_sys_command) { + if(lcargs) + *(--lcargs) = '='; + + retval = grbl.on_unknown_sys_command(state_get(), lcline); + + if(lcargs) + *lcargs++ = '\0'; + } + + if (retval == Status_Unhandled) { + // Check for global setting, store if so + if(state_get() == STATE_IDLE || (state_get() & (STATE_ALARM|STATE_ESTOP|STATE_CHECK_MODE))) { + uint_fast8_t counter = 1; + float parameter; + if(!read_float(line, &counter, ¶meter)) + retval = Status_BadNumberFormat; + else if(!isintf(parameter)) + retval = Status_InvalidStatement; + else if(args) + retval = settings_store_setting((setting_id_t)parameter, lcargs); + else + retval = report_grbl_setting((setting_id_t)parameter, NULL); + } else + retval = Status_IdleError; + } + + return retval; +} + +// System commands + +static status_code_t jog (sys_state_t state, char *args) +{ + if(!(state == STATE_IDLE || (state & (STATE_JOG|STATE_TOOL_CHANGE)))) + return Status_IdleError; + + if(args != NULL) { + *(--args) = '='; + args -= 2; + } + + return args == NULL ? Status_InvalidStatement : gc_execute_block(strcaps(args), NULL); // NOTE: $J= is ignored inside g-code parser and used to detect jog motions. +} + +static status_code_t enumerate_alarms (sys_state_t state, char *args) +{ + return report_alarm_details(); +} + +static status_code_t enumerate_errors (sys_state_t state, char *args) +{ + return report_error_details(); +} + +static status_code_t enumerate_groups (sys_state_t state, char *args) +{ + return report_setting_group_details(true, NULL); +} + +static status_code_t enumerate_settings (sys_state_t state, char *args) +{ + return report_settings_details(false, Setting_SettingsAll, Group_All); +} + +static status_code_t enumerate_all (sys_state_t state, char *args) +{ + report_alarm_details(); + report_error_details(); + report_setting_group_details(true, NULL); + return report_settings_details(false, Setting_SettingsAll, Group_All); +} + +static status_code_t output_settings (sys_state_t state, char *args) +{ + status_code_t retval = Status_OK; + + if(args) { + int32_t id; + retval = read_int(args, &id); + if(retval == Status_OK && id >= 0) + retval = report_settings_details(true, (setting_id_t)id, Group_All); + } else if (state & (STATE_CYCLE|STATE_HOLD)) + retval = Status_IdleError; // Block during cycle. Takes too long to print. + else +#if COMPATIBILITY_LEVEL <= 1 + report_grbl_settings(true, NULL); +#else + report_grbl_settings(false, NULL); +#endif + + return retval; +} + +static status_code_t output_all_settings (sys_state_t state, char *args) +{ + status_code_t retval = Status_OK; + + if(args) { + int32_t id; + retval = read_int(args, &id); + if(retval == Status_OK && id >= 0) + retval = report_settings_details(true, (setting_id_t)id, Group_All); + } else if (state & (STATE_CYCLE|STATE_HOLD)) + retval = Status_IdleError; // Block during cycle. Takes too long to print. + else + report_grbl_settings(true, NULL); + + return retval; +} + +static status_code_t output_parser_state (sys_state_t state, char *args) +{ + report_gcode_modes(); + sys.report.homed = On; // Report homed state on next realtime report + + return Status_OK; +} + +static status_code_t toggle_block_delete (sys_state_t state, char *args) +{ + sys.flags.block_delete_enabled = !sys.flags.block_delete_enabled; + grbl.report.feedback_message(sys.flags.block_delete_enabled ? Message_Enabled : Message_Disabled); + + return Status_OK; +} + +static status_code_t check_mode (sys_state_t state, char *args) +{ + if (state == STATE_CHECK_MODE) { + // Perform reset when toggling off. Check g-code mode should only work if Grbl + // is idle and ready, regardless of alarm locks. This is mainly to keep things + // simple and consistent. + mc_reset(); + grbl.report.feedback_message(Message_Disabled); + } else if (state == STATE_IDLE) { // Requires idle mode. + state_set(STATE_CHECK_MODE); + grbl.report.feedback_message(Message_Enabled); + } else + return Status_IdleError; + + return Status_OK; +} + +static status_code_t disable_lock (sys_state_t state, char *args) +{ + status_code_t retval = Status_OK; + + if(state & (STATE_ALARM|STATE_ESTOP)) { + + control_signals_t control_signals = hal.control.get_state(); + + // Block if self-test failed + if(sys.alarm == Alarm_SelftestFailed) + retval = Status_SelfTestFailed; + // Block if e-stop is active. + else if (control_signals.e_stop) + retval = Status_EStop; + // Block if safety door is ajar. + else if (control_signals.safety_door_ajar) + retval = Status_CheckDoor; + // Block if safety reset is active. + else if(control_signals.reset) + retval = Status_Reset; + else if(settings.limits.flags.hard_enabled && settings.limits.flags.check_at_init && limit_signals_merge(hal.limits.get_state()).value) + retval = Status_LimitsEngaged; + else if(limits_homing_required()) + retval = Status_HomingRequired; + else { + grbl.report.feedback_message(Message_AlarmUnlock); + state_set(STATE_IDLE); + } + // Don't run startup script. Prevents stored moves in startup from causing accidents. + } // Otherwise, no effect. + + return retval; +} + +static status_code_t output_help (sys_state_t state, char *args) +{ + return report_help(args, args); +} + +static status_code_t go_home (sys_state_t state, axes_signals_t axes) +{ + if(!(state_get() == STATE_IDLE || (state_get() & (STATE_ALARM|STATE_ESTOP)))) + return Status_IdleError; + + status_code_t retval = Status_OK; + + control_signals_t control_signals = hal.control.get_state(); + + // Block if self-test failed + if(sys.alarm == Alarm_SelftestFailed) + retval = Status_SelfTestFailed; + // Block if e-stop is active. + else if (control_signals.e_stop) + retval = Status_EStop; + else if(control_signals.motor_fault) + retval = Status_MotorFault; + else if (!(settings.homing.flags.enabled && (sys.homing.mask || settings.homing.flags.single_axis_commands || settings.homing.flags.manual))) + retval = Status_HomingDisabled; + // Block if safety door is ajar. + else if (control_signals.safety_door_ajar && !settings.flags.safety_door_ignore_when_idle) + retval = Status_CheckDoor; + // Block if safety reset is active. + else if(control_signals.reset) + retval = Status_Reset; + + if(retval == Status_OK) + retval = mc_homing_cycle(axes); // Home axes according to configuration + + if (retval == Status_OK && !sys.abort) { + state_set(STATE_IDLE); // Set to IDLE when complete. + st_go_idle(); // Set steppers to the settings idle state before returning. + report_feedback_message(Message_None); + // Execute startup scripts after successful homing. + if (sys.homing.mask && (sys.homing.mask & sys.homed.mask) == sys.homing.mask) + system_execute_startup(); + else if(limits_homing_required()) { // Keep alarm state active if homing is required and not all axes homed. + sys.alarm = Alarm_HomingRequried; + state_set(STATE_ALARM); + } + } + + return retval == Status_Unhandled ? Status_OK : retval; +} + +static status_code_t home (sys_state_t state, char *args) +{ + return go_home(state, (axes_signals_t){0}); +} + +static status_code_t home_x (sys_state_t state, char *args) +{ + return go_home(state, (axes_signals_t){X_AXIS_BIT}); +} + +static status_code_t home_y (sys_state_t state, char *args) +{ + return go_home(state, (axes_signals_t){Y_AXIS_BIT}); +} + +static status_code_t home_z (sys_state_t state, char *args) +{ + return go_home(state, (axes_signals_t){Z_AXIS_BIT}); +} + +#ifdef A_AXIS +static status_code_t home_a (sys_state_t state, char *args) +{ + return go_home(state, (axes_signals_t){A_AXIS_BIT}); +} +#endif + +#ifdef B_AXIS +static status_code_t home_b (sys_state_t state, char *args) +{ + return go_home(state, (axes_signals_t){B_AXIS_BIT}); +} +#endif + +#ifdef C_AXIS +static status_code_t home_c (sys_state_t state, char *args) +{ + return go_home(state, (axes_signals_t){C_AXIS_BIT}); +} +#endif + +static status_code_t enter_sleep (sys_state_t state, char *args) +{ + if(!settings.flags.sleep_enable) + return Status_InvalidStatement; + else if(!(state == STATE_IDLE || state == STATE_ALARM)) + return Status_IdleError; + else + system_set_exec_state_flag(EXEC_SLEEP); // Set to execute enter_sleep mode immediately + + return Status_OK; +} + +static status_code_t set_tool_reference (sys_state_t state, char *args) +{ +#ifdef TOOL_LENGTH_OFFSET_AXIS + if(sys.flags.probe_succeeded) { + sys.tlo_reference_set.mask = bit(TOOL_LENGTH_OFFSET_AXIS); + sys.tlo_reference[TOOL_LENGTH_OFFSET_AXIS] = sys.probe_position[TOOL_LENGTH_OFFSET_AXIS]; // - gc_state.tool_length_offset[Z_AXIS])); + } else + sys.tlo_reference_set.mask = 0; +#else + plane_t plane; + gc_get_plane_data(&plane, gc_state.modal.plane_select); + if(sys.flags.probe_succeeded) { + sys.tlo_reference_set.mask |= bit(plane.axis_linear); + sys.tlo_reference[plane.axis_linear] = sys.probe_position[plane.axis_linear]; +// - lroundf(gc_state.tool_length_offset[plane.axis_linear] * settings.axis[plane.axis_linear].steps_per_mm); + } else + sys.tlo_reference_set.mask = 0; +#endif + sys.report.tlo_reference = On; + + return Status_OK; +} + +static status_code_t tool_probe_workpiece (sys_state_t state, char *args) +{ + return tc_probe_workpiece(); +} + +static status_code_t output_ngc_parameters (sys_state_t state, char *args) +{ + report_ngc_parameters(); + + return Status_OK; +} + +static status_code_t build_info (sys_state_t state, char *args) +{ + if (!(state == STATE_IDLE || (state & (STATE_ALARM|STATE_ESTOP|STATE_CHECK_MODE)))) + return Status_IdleError; + + if (args == NULL) { + char info[sizeof(stored_line_t)]; + settings_read_build_info(info); + report_build_info(info, false); + } + #ifndef DISABLE_BUILD_INFO_WRITE_COMMAND + else if (strlen(args) < (sizeof(stored_line_t) - 1)) + settings_write_build_info(args); + #endif + else + return Status_InvalidStatement; + + return Status_OK; +} + +static status_code_t output_all_build_info (sys_state_t state, char *args) +{ + char info[sizeof(stored_line_t)]; + + settings_read_build_info(info); + report_build_info(info, false); + + return Status_OK; +} + +static status_code_t settings_reset (sys_state_t state, char *args) +{ + settings_restore_t restore = {0}; + status_code_t retval = Status_OK; + + if (!(state == STATE_IDLE || (state & (STATE_ALARM|STATE_ESTOP)))) + retval = Status_IdleError; + + else switch (*args) { + + #ifndef DISABLE_RESTORE_NVS_DEFAULT_SETTINGS + case '$': + restore.defaults = On; + break; + #endif + + #ifndef DISABLE_RESTORE_NVS_CLEAR_PARAMETERS + case '#': + restore.parameters = On; + break; + #endif + + #ifndef DISABLE_RESTORE_NVS_WIPE_ALL + case '*': + restore.mask = settings_all.mask; + break; + #endif + + #ifndef DISABLE_RESTORE_DRIVER_PARAMETERS + case '&': + restore.driver_parameters = On; + break; + #endif + + default: + retval = Status_InvalidStatement; + break; + } + + if(retval == Status_OK && restore.mask) { + settings_restore(restore); + grbl.report.feedback_message(Message_RestoreDefaults); + mc_reset(); // Force reset to ensure settings are initialized correctly. + } + + return retval; +} + +static status_code_t output_startup_lines (sys_state_t state, char *args) +{ + if (!(state == STATE_IDLE || (state & (STATE_ALARM|STATE_ESTOP|STATE_CHECK_MODE)))) + return Status_IdleError; + + // Print startup lines + + uint_fast8_t counter; + char line[sizeof(stored_line_t)]; + + for (counter = 0; counter < N_STARTUP_LINE; counter++) { + if (!(settings_read_startup_line(counter, line))) + grbl.report.status_message(Status_SettingReadFail); + else + report_startup_line(counter, line); + } + + return Status_OK; +} + +static status_code_t set_startup_line (sys_state_t state, char *args, uint_fast8_t lnr) +{ + // Store startup line [IDLE Only] Prevents motion during ALARM. + if (!(state == STATE_IDLE || (state & (STATE_ALARM|STATE_ESTOP|STATE_CHECK_MODE)))) + return Status_IdleError; + + if(args == NULL) + return Status_InvalidStatement; + + status_code_t retval = Status_OK; + + strcaps(args); + + if(strlen(args) >= (sizeof(stored_line_t) - 1)) + retval = Status_Overflow; + else if ((retval = gc_execute_block(args, NULL)) == Status_OK) // Execute gcode block to ensure block is valid. + settings_write_startup_line(lnr, args); + + return retval; +} + +static status_code_t set_startup_line0 (sys_state_t state, char *args) +{ + return set_startup_line(state, args, 0); +} + +static status_code_t set_startup_line1 (sys_state_t state, char *args) +{ + return set_startup_line(state, args, 1); +} + +#ifdef DEBUGOUT +static status_code_t output_memmap (sys_state_t state, char *args) +{ + nvs_output_memmap(); + + return Status_OK; +} +#endif + +// End system commands + +void system_flag_wco_change (void) +{ + if(!settings.status_report.sync_on_wco_change) + protocol_buffer_synchronize(); + + sys.report.wco = On; +} + +// Sets machine position. Must be sent a 'step' array. +// NOTE: If motor steps and machine position are not in the same coordinate frame, this function +// serves as a central place to compute the transformation. +void system_convert_array_steps_to_mpos (float *position, int32_t *steps) +{ +#ifdef KINEMATICS_API + kinematics.convert_array_steps_to_mpos(position, steps); +#else + uint_fast8_t idx = N_AXIS; + do { + idx--; + position[idx] = steps[idx] / settings.axis[idx].steps_per_mm; + } while(idx); +#endif +} + +// Checks and reports if target array exceeds machine travel limits. Returns false if check failed. +// NOTE: max_travel is stored as negative +// TODO: only check homed axes? +bool system_check_travel_limits (float *target) +{ + bool failed = false; + uint_fast8_t idx = N_AXIS; + + if(settings.homing.flags.force_set_origin) { + do { + idx--; + // When homing forced set origin is enabled, soft limits checks need to account for directionality. + failed = settings.axis[idx].max_travel < -0.0f && + (bit_istrue(settings.homing.dir_mask.value, bit(idx)) + ? (target[idx] < 0.0f || target[idx] > -settings.axis[idx].max_travel) + : (target[idx] > 0.0f || target[idx] < settings.axis[idx].max_travel)); + } while(!failed && idx); + } else do { + idx--; + failed = settings.axis[idx].max_travel < -0.0f && (target[idx] > 0.0f || target[idx] < settings.axis[idx].max_travel); + } while(!failed && idx); + + return !failed; +} + +// Limits jog commands to be within machine limits, homed axes only. +// When hard limits are enabled pulloff distance is subtracted to avoid triggering limit switches. +// NOTE: max_travel is stored as negative +void system_apply_jog_limits (float *target) +{ + uint_fast8_t idx = N_AXIS; + + if(sys.homed.mask) do { + idx--; + float pulloff = settings.limits.flags.hard_enabled && bit_istrue(sys.homing.mask, bit(idx)) ? settings.homing.pulloff : 0.0f; + if(bit_istrue(sys.homed.mask, bit(idx)) && settings.axis[idx].max_travel < -0.0f) { + if(settings.homing.flags.force_set_origin) { + if(bit_isfalse(settings.homing.dir_mask.value, bit(idx))) { + if(target[idx] > 0.0f) + target[idx] = 0.0f; + else if(target[idx] < (settings.axis[idx].max_travel + pulloff)) + target[idx] = (settings.axis[idx].max_travel + pulloff); + } else { + if(target[idx] < 0.0f) + target[idx] = 0.0f; + else if(target[idx] > -(settings.axis[idx].max_travel + pulloff)) + target[idx] = -(settings.axis[idx].max_travel + pulloff); + } + } else { + if(target[idx] > -pulloff) + target[idx] = -pulloff; + else if(target[idx] < (settings.axis[idx].max_travel + pulloff)) + target[idx] = (settings.axis[idx].max_travel + pulloff); + } + } + } while(idx); +} + +void system_raise_alarm (alarm_code_t alarm) +{ + sys.alarm = alarm; + state_set(alarm == Alarm_EStop ? STATE_ESTOP : STATE_ALARM); + if(sys.driver_started) + report_alarm_message(alarm); +} diff --git a/system.h b/system.h new file mode 100644 index 0000000..1b8fa83 --- /dev/null +++ b/system.h @@ -0,0 +1,301 @@ +/* + system.h - Header for system level commands and real-time processes + + Part of grblHAL + + Copyright (c) 2017-2021 Terje Io + Copyright (c) 2014-2016 Sungeun K. Jeon for Gnea Research LLC + + Grbl is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + Grbl is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with Grbl. If not, see . +*/ + +#ifndef _SYSTEM_H_ +#define _SYSTEM_H_ + +#include "gcode.h" +#include "probe.h" +#include "alarms.h" + +// Define system executor bit map. Used internally by realtime protocol as realtime command flags, +// which notifies the main program to execute the specified realtime command asynchronously. +// NOTE: The system executor uses an unsigned 16-bit volatile variable (16 flag limit.) The default +// flags are always false, so the realtime protocol only needs to check for a non-zero value to +// know when there is a realtime command to execute. +#define EXEC_STATUS_REPORT bit(0) +#define EXEC_CYCLE_START bit(1) +#define EXEC_CYCLE_COMPLETE bit(2) +#define EXEC_FEED_HOLD bit(3) +#define EXEC_STOP bit(4) +#define EXEC_RESET bit(5) +#define EXEC_SAFETY_DOOR bit(6) +#define EXEC_MOTION_CANCEL bit(7) +#define EXEC_SLEEP bit(8) +#define EXEC_TOOL_CHANGE bit(9) +#define EXEC_PID_REPORT bit(10) +#define EXEC_GCODE_REPORT bit(11) +#define EXEC_TLO_REPORT bit(12) +#define EXEC_RT_COMMAND bit(13) + +// Define system state bit map. The state variable primarily tracks the individual functions +// of Grbl to manage each without overlapping. It is also used as a messaging flag for +// critical events. +// NOTE: flags are mutually exclusive, bit map allows testing for multiple states (except STATE_IDLE) in a single statement +#define STATE_IDLE 0 // Must be zero. No flags. +#define STATE_ALARM bit(0) // In alarm state. Locks out all g-code processes. Allows settings access. +#define STATE_CHECK_MODE bit(1) // G-code check mode. Locks out planner and motion only. +#define STATE_HOMING bit(2) // Performing homing cycle +#define STATE_CYCLE bit(3) // Cycle is running or motions are being executed. +#define STATE_HOLD bit(4) // Active feed hold +#define STATE_JOG bit(5) // Jogging mode. +#define STATE_SAFETY_DOOR bit(6) // Safety door is ajar. Feed holds and de-energizes system. +#define STATE_SLEEP bit(7) // Sleep state. +#define STATE_ESTOP bit(8) // EStop mode, reports and is mainly handled similar to alarm state +#define STATE_TOOL_CHANGE bit(9) // Manual tool change, similar to STATE_HOLD - but stops spindle and allows jogging. + +// Define Grbl feedback message codes. Valid values (0-255). +typedef enum { + Message_None = 0, // Reserved, do not change value + Message_CriticalEvent = 1, + Message_AlarmLock = 2, + Message_AlarmUnlock = 3, + Message_Enabled = 4, + Message_Disabled = 5, + Message_SafetyDoorAjar = 6, + Message_CheckLimits = 7, + Message_ProgramEnd = 8, + Message_RestoreDefaults = 9, + Message_SpindleRestore = 10, + Message_SleepMode = 11, + Message_EStop = 12, + Message_HomingCycleRequired = 13, + Message_CycleStartToRerun = 14, + Message_ReferenceTLOEstablished = 15, + Message_MotorFault = 16, + Message_NextMessage // Next unassigned message number +} message_code_t; + +typedef enum { + Parking_DoorClosed = 0, + Parking_DoorAjar, + Parking_Retracting, + Parking_Cancel, + Parking_Resuming +} parking_state_t; + +typedef enum { + Hold_NotHolding = 0, + Hold_Complete = 1, + Hold_Pending = 2 +} hold_state_t; + +typedef uint_fast16_t sys_state_t; + +// Define step segment generator state flags. +typedef union { + uint8_t flags; + struct { + uint8_t end_motion :1, + execute_hold :1, + execute_sys_motion :1, + update_spindle_rpm :1, + unassigned :4; + }; +} step_control_t; + + +typedef union { + uint16_t value; + uint16_t mask; + struct { + uint16_t reset :1, + feed_hold :1, + cycle_start :1, + safety_door_ajar :1, + block_delete :1, + stop_disable :1, // M1 + e_stop :1, + probe_disconnected :1, + motor_fault :1, + motor_warning :1, + limits_override :1, + unassigned :3, + probe_triggered :1, // used for probe protection + deasserted :1; // this flag is set if signals are deasserted. Note: do NOT pass on to Grbl control_interrupt_handler if set. + }; +} control_signals_t; + + +// Define spindle stop override control states. +typedef union { + uint8_t value; + struct { + uint8_t enabled :1, + initiate :1, + restore :1, + restore_cycle :1, + unassigned :4; + }; +} spindle_stop_t; + +#ifdef PID_LOG + +typedef struct { + uint_fast16_t idx; + float setpoint; + float t_sample; + float target[PID_LOG]; + float actual[PID_LOG]; +} pid_data_t; + +#endif + +typedef union { + uint16_t value; + struct { + uint16_t mpg_mode :1, // MPG mode changed. + scaling :1, // Scaling (G50/G51) changed. + homed :1, // Homed state changed. + xmode :1, // Lathe radius/diameter mode changed. + spindle :1, // Spindle state changed. + coolant :1, // Coolant state changed. + overrides :1, // Overrides changed. + tool :1, // Tool changed. + wco :1, // Add work coordinates. + gwco :1, // Add work coordinate. + tool_offset :1, // Tool offsets changed. + pwm :1, // Add PWM information (optional: to be added by driver). + motor :1, // Add motor information (optional: to be added by driver). + encoder :1, // Add encoder information (optional: to be added by driver). + tlo_reference :1, // Tool length offset reference changed + all :1; // Set when CMD_STATUS_REPORT_ALL is requested, may be used by user code + }; +} report_tracking_flags_t; + +typedef struct { + uint8_t feed_rate; // Feed rate override value in percent + uint8_t rapid_rate; // Rapids override value in percent + uint8_t spindle_rpm; // Spindle speed override value in percent + spindle_stop_t spindle_stop; // Tracks spindle stop override states + gc_override_flags_t control; // Tracks override control states. +} overrides_t; + +typedef union { + uint8_t value; + struct { + uint8_t mpg_mode :1, // MPG mode flag. Set when switched to secondary input stream. (unused for now) + probe_succeeded :1, // Tracks if last probing cycle was successful. + soft_limit :1, // Tracks soft limit errors for the state machine. + exit :1, // System exit flag. Used in combination with abort to terminate main loop. + block_delete_enabled :1, // Set to true to enable block delete + feed_hold_pending :1, + delay_overrides :1, + optional_stop_disable :1; // Set to true to disable M1 (optional stop), via realtime command + }; +} system_flags_t; + +typedef struct +{ + control_signals_t control; + limit_signals_t limits; +} signal_event_t; + +// Define global system variables +// NOTE: probe_position and position variables may need to be declared as volatiles, if problems arise. +typedef struct { + bool abort; // System abort flag. Forces exit back to main loop for reset. + bool cancel; // System cancel flag. + bool suspend; // System suspend state flag. + bool position_lost; // Set when mc_reset is called when machine is moving. + volatile bool steppers_deenergize; // Set to true to deenergize stepperes + axes_signals_t tlo_reference_set; // Axes with tool length reference offset set + int32_t tlo_reference[N_AXIS]; // Tool length reference offset + alarm_code_t alarm_pending; // Delayed alarm, currently used for probe protection + system_flags_t flags; // Assorted state flags + step_control_t step_control; // Governs the step segment generator depending on system state. + axes_signals_t homing_axis_lock; // Locks axes when limits engage. Used as an axis motion mask in the stepper ISR. + axes_signals_t homing; // Axes with homing enabled. + overrides_t override; // Override values & states + report_tracking_flags_t report; // Tracks when to add data to status reports. + parking_state_t parking_state; // Tracks parking state + hold_state_t holding_state; // Tracks holding state + int32_t probe_position[N_AXIS]; // Last probe position in machine coordinates and steps. + volatile probing_state_t probing_state; // Probing state value. Used to coordinate the probing cycle with stepper ISR. + volatile uint_fast16_t rt_exec_state; // Realtime executor bitflag variable for state management. See EXEC bitmasks. + volatile uint_fast16_t rt_exec_alarm; // Realtime executor bitflag variable for setting various alarms. + float spindle_rpm; +#ifdef PID_LOG + pid_data_t pid_log; +#endif + // The following variables are only cleared upon soft reset if position is likely lost, do NOT move. homed must be first! + axes_signals_t homed; // Indicates which axes has been homed. + float home_position[N_AXIS]; // Home position for homed axes + // The following variables are not cleared upon soft reset, do NOT move. alarm must be first! + alarm_code_t alarm; // Current alarm, only valid if system state is STATE_ALARM. + bool cold_start; // Set to true on boot, is false on subsequent soft resets. + bool driver_started; // Set to true when driver initialization is completed. + bool mpg_mode; // To be moved to system_flags_t + signal_event_t last_event; // Last signal events (control and limits signal). + int32_t position[N_AXIS]; // Real-time machine (aka home) position vector in steps. +} system_t; + +typedef status_code_t (*sys_command_ptr)(sys_state_t state, char *args); + +typedef struct +{ + const char *command; + bool noargs; + sys_command_ptr execute; +} sys_command_t; + +typedef struct sys_commands_str { + const uint8_t n_commands; + const sys_command_t *commands; + struct sys_commands_str *(*on_get_commands)(void); +} sys_commands_t; + +extern system_t sys; + +// Executes an internal system command, defined as a string starting with a '$' +status_code_t system_execute_line (char *line); + +// Execute the startup script lines stored in non-volatile storage upon initialization +void system_execute_startup (void); + +void system_flag_wco_change (void); + +// Returns machine position of axis 'idx'. Must be sent a 'step' array. +//float system_convert_axis_steps_to_mpos(int32_t *steps, uint_fast8_t idx); + +// Updates a machine 'position' array based on the 'step' array sent. +void system_convert_array_steps_to_mpos (float *position, int32_t *steps); + +// Checks and reports if target array exceeds machine travel limits. +bool system_check_travel_limits (float *target); + +// Checks and limit jog commands to within machine travel limits. +void system_apply_jog_limits (float *target); + +// Raise and report alarm state +void system_raise_alarm (alarm_code_t alarm); + +// Special handlers for setting and clearing Grbl's real-time execution flags. +#define system_set_exec_state_flag(mask) hal.set_bits_atomic(&sys.rt_exec_state, (mask)) +#define system_clear_exec_state_flag(mask) hal.clear_bits_atomic(&sys.rt_exec_state, (mask)) +#define system_clear_exec_states() hal.set_value_atomic(&sys.rt_exec_state, 0) +#define system_set_exec_alarm(code) hal.set_value_atomic(&sys.rt_exec_alarm, (uint_fast16_t)(code)) +#define system_clear_exec_alarm() hal.set_value_atomic(&sys.rt_exec_alarm, 0) + +void control_interrupt_handler (control_signals_t signals); + +#endif diff --git a/tool_change.c b/tool_change.c new file mode 100644 index 0000000..797c666 --- /dev/null +++ b/tool_change.c @@ -0,0 +1,476 @@ +/* + tool_change.c - An embedded CNC Controller with rs274/ngc (g-code) support + + Manual tool change with option for automatic touch off + + Part of grblHAL + + Copyright (c) 2020-2021 Terje Io + + Grbl is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + Grbl is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with Grbl. If not, see . +*/ + +#include + +#include "hal.h" +#include "motion_control.h" +#include "protocol.h" +#include "tool_change.h" + +// NOTE: only used when settings.homing.flags.force_set_origin is true +#ifndef LINEAR_AXIS_HOME_OFFSET +#define LINEAR_AXIS_HOME_OFFSET -1.0f +#endif + +#ifndef TOOL_CHANGE_PROBE_RETRACT_DISTANCE +#define TOOL_CHANGE_PROBE_RETRACT_DISTANCE 2.0f +#endif + +static bool block_cycle_start; +static volatile bool execute_posted = false; +static volatile uint32_t spin_lock = 0; +static float tool_change_position; +static tool_data_t current_tool = {0}, *next_tool = NULL; +static plane_t plane; +static coord_data_t target = {0}, previous; +static driver_reset_ptr driver_reset = NULL; +static enqueue_realtime_command_ptr enqueue_realtime_command = NULL; +static control_signals_callback_ptr control_interrupt_callback = NULL; + +// Set tool offset on successful $TPW probe, prompt for retry on failure. +// Called via probe completed event. +static void on_probe_completed (void) +{ + if(!sys.flags.probe_succeeded) + report_message("Probe failed, try again.", Message_Plain); + else if(sys.tlo_reference_set.mask & bit(plane.axis_linear)) + gc_set_tool_offset(ToolLengthOffset_EnableDynamic, plane.axis_linear, sys.probe_position[plane.axis_linear] - sys.tlo_reference[plane.axis_linear]); +// else error? +} + +// Restore HAL pointers on completion or reset. +static void change_completed (void) +{ + if(enqueue_realtime_command) { + while(spin_lock); + hal.irq_disable(); + hal.stream.enqueue_realtime_command = enqueue_realtime_command; + enqueue_realtime_command = NULL; + hal.irq_enable(); + } + + if(control_interrupt_callback) { + while(spin_lock); + hal.irq_disable(); + hal.control.interrupt_callback = control_interrupt_callback; + control_interrupt_callback = NULL; + hal.irq_enable(); + } + + grbl.on_probe_completed = NULL; + gc_state.tool_change = false; +} + + +// Reset claimed HAL entry points and restore previous tool if needed on soft restart. +// Called from EXEC_RESET and EXEC_STOP handlers (via HAL). +static void reset (void) +{ + if(next_tool) { //TODO: move to gc_xxx() function? + // Restore previous tool if reset is during change +#ifdef N_TOOLS + if((sys.report.tool = current_tool.tool != next_tool->tool)) + memcpy(gc_state.tool, ¤t_tool, sizeof(tool_data_t)); +#else + if((sys.report.tool = current_tool.tool != next_tool->tool)) + memcpy(next_tool, ¤t_tool, sizeof(tool_data_t)); +#endif + gc_state.tool_pending = gc_state.tool->tool; + next_tool = NULL; + } + + change_completed(); + driver_reset(); +} + +// Restore coolant and spindle status, return controlled point to original position. +static bool restore (void) +{ + plan_line_data_t plan_data = {0}; + + plan_data.condition.rapid_motion = On; + + target.values[plane.axis_linear] = tool_change_position; + mc_line(target.values, &plan_data); + + memcpy(&target, &previous, sizeof(coord_data_t)); + target.values[plane.axis_linear] = tool_change_position; + mc_line(target.values, &plan_data); + + if(protocol_buffer_synchronize()) { + + sync_position(); + + coolant_sync(gc_state.modal.coolant); + spindle_restore(gc_state.modal.spindle, gc_state.spindle.rpm); + + previous.values[plane.axis_linear] += gc_get_offset(plane.axis_linear); + mc_line(previous.values, &plan_data); + } + + if(protocol_buffer_synchronize()) { + sync_position(); + memcpy(¤t_tool, next_tool, sizeof(tool_data_t)); + } + + return !ABORTED; +} + +// Issue warning on cycle start event if touch off by $TPW is pending. +// Used in Manual and Manual_G59_3 modes ($341=1 or $341=2). Called from the foreground process. +static void execute_warning (sys_state_t state) +{ + report_message("Perform a probe with $TPW first!", Message_Plain); +} + +// Execute restore position after touch off (on cycle start event). +// Used in Manual and Manual_G59_3 modes ($341=1 or $341=2). Called from the foreground process. +static void execute_restore (sys_state_t state) +{ + // Get current position. + system_convert_array_steps_to_mpos(target.values, sys.position); + + bool ok = restore(); + + change_completed(); + + report_feedback_message(Message_None); + + if(ok) + system_set_exec_state_flag(EXEC_CYCLE_START); +} + +// Execute touch off on cycle start event from @ G59.3 position. +// Used in SemiAutomatic mode ($341=3) only. Called from the foreground process. +static void execute_probe (sys_state_t state) +{ +#if COMPATIBILITY_LEVEL <= 1 + bool ok; + coord_data_t offset; + plan_line_data_t plan_data = {0}; + gc_parser_flags_t flags = {0}; + + // G59.3 contains offsets to position of TLS. + settings_read_coord_data(CoordinateSystem_G59_3, &offset.values); + + plan_data.condition.rapid_motion = On; + + target.values[plane.axis_0] = offset.values[plane.axis_0]; + target.values[plane.axis_1] = offset.values[plane.axis_1]; + + if((ok = mc_line(target.values, &plan_data))) { + + target.values[plane.axis_linear] = offset.values[plane.axis_linear]; + ok = mc_line(target.values, &plan_data); + + plan_data.feed_rate = settings.tool_change.seek_rate; + plan_data.condition.value = 0; + target.values[plane.axis_linear] -= settings.tool_change.probing_distance; + + if((ok = ok && mc_probe_cycle(target.values, &plan_data, flags) == GCProbe_Found)) + { + system_convert_array_steps_to_mpos(target.values, sys.probe_position); + + // Retract a bit and perform slow probe. + plan_data.feed_rate = settings.tool_change.pulloff_rate; + target.values[plane.axis_linear] += TOOL_CHANGE_PROBE_RETRACT_DISTANCE; + if((ok = mc_line(target.values, &plan_data))) { + plan_data.feed_rate = settings.tool_change.feed_rate; + target.values[plane.axis_linear] -= (TOOL_CHANGE_PROBE_RETRACT_DISTANCE + 2.0f); + ok = mc_probe_cycle(target.values, &plan_data, flags) == GCProbe_Found; + } + } + + if(ok) { + if(!(sys.tlo_reference_set.mask & bit(plane.axis_linear))) { + sys.tlo_reference[plane.axis_linear] = sys.probe_position[plane.axis_linear]; + sys.tlo_reference_set.mask |= bit(plane.axis_linear); + sys.report.tlo_reference = On; + report_feedback_message(Message_ReferenceTLOEstablished); + } else + gc_set_tool_offset(ToolLengthOffset_EnableDynamic, plane.axis_linear, + sys.probe_position[plane.axis_linear] - sys.tlo_reference[plane.axis_linear]); + + ok = restore(); + } + } + + change_completed(); + + if(ok) + system_set_exec_state_flag(EXEC_CYCLE_START); +#endif +} + +// Trap cycle start commands and redirect to foreground process +// by adding the function to be called to the realtime execution queue. +ISR_CODE static void trap_control_cycle_start (control_signals_t signals) +{ + spin_lock++; + + if(signals.cycle_start) { + if(!execute_posted) { + if(!block_cycle_start) + execute_posted = protocol_enqueue_rt_command(settings.tool_change.mode == ToolChange_SemiAutomatic ? execute_probe : execute_restore); + else + protocol_enqueue_rt_command(execute_warning); + } + signals.cycle_start = Off; + } else + control_interrupt_callback(signals); + + spin_lock--; +} + +ISR_CODE static bool trap_stream_cycle_start (char c) +{ + bool drop = false; + + spin_lock++; + + if((drop = (c == CMD_CYCLE_START || c == CMD_CYCLE_START_LEGACY))) { + if(!execute_posted) { + if(!block_cycle_start) + execute_posted = protocol_enqueue_rt_command(settings.tool_change.mode == ToolChange_SemiAutomatic ? execute_probe : execute_restore); + else + protocol_enqueue_rt_command(execute_warning); + } + } else + drop = enqueue_realtime_command(c); + + spin_lock--; + + return drop; +} + +// Set next and/or current tool. Called by gcode.c on on a Tn or M61 command (via HAL). +static void tool_select (tool_data_t *tool, bool next) +{ + next_tool = tool; + if(!next) + memcpy(¤t_tool, tool, sizeof(tool_data_t)); +} + +// Start a tool change sequence. Called by gcode.c on a M6 command (via HAL). +static status_code_t tool_change (parser_state_t *parser_state) +{ + if(next_tool == NULL) + return Status_GCodeToolError; + + if(current_tool.tool == next_tool->tool) + return Status_OK; + +#if COMPATIBILITY_LEVEL > 1 + if(settings.tool_change.mode == ToolChange_Manual_G59_3 || settings.tool_change.mode == ToolChange_SemiAutomatic) + return Status_GcodeUnsupportedCommand; +#endif + +#ifdef TOOL_LENGTH_OFFSET_AXIS + plane.axis_linear = TOOL_LENGTH_OFFSET_AXIS; + #if TOOL_LENGTH_OFFSET_AXIS == X_AXIS + plane.axis_0 = Y_AXIS; + plane.axis_1 = Z_AXIS; + #elif TOOL_LENGTH_OFFSET_AXIS == Y_AXIS + plane.axis_0 = Z_AXIS; + plane.axis_1 = X_AXIS; + #else + plane.axis_0 = X_AXIS; + plane.axis_1 = Y_AXIS; + #endif +#else + gc_get_plane_data(&plane, parser_state->modal.plane_select); +#endif + + uint8_t homed_req = settings.tool_change.mode == ToolChange_Manual ? bit(plane.axis_linear) : (X_AXIS_BIT|Y_AXIS_BIT|Z_AXIS_BIT); + + if((sys.homed.mask & homed_req) != homed_req) + return Status_HomingRequired; + + if(settings.tool_change.mode != ToolChange_SemiAutomatic) + grbl.on_probe_completed = on_probe_completed; + + // Trap cycle start command and control signal. + block_cycle_start = settings.tool_change.mode != ToolChange_SemiAutomatic; + enqueue_realtime_command = hal.stream.enqueue_realtime_command; + hal.stream.enqueue_realtime_command = trap_stream_cycle_start; + control_interrupt_callback = hal.control.interrupt_callback; + hal.control.interrupt_callback = trap_control_cycle_start; + + // Stop spindle and coolant. + hal.spindle.set_state((spindle_state_t){0}, 0.0f); + hal.coolant.set_state((coolant_state_t){0}); + + execute_posted = false; + parser_state->tool_change = true; + + // Save current position. + system_convert_array_steps_to_mpos(previous.values, sys.position); + + // Establish axis assignments. + + previous.values[plane.axis_linear] -= gc_get_offset(plane.axis_linear); + + plan_line_data_t plan_data = {0}; + plan_data.condition.rapid_motion = On; + + // TODO: add? + //if(!settings.homing.flags.force_set_origin && bit_istrue(settings.homing.dir_mask.value, bit(plane.axis_linear))) + // tool_change_position = ? + //else + + tool_change_position = sys.home_position[plane.axis_linear] - settings.homing.flags.force_set_origin ? LINEAR_AXIS_HOME_OFFSET : 0.0f; + + // Rapid to home position of linear axis. + memcpy(&target, &previous, sizeof(coord_data_t)); + target.values[plane.axis_linear] = tool_change_position; + if(!mc_line(target.values, &plan_data)) + return Status_Reset; + +#if COMPATIBILITY_LEVEL <= 1 + if(settings.tool_change.mode == ToolChange_Manual_G59_3) { + + // G59.3 contains offsets to tool change position. + settings_read_coord_data(CoordinateSystem_G59_3, &target.values); + + float tmp_pos = target.values[plane.axis_linear]; + + target.values[plane.axis_linear] = tool_change_position; + if(!mc_line(target.values, &plan_data)) + return Status_Reset; + + target.values[plane.axis_linear] = tmp_pos; + if(!mc_line(target.values, &plan_data)) + return Status_Reset; + } +#endif + + protocol_buffer_synchronize(); + sync_position(); + + // Enter tool change mode, waits for cycle start to continue. + system_set_exec_state_flag(EXEC_TOOL_CHANGE); // Set up program pause for manual tool change + protocol_execute_realtime(); // Execute... + + return Status_OK; +} + +// Claim HAL tool change entry points and clear current tool offsets. +// TODO: change to survive a warm reset? +void tc_init (void) +{ + if(hal.driver_cap.atc) // Do not override driver tool change implementation! + return; + + if(!hal.stream.suspend_read) // Tool change requires support for suspending input stream. + return; + + sys.report.tlo_reference = sys.tlo_reference_set.mask != 0; + sys.tlo_reference_set.mask = 0; + + gc_set_tool_offset(ToolLengthOffset_Cancel, 0, 0.0f); + + if(settings.tool_change.mode == ToolChange_Disabled || settings.tool_change.mode == ToolChange_Ignore) { + hal.tool.select = NULL; + hal.tool.change = NULL; + } else { + hal.tool.select = tool_select; + hal.tool.change = tool_change; + if(driver_reset == NULL) { + driver_reset = hal.driver_reset; + hal.driver_reset = reset; + } + } +} + +void tc_clear_tlo_reference (axes_signals_t homing_cycle) +{ + if(settings.tool_change.mode != ToolChange_Disabled) { + + plane_t plane; + +#ifdef TOOL_LENGTH_OFFSET_AXIS + plane.axis_linear = TOOL_LENGTH_OFFSET_AXIS; +#else + gc_get_plane_data(&plane, gc_state.modal.plane_select); +#endif + if(homing_cycle.mask & (settings.mode == Mode_Lathe ? (X_AXIS_BIT|Z_AXIS_BIT) : bit(plane.axis_linear))) { + sys.report.tlo_reference = sys.tlo_reference_set.mask != 0; + sys.tlo_reference_set.mask = 0; // Invalidate tool length offset reference + } + } +} + +// Perform a probe cycle: set tool length offset and restart job if successful. +// Note: tool length offset is set by the on_probe_completed event handler. +// Called by the $TPW system command. +status_code_t tc_probe_workpiece (void) +{ + if(!(settings.tool_change.mode == ToolChange_Manual || settings.tool_change.mode == ToolChange_Manual_G59_3) || enqueue_realtime_command == NULL) + return Status_InvalidStatement; + + // TODO: add check for reference offset set? + + bool ok; + gc_parser_flags_t flags = {0}; + plan_line_data_t plan_data = {0}; + + // Get current position. + system_convert_array_steps_to_mpos(target.values, sys.position); + + flags.probe_is_no_error = On; + plan_data.feed_rate = settings.tool_change.seek_rate; + + target.values[plane.axis_linear] -= settings.tool_change.probing_distance; + + if((ok = mc_probe_cycle(target.values, &plan_data, flags) == GCProbe_Found)) + { + system_convert_array_steps_to_mpos(target.values, sys.probe_position); + + // Retract a bit and perform slow probe. + plan_data.feed_rate = settings.tool_change.pulloff_rate; + target.values[plane.axis_linear] += TOOL_CHANGE_PROBE_RETRACT_DISTANCE; + if((ok = mc_line(target.values, &plan_data))) { + + plan_data.feed_rate = settings.tool_change.feed_rate; + target.values[plane.axis_linear] -= (TOOL_CHANGE_PROBE_RETRACT_DISTANCE + 2.0f); + if((ok = mc_probe_cycle(target.values, &plan_data, flags) == GCProbe_Found)) { + // Retract a bit again so that any touch plate can be removed + system_convert_array_steps_to_mpos(target.values, sys.probe_position); + plan_data.feed_rate = settings.tool_change.seek_rate; + target.values[plane.axis_linear] += TOOL_CHANGE_PROBE_RETRACT_DISTANCE * 2.0f; + ok = mc_line(target.values, &plan_data); + } + } + } + + if(ok && protocol_buffer_synchronize()) { + sync_position(); + block_cycle_start = false; + report_message(settings.tool_change.mode == ToolChange_Manual_G59_3 + ? "Press cycle start to continue." + : "Remove any touch plate and press cycle start to continue.", Message_Plain); + } + + return ok ? Status_OK : Status_GCodeToolError; +} diff --git a/tool_change.h b/tool_change.h new file mode 100644 index 0000000..ed9b3f4 --- /dev/null +++ b/tool_change.h @@ -0,0 +1,31 @@ +/* + tool_change.h - An embedded CNC Controller with rs274/ngc (g-code) support + + Manual tool change with automatic touch off + + Part of grblHAL + + Copyright (c) 2020 Terje Io + + Grbl is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + Grbl is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with Grbl. If not, see . +*/ + +#ifndef _TOOL_CHANGE_H_ +#define _TOOL_CHANGE_H_ + +void tc_init (void); +status_code_t tc_probe_workpiece (void); +void tc_clear_tlo_reference (axes_signals_t homing_cycle); + +#endif diff --git a/wall_plotter.c b/wall_plotter.c new file mode 100644 index 0000000..1c9380a --- /dev/null +++ b/wall_plotter.c @@ -0,0 +1,261 @@ +/* + wall_plotter.c - wall plotter kinematics implementation + + Part of grblHAL + + Code lifted from Grbl_Esp32 pull request by user @ https://github.com/rognlien + + Original code here: https://github.com/jasonwebb/grbl-mega-wall-plotter + Note: homing is not implemented! + + Bits also pulled from: https://github.com/ldocull/MaslowDue + + Grbl is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + Grbl is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with Grbl. If not, see . +*/ + +#include "grbl.h" + +#ifdef WALL_PLOTTER + +#include "settings.h" +#include "planner.h" +#include "kinematics.h" + +#define A_MOTOR X_AXIS // Must be X_AXIS +#define B_MOTOR Y_AXIS // Must be Y_AXIS +#define MAX_SEG_LENGTH_MM 2.0f + +typedef struct { + int32_t width; + float width_mm; + float width_pow; + int32_t height; + int32_t width_2; + int32_t height_2; + int32_t spindlezero[2]; + float spindlezero_mm[2]; +} machine_t; + +typedef struct { + float a; + float b; +} coord_t; + +static machine_t machine = {0}; + +// 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 void wp_convert_array_steps_to_mpos (float *position, int32_t *steps) +{ + coord_t len; + + len.a = (float)steps[A_MOTOR] / settings.axis[A_MOTOR].steps_per_mm; + len.b = (float)steps[B_MOTOR] / settings.axis[B_MOTOR].steps_per_mm; + + position[X_AXIS] = (machine.width_pow + len.a * len.a - len.b * len.b) / (2.0f * machine.width_mm); + len.a = machine.width_mm - position[X_AXIS]; + position[Y_AXIS] = sqrtf(len.b * len.b - len.a * len.a ); + position[Z_AXIS] = steps[Z_AXIS] / settings.axis[Z_AXIS].steps_per_mm; +} + +// 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 int32_t wp_convert_to_a_motor_steps (float *target) +{ + return (int32_t)lroundf(sqrtf(target[A_MOTOR] * target[A_MOTOR] + target[B_MOTOR] * target[B_MOTOR]) * settings.axis[A_MOTOR].steps_per_mm); +} + +inline static int32_t wp_convert_to_b_motor_steps (float *target) +{ + float xpos = machine.width_mm - target[A_MOTOR]; + return (int32_t)lroundf(sqrtf(xpos * xpos + target[B_MOTOR] * target[B_MOTOR]) * settings.axis[B_MOTOR].steps_per_mm); +} + +// Transform absolute position from cartesian coordinate system (mm) to wall plotter coordinate system (step) +static void wp_plan_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); + + target_steps[A_MOTOR] = wp_convert_to_a_motor_steps(target); + target_steps[B_MOTOR] = wp_convert_to_b_motor_steps(target); +} + +// Wall plotter is circular in motion, so long lines must be divided up +static bool wp_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 uint_fast8_t wp_limits_get_axis_mask (uint_fast8_t idx) +{ + return ((idx == A_MOTOR) || (idx == B_MOTOR)) ? (bit(X_AXIS) | bit(Y_AXIS)) : bit(idx); +} + + +static void wp_limits_set_target_pos (uint_fast8_t idx) // fn name? +{ + float xy[2]; + int32_t axis_position; + + xy[X_AXIS] = sys.position[X_AXIS] / settings.axis[X_AXIS].steps_per_mm; + xy[Y_AXIS] = sys.position[Y_AXIS] / settings.axis[Y_AXIS].steps_per_mm; + + switch(idx) { + case X_AXIS: + axis_position = wp_convert_to_b_motor_steps(xy); + sys.position[A_MOTOR] = axis_position; + sys.position[B_MOTOR] = -axis_position; + break; + case Y_AXIS: + sys.position[A_MOTOR] = sys.position[B_MOTOR] = wp_convert_to_a_motor_steps(xy); + break; + default: + sys.position[idx] = 0; + break; + } +} + + +// Set machine positions for homed limit switches. Don't update non-homed axes. +// NOTE: settings.max_travel[] is stored as a negative value. +static void wp_limits_set_machine_positions (axes_signals_t cycle) +{ + float xy[2]; + uint_fast8_t idx = N_AXIS; + + xy[X_AXIS] = sys.position[X_AXIS] / settings.axis[X_AXIS].steps_per_mm; + xy[Y_AXIS] = sys.position[Y_AXIS] / settings.axis[Y_AXIS].steps_per_mm; + + if(settings.homing.flags.force_set_origin) { + if (cycle.mask & bit(--idx)) do { + switch(--idx) { + case X_AXIS: + sys.position[A_MOTOR] = wp_convert_to_b_motor_steps(xy); + sys.position[B_MOTOR] = - sys.position[A_MOTOR]; + break; + case Y_AXIS: + sys.position[A_MOTOR] = wp_convert_to_a_motor_steps(xy); + sys.position[B_MOTOR] = sys.position[A_MOTOR]; + break; + default: + sys.position[idx] = 0; + break; + } + } while (idx); + } else do { + if (cycle.mask & bit(--idx)) { + int32_t off_axis_position; + int32_t set_axis_position = bit_istrue(settings.homing.dir_mask.value, bit(idx)) + ? lroundf((settings.axis[idx].max_travel + settings.homing.pulloff) * settings.axis[idx].steps_per_mm) + : lroundf(-settings.homing.pulloff * settings.axis[idx].steps_per_mm); + switch(idx) { + case X_AXIS: + off_axis_position = wp_convert_to_b_motor_steps(xy); + sys.position[A_MOTOR] = set_axis_position + off_axis_position; + sys.position[B_MOTOR] = set_axis_position - off_axis_position; + break; + case Y_AXIS: + off_axis_position = wp_convert_to_a_motor_steps(xy); + sys.position[A_MOTOR] = off_axis_position + set_axis_position; + sys.position[B_MOTOR] = off_axis_position - set_axis_position; + break; + default: + sys.position[idx] = set_axis_position; + break; + } + } + } while(idx); +} + + +// Initialize API pointers for Wall Plotter kinematics +void wall_plotter_init (void) +{ + machine.width_mm = -settings.axis[A_MOTOR].max_travel; + machine.width = (int32_t)(machine.width_mm * settings.axis[A_MOTOR].steps_per_mm); + machine.width_2 = machine.width >> 1; + machine.width_pow = machine.width_mm * machine.width_mm; + machine.height = (int32_t)((float)settings.axis[B_MOTOR].max_travel * settings.axis[B_MOTOR].steps_per_mm); + machine.height_2 = machine.height >> 1; + machine.spindlezero[A_MOTOR] = 0; // machine.width_2; + machine.spindlezero[B_MOTOR] = 0; // machine.height_2; + machine.spindlezero_mm[A_MOTOR] = (float)machine.spindlezero[A_MOTOR] / settings.axis[A_MOTOR].steps_per_mm; + machine.spindlezero_mm[B_MOTOR] = (float)machine.spindlezero[B_MOTOR] / settings.axis[B_MOTOR].steps_per_mm; + + sys.position[B_MOTOR] = machine.width; + + kinematics.limits_set_target_pos = wp_limits_set_target_pos; + kinematics.limits_get_axis_mask = wp_limits_get_axis_mask; + kinematics.limits_set_machine_positions = wp_limits_set_machine_positions; + kinematics.plan_target_to_steps = wp_plan_target_to_steps; + kinematics.convert_array_steps_to_mpos = wp_convert_array_steps_to_mpos; + kinematics.segment_line = wp_segment_line; +} + +#endif diff --git a/wall_plotter.h b/wall_plotter.h new file mode 100644 index 0000000..d7ed23f --- /dev/null +++ b/wall_plotter.h @@ -0,0 +1,28 @@ +/* + wall_plotter.h - wall plotter kinematics implementation + + Part of grblHAL + + Copyright (c) 2019 Terje Io + + Grbl is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + Grbl is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with Grbl. If not, see . +*/ + +#ifndef _WALL_PLOTTER_H_ +#define _WALL_PLOTTER_H_ + +// Initialize HAL pointers for Wall Plotter kinematics +void wall_plotter_init (void); + +#endif