diff --git a/.gitignore b/.gitignore old mode 100755 new mode 100644 index c4b9fe6c..d57872f6 --- a/.gitignore +++ b/.gitignore @@ -24,6 +24,7 @@ xcuserdata *.elf *.hex *.eep +*.log *.lss *.map *.srec @@ -55,6 +56,6 @@ g2core/g2core.componentinfo.xml .tags g2core/compile_commands.json -.vscode/.cortex-debug.peripherals.state.json .vscode/.cortex-debug.registers.state.json +.vscode/.cortex-debug.peripherals.state.json .vscode/ipch diff --git a/.travis.yml b/.travis.yml index bacdcc8f..a8555f40 100644 --- a/.travis.yml +++ b/.travis.yml @@ -45,7 +45,7 @@ deploy: # OR, when we move to Master, we remove this setting. prerelease: true api_key: - secure: hBB3dHzLM97onnpneoxpewDFYfmcDmeE6FPFRxplaGYROO7SjML8orFCGMLX1cEIlvn4lwzAB+NlE2kreujxiB/gEdZy5DNk8ts+QrbfRXax/VmCNgZYp0tOVn037UFi39rKzx8olbFTImdQypMVf5mdbusY64hTpWrrsZ4uBoU= + secure: SFQe2M+qWfu4Lm4Q4W6v+JkSv0d8u5/K6MpEBPT6KUvFcIJb0fm7aHzFcZ03Ul8cx9yjmuGbNGId1hNUXCWxM+MeaIWqnx1cvJAEIQpjZV0nQ6yHeO//jX5PRmEBykNmq/GLqILbHkRAFRxBC4TwtBcRFlS/p7kEC9q3tNac5gg= skip_cleanup: true file: - ${TRAVIS_BUILD_DIR}/g2core/bin/g2core-Othermill-g2v9k-${TRAVIS_TAG}.elf diff --git a/.vscode/settings.json b/.vscode/settings.json index 2eb5aeca..cc13f753 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -31,7 +31,21 @@ "string_view": "cpp", "tuple": "cpp", "utility": "cpp", - "cmath": "cpp" + "cmath": "cpp", + "chrono": "cpp", + "__bit_reference": "cpp", + "__node_handle": "cpp", + "bitset": "cpp", + "deque": "cpp", + "__memory": "cpp", + "filesystem": "cpp", + "optional": "cpp", + "ratio": "cpp", + "system_error": "cpp", + "vector": "cpp", + "array": "cpp", + "istream": "cpp", + "ostream": "cpp" }, "C_Cpp.dimInactiveRegions": true, "C_Cpp.clang_format_fallbackStyle": "{ BasedOnStyle: Google, IndentWidth: 4, ColumnLimit: 120 }", diff --git a/LICENSE b/LICENSE new file mode 100644 index 00000000..07573afc --- /dev/null +++ b/LICENSE @@ -0,0 +1,75 @@ +## Intent + +The g2core **project** is a motion control **application** built on a set of underlying **components**. We want the licensing to reflect this, and to treat using the application as a whole somewhat differently than using the components as a library. The intent of the g2core licensing is summarized as: +* Ensure that the g2core project remains open source +* Encourage contribution to the project and ensure that most changes and enhancements are returned to the community +* Make it easy to use g2core components in free and commercial projects/products- i.e. encourage use of g2core components as a library +* Notwithstanding the above, make the application when used as a whole retain GPLv2 provisions + +G2core licensing is based on GPLv2 with the [BeRTOS extension](http://www.bertos.org/discover/license) to enable using component files without invoking GPL copyleft. To this end, all files in the project are licensed under GPLv2. Those files that are considered "components" carry the BeRTOS exception, that allows their use without opening source code that they come in contact with. + +G2core also includes [Motate](https://github.com/synthetos/Motate) hardware abstraction components that are licensed under the same scheme. + +Most of what follows is shamelessly cribbed from the BeRTOS site. + + +## Info + +### You are free to... + +- Include g2core components within any product, distributed under any license, including commercial licenses and/or closed-source licenses +- Modify g2core components as you want under the following conditions: + - Attribution - You must declare in a written statement in documentation and source code that you are using g2core components in your application and offer to provide the (possibly modified) g2core source code + - Share-alike - If you modify g2core components, you may distribute them only under the original license + - Contribution - If you modify g2core components, you must contribute the modifications back to the g2core project + + +### What you can do with g2core components... + +If you are a company or individual doing commercial embedded products, you can: +- Download and use g2core components as you want +- Sell products based on g2core components without paying royalties or other fees +- Sell products based on g2core components without opening or giving away your other application source code + + +### What you must do with g2core components... + +If you sell or otherwise distribute code/products that use g2core components you must: +* Provide attribution that you are using g2core components in the documentation and source code. Text like this is sufficient: +
+"This product uses g2core components (http://www.synthetos.com), Copyright 2018"
+
+* If you modified g2core components offer the modified versions for download from your website. Alternately, you can contribute your modifications to the g2core project, where they may be integrated in whole or in part, and/or hosted independently under the project if not integrated. + + +## Example Text That Includes the BeRTOS Exception + +
+/*
+ * gpio.h - Digital IO  handling functions
+ * This file is part of the g2core project
+ *
+ * Copyright (c) 2015 - 2018 Alden S. Hart, Jr.
+ * Copyright (c) 2015 - 2018 Robert Giseburt
+ *
+ * This file ("the software") is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License, version 2 as published by the
+ * Free Software Foundation. You should have received a copy of the GNU General Public
+ * License, version 2 along with the software.  If not, see http://www.gnu.org/licenses.
+ *
+ * As a special exception, you may use this file as part of a software library without
+ * restriction. Specifically, if other files instantiate templates or use macros or
+ * inline functions from this file, or you compile this file and link it with  other
+ * files to produce an executable, this file does not by itself cause the resulting
+ * executable to be covered by the GNU General Public License. This exception does not
+ * however invalidate any other reasons why the executable file might be covered by the
+ * GNU General Public License.
+ *
+ * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT WITHOUT ANY
+ * WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
+ * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT
+ * SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF
+ * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ */
+
diff --git a/README.md b/README.md index bb96f933..a715122b 100644 --- a/README.md +++ b/README.md @@ -2,15 +2,63 @@ [![Build Status](https://travis-ci.org/synthetos/g2.svg?branch=edge)](https://travis-ci.org/synthetos/g2) [![Issues in Ready](https://badge.waffle.io/synthetos/g2.svg?label=ready&title=Ready)](http://waffle.io/synthetos/g2) [![Issues in Progress](https://badge.waffle.io/synthetos/g2.svg?label=in%20progress&title=In%20Progress)](http://waffle.io/synthetos/g2) +# What it is + +g2core is a 9 axes (XYZABC+UVW) motion control system designed for high-performance on small to mid-sized machines. + +* CNC +* 3D printing +* Laser cutting +* Robotics + +Our default target is the [Arduino Due](https://store.arduino.cc/arduino-due), though it can also be used with other boards. + +Some features: + +* 9 axis motion (XYZABC+UVW). + * **Note** - UVW is only in the `edge` branch for now. +* Jerk controlled motion for acceleration planning (3rd order motion planning) +* Status displays ('?' character) +* XON/XOFF and RTS/CTS protocol over USB serial +* RESTful interface using JSON + +# Mailing List + +For both user and developer discussions of g2core, we recently created a mailing list: + +* https://lists.links.org/mailman/listinfo/g2core + +Please feel welcome to join in. :smile: + # g2core - Edge Branch -G2 [Edge](https://github.com/synthetos/g2/tree/edge) is the branch for beta testing new features under development. New features are developed in feature branches and merged into the edge branch. Periodically edge is promoted to master. - -That said, Edge is for the adventurous. It is not guaranteed to be stable, but we do our best to achieve this. For production uses we recommend using the [Master branch](https://github.com/synthetos/g2/tree/master). +G2 [Edge](https://github.com/synthetos/g2/tree/edge) is the branch for beta testing new features under development. New features are developed in feature branches and merged into the edge branch. Periodically edge is promoted to (stable) master. +Edge is for the adventurous. It is not guaranteed to be stable, but we do our best to achieve this. For production uses we recommend using the [Master branch](https://github.com/synthetos/g2/tree/master). ## Firmware Build 101 `{fb:101.xx}` ### Feature Enhancements +New features added. See linked issues and pull requests for details +- Added [UVW axes](https://github.com/synthetos/g2/wiki/9-Axis-UVW-Operation) for 9 axis control. [See also: Issue 304](https://github.com/synthetos/g2/issues/304) +- Added [Enhanced Feedhold Functions](https://github.com/synthetos/g2/wiki/Feedhold,-Resume,-and-Other-Simple-Commands) +- Added explicit [Job Kill ^d](https://github.com/synthetos/g2/wiki/Feedhold,-Resume,-and-Other-Simple-Commands#job-kill) - has the effect of an M30 (program end) +- Documented [Communications Startup Tests](https://github.com/synthetos/g2/wiki/g2core-Communications#enqack---checking-for-clean-startup) + + +### Internal Changes and Bug Fixes +Many things have changed in the internals for this very large pull request. The following list highlights some of these changes but is not meant to be comprehensive. +- Added explicit typing and type testing to JSON variables. +- As part of the above, 32bit integers are not float casts, and therefore retain full accuracy. Line numbers may now reliably go to 2,000,000,000 +- Movement towards getters and setters as initial stage of refactoring the Big Table :) +- Bugfix: Fixed root finding problem in feedhold exit velocity calculation +- Bugfix: fixed bug in B and C axis assignment in coordinate rotation code +- PR #334 A, B, C axes radius defaults to use motors 4, 5, & 6 +- PR #336, Issue #336 partial solution to coolant initialization +- PR #299, Issue #298 fix for reading nested JSON value errors + +## Feature Enhancements + +### Firmware Build 101 `{fb:101.xx}` The fb:101 release is a mostly internal change from the fb:100 branches. Here are the highlights, more detailed on each item are further below: - Updated motion execution at the segment (smallest) level to be linear velocity instead of constant velocity, resulting in notably smoother motion and more faithful execution of the jerk limitations. (Incidentally, the sound of the motors is also slightly quieter and more "natural.") @@ -26,14 +74,47 @@ The fb:101 release is a mostly internal change from the fb:100 branches. Here ar - Experimental setting to have traverse (G0) use the 'high jerk' axis settings. - Outputs are now configured at board initialization (and later) to honor the settings more faithfully. This includes setting the pin high or low as soon as possible. +### Firmware Build 100 `{fb:100.xx}` + +The fb:100 release is a major change from the fb:089 and earlier branches. It represents about a year of development and has many major feature enhancements summarized below. These are described in more detail in the rest of this readme and the linked wiki pages. +- New Gcode and CNC features +- 3d printing support, including [Marlin Compatibility](https://github.com/synthetos/g2/wiki/Marlin-Compatibility) +- GPIO system enhancements +- Planner enhancements and other operating improvements for high-speed operation +- Initial support for new processors, including the ARM M7 + ### Project Changes +The project is now called g2core (even if the repo remains g2). As of this release the g2core code base is split from the TinyG code base. TinyG will continue to be supported for the Xmega 8-bit platform, and new features will be added, specifically as related to continued support for CNC milling applications. The g2core project will focus on various ARM platforms, as it currently does, and add functions that are not possible in the 8-bit platform. + +In this release the Motate hardware abstraction layer has been split into a separate project and is included in g2core as a git submodule. This release also provides better support for cross platform / cross target compilation. A summary of project changes is provided below, with details in this readme and linked wiki pages. +- Motate submodule +- Cross platform / cross target support +- Multiple processor support - ARM M3, M4, M7 cores +- Device tree / multiple motor types +- Simplified host-to-board communication protocol (line mode) +- NodeJS host module for host-to-board communications + +### More To Come +The fb:100 release is the base for number of other enhancements in the works and planned, including: +- Further enhancements to GPIO system +- Additional JSON processing and UI support +- Enhancements to 3d printer support, including a simplified g2 printer dialect + ## Changelog for Edge Branch ### Edge branch, Build 101.xx This build is primarily focused on support for the new boards based on the Atmel SamS70 family, as well as refining the motion control and long awaited feature enhancements. This list will be added to as development proceed.s +### Edge branch, Build 100.xx + +Build 100.xx has a number of changes, mostly related to extending Gcode support and supporting 3D printing using g2core. These include temperature controls, auto-bed leveling, planner performance improvements and active JSON comments in Gcode. + +Communications has advanced to support a linemode protocol to greatly simplify host communications and flow control for very rapid Gcode streams. Please read the Communications pages for details. Also see the NodeJS communications module docs if you are building a UI or host controller. + +Build 100.xx also significantly advances the project structure to support multiple processor architectures, hardware configurations and machine configurations in the same code base. Motate has been cleaved off into its own subproject. We recommend carefully reading the Dev pages if you are coding or compiling. + #### Functional Changes: *Note: Click the header next to the arrow to expand and display the details.* @@ -112,8 +193,8 @@ This build is primarily focused on support for the new boards based on the Atmel
Planner settings control from board files - - The defines `PLANNER_BUFFER_POOL_SIZE` and `MIN_SEGMENT_MS` are now set in the `board/*/hardware.h` files. - - `PLANNER_BUFFER_POOL_SIZE` sets the size of the planner buffer array. + - The defines `PLANNER_QUEUE_SIZE` and `MIN_SEGMENT_MS` are now set in the `board/*/hardware.h` files. + - `PLANNER_QUEUE_SIZE` sets the size of the planner buffer array. - Default value if not defined: `48` - `MIN_SEGMENT_MS` sets the minimum segment time (in milliseconds) and several other settings that are comuted based on it. - Default values if not defined: `0.75` diff --git a/Resources/Documentation/SystemStates.graffle b/Resources/Documentation/SystemStates.graffle new file mode 100644 index 00000000..7ef6770e Binary files /dev/null and b/Resources/Documentation/SystemStates.graffle differ diff --git a/Resources/TinyG2-OSX-Programmer/ReadMe.txt b/Resources/TinyG2-OSX-Programmer/ReadMe.txt index cdfdbe9c..0f069025 100644 --- a/Resources/TinyG2-OSX-Programmer/ReadMe.txt +++ b/Resources/TinyG2-OSX-Programmer/ReadMe.txt @@ -2,7 +2,7 @@ PROGRAMMING THE DUE FROM OS X ============================= These notes are also available here and may be more up-to-date: -https://github.com/synthetos/g2/wiki/Programming-TinyG2 +https://github.com/synthetos/g2/wiki/Flashing-g2core-with-OSX Download the Arduino 1.5.2 BETA (not the 1.0.5 release at the top of the page) from http://arduino.cc/en/Main/Software for OS X. @@ -37,4 +37,4 @@ The output should look something like this (the numbers after cu.usbmodem will b Now disconnect the Due and plug the USB cable into the other port. You now have a Due/TinyG2. --Synthetos Team \ No newline at end of file +-Synthetos Team diff --git a/Resources/generate_motors_cfgArray.js b/Resources/generate_motors_cfgArray.js index d41aee68..341048cc 100755 --- a/Resources/generate_motors_cfgArray.js +++ b/Resources/generate_motors_cfgArray.js @@ -5,34 +5,36 @@ for (n=1; n<=6; n++) { console.log(` #if (MOTORS >= ${n}) - { "${n}","${n}ma", _fip, 0, st_print_ma, get_ui8, st_set_ma, (float *)&st_cfg.mot[MOTOR_${n}].motor_map, M${n}_MOTOR_MAP }, - { "${n}","${n}sa", _fip, 3, st_print_sa, get_flt, st_set_sa, (float *)&st_cfg.mot[MOTOR_${n}].step_angle, M${n}_STEP_ANGLE }, - { "${n}","${n}tr", _fipc, 4, st_print_tr, get_flt, st_set_tr, (float *)&st_cfg.mot[MOTOR_${n}].travel_rev, M${n}_TRAVEL_PER_REV }, - { "${n}","${n}mi", _fip, 0, st_print_mi, get_int, st_set_mi, (float *)&st_cfg.mot[MOTOR_${n}].microsteps, M${n}_MICROSTEPS }, - { "${n}","${n}su", _fipi, 5, st_print_su, st_get_su,st_set_su, (float *)&st_cfg.mot[MOTOR_${n}].steps_per_unit, M${n}_STEPS_PER_UNIT }, - { "${n}","${n}po", _fip, 0, st_print_po, get_ui8, set_01, (float *)&st_cfg.mot[MOTOR_${n}].polarity, M${n}_POLARITY }, - { "${n}","${n}pm", _fip, 0, st_print_pm, st_get_pm,st_set_pm, (float *)&cs.null, M${n}_POWER_MODE }, - { "${n}","${n}pl", _fip, 3, st_print_pl, get_flt, st_set_pl, (float *)&st_cfg.mot[MOTOR_${n}].power_level, M${n}_POWER_LEVEL }, -// { "${n}","${n}pi", _fip, 3, st_print_pi, get_flt, st_set_pi, (float *)&st_cfg.mot[MOTOR_${n}].power_idle, M${n}_POWER_IDLE }, -// { "${n}","${n}mt", _fip, 2, st_print_mt, get_flt, st_set_mt, (float *)&st_cfg.mot[MOTOR_${n}].motor_timeout, M${n}_MOTOR_TIMEOUT }, + { "${n}","${n}ma", _fip, 0, st_print_ma, st_get_ma, st_set_ma, &st_cfg.mot[MOTOR_${n}].motor_map, M${n}_MOTOR_MAP }, + { "${n}","${n}sa", _fip, 3, st_print_sa, st_get_sa, st_set_sa, &st_cfg.mot[MOTOR_${n}].step_angle, M${n}_STEP_ANGLE }, + { "${n}","${n}tr", _fipc, 4, st_print_tr, st_get_tr, st_set_tr, &st_cfg.mot[MOTOR_${n}].travel_rev, M${n}_TRAVEL_PER_REV }, + { "${n}","${n}su", _fipi, 5, st_print_su, st_get_su, st_set_su, &st_cfg.mot[MOTOR_${n}].steps_per_unit, M${n}_STEPS_PER_UNIT }, + { "${n}","${n}mi", _fip, 0, st_print_mi, st_get_mi, st_set_mi, &st_cfg.mot[MOTOR_${n}].microsteps, M${n}_MICROSTEPS }, + { "${n}","${n}po", _fip, 0, st_print_po, st_get_po, st_set_po, &st_cfg.mot[MOTOR_${n}].polarity, M${n}_POLARITY }, + { "${n}","${n}pm", _fip, 0, st_print_pm, st_get_pm, st_set_pm, &cs.null, M${n}_POWER_MODE }, + { "${n}","${n}pl", _fip, 3, st_print_pl, st_get_pl, st_set_pl, &st_cfg.mot[MOTOR_${n}].power_level, M${n}_POWER_LEVEL }, + { "${n}","${n}ep", _iip, 0, st_print_ep, st_get_ep, st_set_ep, nullptr, M${n}_ENABLE_POLARITY }, + { "${n}","${n}sp", _iip, 0, st_print_sp, st_get_sp, st_set_sp, nullptr, M${n}_STEP_POLARITY }, +// { "${n}","${n}pi", _fip, 3, st_print_pi, st_get_pi, st_set_pi, &st_cfg.mot[MOTOR_${n}].power_idle, M${n}_POWER_IDLE }, +// { "${n}","${n}mt", _fip, 2, st_print_mt, st_get_mt, st_set_mt, &st_cfg.mot[MOTOR_${n}].motor_timeout, M${n}_MOTOR_TIMEOUT }, #ifdef MOTOR_${n}_IS_TRINAMIC - { "${n}","${n}ts", _f0, 0, tx_print_nul, motor_${n}.get_ts_fn, set_ro, (float *)&motor_${n}, 0 }, - { "${n}","${n}pth", _fip, 0, tx_print_nul, motor_${n}.get_pth_fn, motor_${n}.set_pth_fn, (float *)&motor_${n}, M${n}_TMC2130_TPWMTHRS }, - { "${n}","${n}cth", _fip, 0, tx_print_nul, motor_${n}.get_cth_fn, motor_${n}.set_cth_fn, (float *)&motor_${n}, M${n}_TMC2130_TCOOLTHRS }, - { "${n}","${n}hth", _fip, 0, tx_print_nul, motor_${n}.get_hth_fn, motor_${n}.set_hth_fn, (float *)&motor_${n}, M${n}_TMC2130_THIGH }, - { "${n}","${n}sgt", _fip, 0, tx_print_nul, motor_${n}.get_sgt_fn, motor_${n}.set_sgt_fn, (float *)&motor_${n}, M${n}_TMC2130_SGT }, - { "${n}","${n}sgr", _f0, 0, tx_print_nul, motor_${n}.get_sgr_fn, set_ro, (float *)&motor_${n}, 0 }, - { "${n}","${n}csa", _f0, 0, tx_print_nul, motor_${n}.get_csa_fn, set_ro, (float *)&motor_${n}, 0 }, - { "${n}","${n}sgs", _f0, 0, tx_print_nul, motor_${n}.get_sgs_fn, set_ro, (float *)&motor_${n}, 0 }, - { "${n}","${n}tbl", _fip, 0, tx_print_nul, motor_${n}.get_tbl_fn, motor_${n}.set_tbl_fn, (float *)&motor_${n}, M${n}_TMC2130_TBL }, - { "${n}","${n}pgrd",_fip, 0, tx_print_nul, motor_${n}.get_pgrd_fn,motor_${n}.set_pgrd_fn, (float *)&motor_${n}, M${n}_TMC2130_PWM_GRAD }, - { "${n}","${n}pamp",_fip, 0, tx_print_nul, motor_${n}.get_pamp_fn,motor_${n}.set_pamp_fn, (float *)&motor_${n}, M${n}_TMC2130_PWM_AMPL }, - { "${n}","${n}hend",_fip, 0, tx_print_nul, motor_${n}.get_hend_fn,motor_${n}.set_hend_fn, (float *)&motor_${n}, M${n}_TMC2130_HEND }, - { "${n}","${n}hsrt",_fip, 0, tx_print_nul, motor_${n}.get_hsrt_fn,motor_${n}.set_hsrt_fn, (float *)&motor_${n}, M${n}_TMC2130_HSTRT }, - { "${n}","${n}smin",_fip, 0, tx_print_nul, motor_${n}.get_smin_fn,motor_${n}.set_smin_fn, (float *)&motor_${n}, M${n}_TMC2130_SMIN }, - { "${n}","${n}smax",_fip, 0, tx_print_nul, motor_${n}.get_smax_fn,motor_${n}.set_smax_fn, (float *)&motor_${n}, M${n}_TMC2130_SMAX }, - { "${n}","${n}sup", _fip, 0, tx_print_nul, motor_${n}.get_sup_fn, motor_${n}.set_sup_fn, (float *)&motor_${n}, M${n}_TMC2130_SUP }, - { "${n}","${n}sdn", _fip, 0, tx_print_nul, motor_${n}.get_sdn_fn, motor_${n}.set_sdn_fn, (float *)&motor_${n}, M${n}_TMC2130_SDN }, + { "${n}","${n}ts", _f0, 0, tx_print_nul, motor_${n}.get_ts_fn, set_ro, &motor_${n}, 0 }, + { "${n}","${n}pth", _fip, 0, tx_print_nul, motor_${n}.get_pth_fn, motor_${n}.set_pth_fn, &motor_${n}, M${n}_TMC2130_TPWMTHRS }, + { "${n}","${n}cth", _fip, 0, tx_print_nul, motor_${n}.get_cth_fn, motor_${n}.set_cth_fn, &motor_${n}, M${n}_TMC2130_TCOOLTHRS }, + { "${n}","${n}hth", _fip, 0, tx_print_nul, motor_${n}.get_hth_fn, motor_${n}.set_hth_fn, &motor_${n}, M${n}_TMC2130_THIGH }, + { "${n}","${n}sgt", _fip, 0, tx_print_nul, motor_${n}.get_sgt_fn, motor_${n}.set_sgt_fn, &motor_${n}, M${n}_TMC2130_SGT }, + { "${n}","${n}sgr", _f0, 0, tx_print_nul, motor_${n}.get_sgr_fn, set_ro, &motor_${n}, 0 }, + { "${n}","${n}csa", _f0, 0, tx_print_nul, motor_${n}.get_csa_fn, set_ro, &motor_${n}, 0 }, + { "${n}","${n}sgs", _f0, 0, tx_print_nul, motor_${n}.get_sgs_fn, set_ro, &motor_${n}, 0 }, + { "${n}","${n}tbl", _fip, 0, tx_print_nul, motor_${n}.get_tbl_fn, motor_${n}.set_tbl_fn, &motor_${n}, M${n}_TMC2130_TBL }, + { "${n}","${n}pgrd",_fip, 0, tx_print_nul, motor_${n}.get_pgrd_fn,motor_${n}.set_pgrd_fn, &motor_${n}, M${n}_TMC2130_PWM_GRAD }, + { "${n}","${n}pamp",_fip, 0, tx_print_nul, motor_${n}.get_pamp_fn,motor_${n}.set_pamp_fn, &motor_${n}, M${n}_TMC2130_PWM_AMPL }, + { "${n}","${n}hend",_fip, 0, tx_print_nul, motor_${n}.get_hend_fn,motor_${n}.set_hend_fn, &motor_${n}, M${n}_TMC2130_HEND }, + { "${n}","${n}hsrt",_fip, 0, tx_print_nul, motor_${n}.get_hsrt_fn,motor_${n}.set_hsrt_fn, &motor_${n}, M${n}_TMC2130_HSTRT }, + { "${n}","${n}smin",_fip, 0, tx_print_nul, motor_${n}.get_smin_fn,motor_${n}.set_smin_fn, &motor_${n}, M${n}_TMC2130_SMIN }, + { "${n}","${n}smax",_fip, 0, tx_print_nul, motor_${n}.get_smax_fn,motor_${n}.set_smax_fn, &motor_${n}, M${n}_TMC2130_SMAX }, + { "${n}","${n}sup", _fip, 0, tx_print_nul, motor_${n}.get_sup_fn, motor_${n}.set_sup_fn, &motor_${n}, M${n}_TMC2130_SUP }, + { "${n}","${n}sdn", _fip, 0, tx_print_nul, motor_${n}.get_sdn_fn, motor_${n}.set_sdn_fn, &motor_${n}, M${n}_TMC2130_SDN }, #endif #endif`); } diff --git a/g2core.atsln b/g2core.atsln index bd205453..e1bc137b 100644 --- a/g2core.atsln +++ b/g2core.atsln @@ -21,6 +21,7 @@ Global sbv300|ARM = sbv300|ARM Shapeoko2|ARM = Shapeoko2|ARM shopbotShield|ARM = shopbotShield|ARM + ShopbotTestV9|ARM = ShopbotTestV9|ARM Stub|ARM = Stub|ARM TestQuadratic|ARM = TestQuadratic|ARM TestQuintic|ARM = TestQuintic|ARM @@ -57,6 +58,8 @@ Global {44EA8FEC-55D7-4149-8A78-A574FC26BF51}.Shapeoko2|ARM.Build.0 = Shapeoko2|ARM {44EA8FEC-55D7-4149-8A78-A574FC26BF51}.shopbotShield|ARM.ActiveCfg = shopbotShield|ARM {44EA8FEC-55D7-4149-8A78-A574FC26BF51}.shopbotShield|ARM.Build.0 = shopbotShield|ARM + {44EA8FEC-55D7-4149-8A78-A574FC26BF51}.ShopbotTestV9|ARM.ActiveCfg = G2v9k|ARM + {44EA8FEC-55D7-4149-8A78-A574FC26BF51}.ShopbotTestV9|ARM.Build.0 = G2v9k|ARM {44EA8FEC-55D7-4149-8A78-A574FC26BF51}.Stub|ARM.ActiveCfg = Shapeoko2|ARM {44EA8FEC-55D7-4149-8A78-A574FC26BF51}.TestQuadratic|ARM.ActiveCfg = TestQuadratic|ARM {44EA8FEC-55D7-4149-8A78-A574FC26BF51}.TestQuadratic|ARM.Build.0 = TestQuadratic|ARM @@ -83,6 +86,7 @@ Global {D7779B24-5CD6-4A1B-893C-2CAE8CF7A3A0}.sbv300|ARM.ActiveCfg = Stub|ARM {D7779B24-5CD6-4A1B-893C-2CAE8CF7A3A0}.Shapeoko2|ARM.ActiveCfg = Stub|ARM {D7779B24-5CD6-4A1B-893C-2CAE8CF7A3A0}.shopbotShield|ARM.ActiveCfg = Stub|ARM + {D7779B24-5CD6-4A1B-893C-2CAE8CF7A3A0}.ShopbotTestV9|ARM.ActiveCfg = Stub|ARM {D7779B24-5CD6-4A1B-893C-2CAE8CF7A3A0}.Stub|ARM.ActiveCfg = Stub|ARM {D7779B24-5CD6-4A1B-893C-2CAE8CF7A3A0}.TestQuadratic|ARM.ActiveCfg = Stub|ARM {D7779B24-5CD6-4A1B-893C-2CAE8CF7A3A0}.TestQuintic|ARM.ActiveCfg = Stub|ARM diff --git a/g2core/Makefile b/g2core/Makefile index 0cbd3f26..bc6580d3 100644 --- a/g2core/Makefile +++ b/g2core/Makefile @@ -1,8 +1,8 @@ # # Makefile # -# Copyright (c) 2012 - 2016 Robert Giseburt -# Copyright (c) 2013 - 2016 Alden S. Hart Jr. +# Copyright (c) 2012 - 2018 Robert Giseburt +# Copyright (c) 2013 - 2018 Alden S. Hart Jr. # # This file is part of the g2core project. # @@ -61,6 +61,9 @@ endif ifeq ($(DEBUG),3) DEVICE_DEFINES += DEBUG=1 IN_DEBUGGER=1 DEBUG_SEMIHOSTING=1 endif +#ifeq ($(DEBUG),3) +# DEVICE_DEFINES += DEBUG=1 IN_DEBUGGER=1 DEBUG_SEMIHOSTING=1 +#endif # TOOLS_VERSION = 6.2u1 TOOLS_VERSION = 7u2 diff --git a/g2core/alarm.cpp b/g2core/alarm.cpp new file mode 100644 index 00000000..8c9bd395 --- /dev/null +++ b/g2core/alarm.cpp @@ -0,0 +1,242 @@ +/* + * alarm.cpp - canonical machine alarm handlers + * This file is part of the g2core project + * + * Copyright (c) 2010 - 2018 Alden S Hart, Jr. + * Copyright (c) 2014 - 2018 Robert Giseburt + * + * This file ("the software") is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License, version 2 as published by the + * Free Software Foundation. You should have received a copy of the GNU General Public + * License, version 2 along with the software. If not, see . + * + * As a special exception, you may use this file as part of a software library without + * restriction. Specifically, if other files instantiate templates or use macros or + * inline functions from this file, or you compile this file and link it with other + * files to produce an executable, this file does not by itself cause the resulting + * executable to be covered by the GNU General Public License. This exception does not + * however invalidate any other reasons why the executable file might be covered by the + * GNU General Public License. + * + * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT WITHOUT ANY + * WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES + * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT + * SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF + * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + */ + +#include "g2core.h" // #1 +#include "config.h" // #2 +#include "gcode.h" // #3 +#include "canonical_machine.h" +#include "planner.h" +#include "report.h" +#include "spindle.h" +#include "coolant.h" +#include "temperature.h" +#include "util.h" + +/**************************************************************************************** + * ALARM, SHUTDOWN, and PANIC are nested dolls. + * + * cm_alrm() - invoke alarm from command + * cm_shutd() - invoke shutdown from command + * cm_pnic() - invoke panic from command + * cm_clr() - clear alarm or shutdown from command + * + * The alarm states can be invoked from the above commands for testing and clearing + */ +stat_t cm_alrm(nvObj_t *nv) // invoke alarm from command +{ + cm_alarm(STAT_ALARM, "sent by host"); + return (STAT_OK); +} + +stat_t cm_shutd(nvObj_t *nv) // invoke shutdown from command +{ + cm_shutdown(STAT_SHUTDOWN, "sent by host"); + return (STAT_OK); +} + +stat_t cm_pnic(nvObj_t *nv) // invoke panic from command +{ + cm_panic(STAT_PANIC, "sent by host"); + return (STAT_OK); +} + +stat_t cm_clr(nvObj_t *nv) // clear alarm or shutdown from command line +{ + cm_clear(); + return (STAT_OK); +} + +/**************************************************************************************** + * cm_clear() - clear ALARM and SHUTDOWN states + * cm_parse_clear() - parse incoming gcode for M30 or M2 clears if in ALARM state + * + * Parse clear interprets an M30 or M2 PROGRAM_END as a $clear condition and clear ALARM + * but not SHUTDOWN or PANIC. Assumes Gcode string has no leading or embedded whitespace + */ + +void cm_clear() +{ + if (cm->machine_state == MACHINE_ALARM) { + cm->machine_state = MACHINE_PROGRAM_STOP; + } else if (cm->machine_state == MACHINE_SHUTDOWN) { + cm->machine_state = MACHINE_READY; + } +} + +void cm_parse_clear(const char *s) +{ + if (cm->machine_state == MACHINE_ALARM) { + if (toupper(s[0]) == 'M') { + if (( (s[1]=='3') && (s[2]=='0') && (s[3]==0)) || ((s[1]=='2') && (s[2]==0) )) { + cm_clear(); + } + } + } +} + +/**************************************************************************************** + * cm_is_alarmed() - return alarm status code or OK if no alarms + */ + +stat_t cm_is_alarmed() +{ + if (cm->machine_state == MACHINE_ALARM) { return (STAT_COMMAND_REJECTED_BY_ALARM); } + if (cm->machine_state == MACHINE_SHUTDOWN) { return (STAT_COMMAND_REJECTED_BY_SHUTDOWN); } + if (cm->machine_state == MACHINE_PANIC) { return (STAT_COMMAND_REJECTED_BY_PANIC); } + return (STAT_OK); +} + +/**************************************************************************************** + * cm_halt() - stop motion, spindle, coolant and heaters immediately + * cm_halt_motion() - stop motion immediately. Does not affect spindle, coolant, or other IO + * + * Stop motors and reset all system states accordingly. + * Does not de-energize motors as in some cases the motors must remain energized + * in order to prevent an axis from crashing. + */ + +void cm_halt(void) +{ + cm_halt_motion(); + spindle_control_immediate(SPINDLE_OFF); + coolant_control_immediate(COOLANT_OFF, COOLANT_BOTH); + temperature_init(); +} + +void cm_halt_motion(void) +{ + mp_halt_runtime(); // stop the runtime. Do this immediately. (Reset is in cm_clear) + canonical_machine_reset(cm); // halt the currently active machine + cm->cycle_type = CYCLE_NONE; // Note: leaves machine_state alone + cm->motion_state = MOTION_STOP; + cm->hold_state = FEEDHOLD_OFF; +} + +/**************************************************************************************** + * cm_alarm() - enter ALARM state + * + * An ALARM sets the ALARM machine state, starts a feedhold to stop motion, stops the + * spindle, turns off coolant, clears out queued planner moves and serial input, + * and rejects new action commands (gcode blocks, SET commands, and other actions) + * until the alarm is cleared. + * + * ALARM is typically entered by a soft limit or a limit switch being hit. In the + * limit switch case the INPUT_ACTION will override the feedhold - i.e. if the + * input action is "FAST_STOP" or "HALT" that setting will take precedence over + * the feedhold native to the alarm function. + * + * Gcode and machine state is preserved. It may be possible to recover the job from + * an alarm, but in many cases this is not possible. Since ALARM attempts to preserve + * Gcode and machine state it does not END the job. + * + * ALARM may also be invoked from the command line using {alarm:n} or $alarm + * ALARM can be manually cleared by entering: {clear:n}, {clr:n}, $clear, or $clr + * ALARMs will also clear on receipt of an M30 or M2 command if one is received + * while draining the host command queue. + */ + +stat_t cm_alarm(const stat_t status, const char *msg) +{ + if ((cm->machine_state == MACHINE_ALARM) || (cm->machine_state == MACHINE_SHUTDOWN) || + (cm->machine_state == MACHINE_PANIC)) { + return (STAT_OK); // don't alarm if already in an alarm state + } + cm_request_feedhold(FEEDHOLD_TYPE_SCRAM, FEEDHOLD_EXIT_ALARM); // fast stop and alarm + rpt_exception(status, msg); // send alarm message + sr_request_status_report(SR_REQUEST_TIMED); + return (status); +} + +/**************************************************************************************** + * cm_shutdown() - enter shutdown state + * + * SHUTDOWN stops all motion, spindle and coolant immediately, sets a SHUTDOWN machine + * state, clears out queued moves and serial input, and rejects new action commands + * (gcode blocks, SET commands, and some others). + * + * Shutdown is typically invoked as an electrical input signal sent to the board as + * part of an external emergency stop (Estop). Shutdown is meant to augment but not + * replace the external Estop functions that shut down power to motors, spindles and + * other moving parts. + * + * Shutdown may also be invoked from the command line using {shutd:n} or $shutd + * Shutdown must be manually cleared by entering: {clear:n}, {clr:n}, $clear, or $clr + * Shutdown does not clear on M30 or M2 Gcode commands + */ + +stat_t cm_shutdown(const stat_t status, const char *msg) +{ + if ((cm->machine_state == MACHINE_SHUTDOWN) || (cm->machine_state == MACHINE_PANIC)) { + return (STAT_OK); // don't shutdown if shutdown or panic'd + } + cm_request_feedhold(FEEDHOLD_TYPE_SCRAM, FEEDHOLD_EXIT_SHUTDOWN); // fast stop and shutdown + +// spindle_reset(); // stop spindle immediately and set speed to 0 RPM +// coolant_reset(); // stop coolant immediately +// temperature_reset(); // turn off heaters and fans +// cm_queue_flush(&cm1); // flush all queues and reset positions + + for (uint8_t i = 0; i < HOMING_AXES; i++) { // unhome axes and the machine + cm->homed[i] = false; + } + cm->homing_state = HOMING_NOT_HOMED; + +// cm1.machine_state = MACHINE_SHUTDOWN; // shut down both machines... +// cm2.machine_state = MACHINE_SHUTDOWN; //...do this after all other activity + rpt_exception(status, msg); // send exception report + sr_request_status_report(SR_REQUEST_TIMED); + return (status); +} + +/**************************************************************************************** + * cm_panic() - enter panic state + * + * PANIC occurs if the firmware has detected an unrecoverable internal error + * such as an assertion failure or a code condition that should never occur. + * It sets PANIC machine state, and leaves the system inspect able (if possible). + * + * PANIC can only be exited by a hardware reset or soft reset (^x) + */ + +stat_t cm_panic(const stat_t status, const char *msg) +{ + debug_trap(msg); + + if (cm->machine_state == MACHINE_PANIC) { // only do this once + return (STAT_OK); + } + cm_halt_motion(); // halt motors (may have already been done from GPIO) + spindle_reset(); // stop spindle immediately and set speed to 0 RPM + coolant_reset(); // stop coolant immediately + temperature_reset(); // turn off heaters and fans + + cm1.machine_state = MACHINE_PANIC; // don't reset anything. Panics are not recoverable + cm2.machine_state = MACHINE_PANIC; // don't reset anything. Panics are not recoverable + rpt_exception(status, msg); // send panic report + return (status); +} diff --git a/g2core/board/Archim/0_hardware.cpp b/g2core/board/Archim/0_hardware.cpp old mode 100755 new mode 100644 index 55d553ef..80539633 --- a/g2core/board/Archim/0_hardware.cpp +++ b/g2core/board/Archim/0_hardware.cpp @@ -1,5 +1,6 @@ /* * hardware.cpp - general hardware support functions + * For: /board/Archim * This file is part of the g2core project * * Copyright (c) 2010 - 2015 Alden S. Hart, Jr. @@ -93,15 +94,18 @@ void _get_id(char *id) ***********************************************************************************/ /* + * hw_get_fb() - get firmware build number + * hw_get_fv() - get firmware version number + * hw_get_hp() - get hardware platform string + * hw_get_hv() - get hardware version string * hw_get_fbs() - get firmware build string */ -stat_t hw_get_fbs(nvObj_t *nv) -{ - nv->valuetype = TYPE_STRING; - ritorno(nv_copy_string(nv, G2CORE_FIRMWARE_BUILD_STRING)); - return (STAT_OK); -} +stat_t hw_get_fb(nvObj_t *nv) { return (get_float(nv, cs.fw_build)); } +stat_t hw_get_fv(nvObj_t *nv) { return (get_float(nv, cs.fw_version)); } +stat_t hw_get_hp(nvObj_t *nv) { return (get_string(nv, G2CORE_HARDWARE_PLATFORM)); } +stat_t hw_get_hv(nvObj_t *nv) { return (get_string(nv, G2CORE_HARDWARE_VERSION)); } +stat_t hw_get_fbs(nvObj_t *nv) { return (get_string(nv, G2CORE_FIRMWARE_BUILD_STRING)); } /* * hw_get_fbc() - get configuration settings file @@ -145,15 +149,6 @@ stat_t hw_flash(nvObj_t *nv) return(STAT_OK); } -/* - * hw_set_hv() - set hardware version number - */ -stat_t hw_set_hv(nvObj_t *nv) -{ - return (STAT_OK); -} - - /*********************************************************************************** * TEXT MODE SUPPORT * Functions to print variables from the cfgArray table @@ -161,22 +156,20 @@ stat_t hw_set_hv(nvObj_t *nv) #ifdef __TEXT_MODE -static const char fmt_fb[] = "[fb] firmware build %18.2f\n"; -static const char fmt_fbs[] = "[fbs] firmware build \"%s\"\n"; -static const char fmt_fbc[] = "[fbc] firmware config \"%s\"\n"; -static const char fmt_fv[] = "[fv] firmware version%16.2f\n"; -static const char fmt_cv[] = "[cv] configuration version%11.2f\n"; -static const char fmt_hp[] = "[hp] hardware platform%15.2f\n"; -static const char fmt_hv[] = "[hv] hardware version%16.2f\n"; -static const char fmt_id[] = "[id] g2core ID%21s\n"; + static const char fmt_fb[] = "[fb] firmware build%18.2f\n"; + static const char fmt_fv[] = "[fv] firmware version%16.2f\n"; + static const char fmt_fbs[] = "[fbs] firmware build%34s\n"; + static const char fmt_fbc[] = "[fbc] firmware config%33s\n"; + static const char fmt_hp[] = "[hp] hardware platform%15s\n"; + static const char fmt_hv[] = "[hv] hardware version%13s\n"; + static const char fmt_id[] = "[id] g2core ID%37s\n"; -void hw_print_fb(nvObj_t *nv) { text_print(nv, fmt_fb);} // TYPE_FLOAT -void hw_print_fbs(nvObj_t *nv) { text_print(nv, fmt_fbs);} // TYPE_STRING -void hw_print_fbc(nvObj_t *nv) { text_print(nv, fmt_fbc);} // TYPE_STRING -void hw_print_fv(nvObj_t *nv) { text_print(nv, fmt_fv);} // TYPE_FLOAT -void hw_print_cv(nvObj_t *nv) { text_print(nv, fmt_cv);} // TYPE_FLOAT -void hw_print_hp(nvObj_t *nv) { text_print(nv, fmt_hp);} // TYPE_FLOAT -void hw_print_hv(nvObj_t *nv) { text_print(nv, fmt_hv);} // TYPE_FLOAT -void hw_print_id(nvObj_t *nv) { text_print(nv, fmt_id);} // TYPE_STRING + void hw_print_fb(nvObj_t *nv) { text_print(nv, fmt_fb);} // TYPE_FLOAT + void hw_print_fv(nvObj_t *nv) { text_print(nv, fmt_fv);} // TYPE_FLOAT + void hw_print_fbs(nvObj_t *nv) { text_print(nv, fmt_fbs);} // TYPE_STRING + void hw_print_fbc(nvObj_t *nv) { text_print(nv, fmt_fbc);} // TYPE_STRING + void hw_print_hp(nvObj_t *nv) { text_print(nv, fmt_hp);} // TYPE_STRING + void hw_print_hv(nvObj_t *nv) { text_print(nv, fmt_hv);} // TYPE_STRING + void hw_print_id(nvObj_t *nv) { text_print(nv, fmt_id);} // TYPE_STRING #endif //__TEXT_MODE diff --git a/g2core/board/Archim/Archim-pinout.h b/g2core/board/Archim/Archim-pinout.h old mode 100755 new mode 100644 index 77f61c23..90e14ed9 --- a/g2core/board/Archim/Archim-pinout.h +++ b/g2core/board/Archim/Archim-pinout.h @@ -1,9 +1,10 @@ /* - * motate_pin_assignments.h - pin assignments for the Archim boards + * motate_pin_assignments.h - pin assignments + * For: /board/Archim * This file is part of the g2core project * - * Copyright (c) 2013 - 2016 Robert Giseburt - * Copyright (c) 2013 - 2016 Alden S. Hart Jr. + * Copyright (c) 2013 - 2018 Robert Giseburt + * Copyright (c) 2013 - 2018 Alden S. Hart Jr. * * This file is part of the Motate Library. * diff --git a/g2core/board/Archim/board_stepper.cpp b/g2core/board/Archim/board_stepper.cpp old mode 100755 new mode 100644 index b4c254c5..79215f94 --- a/g2core/board/Archim/board_stepper.cpp +++ b/g2core/board/Archim/board_stepper.cpp @@ -1,9 +1,10 @@ /* * board_stepper.cpp - board-specific code for stepper.cpp + * For: /board/Archim * This file is part of g2core project * - * Copyright (c) 2016 Alden S. Hart, Jr. - * Copyright (c) 2016 Robert Giseburt + * Copyright (c) 2016 - 2018 Alden S. Hart, Jr. + * Copyright (c) 2016 - 2018 Robert Giseburt * * This file ("the software") is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License, version 2 as published by the @@ -36,7 +37,7 @@ StepDirStepper - motor_1{}; + motor_1{M1_STEP_POLARITY, M1_ENABLE_POLARITY}; StepDirStepper - motor_2{}; + motor_2{M2_STEP_POLARITY, M2_ENABLE_POLARITY}; StepDirStepper - motor_3{}; + motor_3{M3_STEP_POLARITY, M3_ENABLE_POLARITY}; StepDirStepper - motor_4{}; + motor_4{M4_STEP_POLARITY, M4_ENABLE_POLARITY}; // StepDirStepper< // Motate::kSocket5_StepPinNumber, @@ -72,7 +73,8 @@ StepDirStepper motor_5 {}; +// Motate::kSocket5_VrefPinNumber> +// motor_5 {M5_STEP_POLARITY, M5_ENABLE_POLARITY}; // StepDirStepper< // Motate::kSocket6_StepPinNumber, @@ -81,7 +83,9 @@ StepDirStepper motor_6 {}; +// Motate::kSocket6_VrefPinNumber> +// motor_6 {M6_STEP_POLARITY, M6_ENABLE_POLARITY}; + Stepper* Motors[MOTORS] = {&motor_1, &motor_2, &motor_3, &motor_4}; diff --git a/g2core/board/Archim/board_stepper.h b/g2core/board/Archim/board_stepper.h old mode 100755 new mode 100644 index d59d6a2c..9291e3be --- a/g2core/board/Archim/board_stepper.h +++ b/g2core/board/Archim/board_stepper.h @@ -1,9 +1,10 @@ /* * board_stepper.h - board-specific code for stepper.h + * For: /board/Archim * This file is part of g2core project * - * Copyright (c) 2016 Alden S. Hart, Jr. - * Copyright (c) 2016 Robert Giseburt + * Copyright (c) 2016 - 2018 Alden S. Hart, Jr. + * Copyright (c) 2016 - 2018 Robert Giseburt * * This file ("the software") is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License, version 2 as published by the @@ -74,7 +75,8 @@ extern StepDirStepper motor_5; +// Motate::kSocket5_VrefPinNumber> +// motor_5; // extern StepDirStepper< // Motate::kSocket6_StepPinNumber, @@ -83,7 +85,8 @@ extern StepDirStepper motor_6 {}; +// Motate::kSocket6_VrefPinNumber> +// motor_6; extern Stepper* Motors[MOTORS]; diff --git a/g2core/board/Archim/board_xio.cpp b/g2core/board/Archim/board_xio.cpp index ca077c05..82762239 100644 --- a/g2core/board/Archim/board_xio.cpp +++ b/g2core/board/Archim/board_xio.cpp @@ -1,5 +1,6 @@ /* * board_xio.cpp - extended IO functions that are board-specific + * For: /board/Archim * This file is part of the g2core project * * Copyright (c) 2016 Alden S. Hart Jr. diff --git a/g2core/board/Archim/board_xio.h b/g2core/board/Archim/board_xio.h old mode 100755 new mode 100644 index ea0a3fa7..924aa665 --- a/g2core/board/Archim/board_xio.h +++ b/g2core/board/Archim/board_xio.h @@ -1,5 +1,6 @@ /* * board_xio.h - extended IO functions that are board-specific + * For: /board/Archim * This file is part of the g2core project * * Copyright (c) 2016 Alden S. Hart Jr. diff --git a/g2core/board/Archim/hardware.h b/g2core/board/Archim/hardware.h index 928d20da..87875848 100644 --- a/g2core/board/Archim/hardware.h +++ b/g2core/board/Archim/hardware.h @@ -1,11 +1,12 @@ /* * hardware.h - system hardware configuration - * THIS FILE IS HARDWARE PLATFORM SPECIFIC - ARM version + * For: /board/Archim + * THIS FILE IS HARDWARE PLATFORM SPECIFIC - ARM version * * This file is part of the g2core project * - * Copyright (c) 2013 - 2016 Alden S. Hart, Jr. - * Copyright (c) 2013 - 2016 Robert Giseburt + * Copyright (c) 2013 - 2018 Alden S. Hart, Jr. + * Copyright (c) 2013 - 2018 Robert Giseburt * * This file ("the software") is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License, version 2 as published by the @@ -36,40 +37,31 @@ /*--- Hardware platform enumerations ---*/ -enum hwPlatform { - HM_PLATFORM_NONE = 0, - HW_PLATFORM_TINYG_XMEGA, // TinyG code base on Xmega boards. - HW_PLATFORM_G2_DUE, // G2 code base on native Arduino Due - HW_PLATFORM_V9 // G2 code base on v9 boards -}; +#define G2CORE_HARDWARE_PLATFORM "Archim" +#define G2CORE_HARDWARE_VERSION "na" -#define HW_VERSION_TINYGV6 6 -#define HW_VERSION_TINYGV7 7 -#define HW_VERSION_TINYGV8 8 - -#define HW_VERSION_TINYGV9I 4 -#define HW_VERSION_TINYGV9K 5 - - -/***** Axes, motors & PWM channels used by the application *****/ -// Axes, motors & PWM channels must be defines (not enums) so expressions like this: +/***** Motors & PWM channels supported by this hardware *****/ +// These must be defines (not enums) so expressions like this: // #if (MOTORS >= 6) will work -#define AXES 6 // number of axes supported in this version -#define HOMING_AXES 4 // number of axes that can be homed (assumes Zxyabc sequence) -#define MOTORS 4 // number of motors on the board -#define COORDS 6 // number of supported coordinate systems (index starts at 1) -#define PWMS 2 // number of supported PWM channels -#define TOOLS 32 // number of entries in tool table (index starts at 1) +#define MOTORS 4 // number of motors supported the hardware +#define PWMS 2 // number of PWM channels supported the hardware +/************************* + * Global System Defines * + *************************/ -//////////////////////////// -/////// ARM VERSION //////// -//////////////////////////// +#define MILLISECONDS_PER_TICK 1 // MS for system tick (systick * N) +#define SYS_ID_DIGITS 16 // actual digits in system ID (up to 16) +#define SYS_ID_LEN 24 // total length including dashes and NUL + +/************************* + * Motate Setup * + *************************/ #include "MotatePins.h" -#include "MotateTimers.h" // for TimerChanel<> and related... -#include "MotateServiceCall.h" // for ServiceCall<> +#include "MotateTimers.h" // for TimerChanel<> and related... +#include "MotateServiceCall.h" // for ServiceCall<> using Motate::TimerChannel; using Motate::ServiceCall; @@ -79,14 +71,6 @@ using Motate::Pin; using Motate::PWMOutputPin; using Motate::OutputPin; -/************************* - * Global System Defines * - *************************/ - -#define MILLISECONDS_PER_TICK 1 // MS for system tick (systick * N) -#define SYS_ID_DIGITS 12 // actual digits in system ID (up to 16) -#define SYS_ID_LEN 24 // total length including dashes and NUL - /************************************************************************************ **** ARM SAM3X8E SPECIFIC HARDWARE ************************************************* ************************************************************************************/ @@ -174,32 +158,33 @@ stat_t hardware_periodic(); // callback from the main loop (time sensitive) void hw_hard_reset(void); stat_t hw_flash(nvObj_t* nv); -stat_t hw_get_fbs(nvObj_t* nv); -stat_t hw_get_fbc(nvObj_t* nv); -stat_t hw_set_hv(nvObj_t* nv); -stat_t hw_get_id(nvObj_t* nv); +stat_t hw_get_fb(nvObj_t *nv); +stat_t hw_get_fv(nvObj_t *nv); +stat_t hw_get_hp(nvObj_t *nv); +stat_t hw_get_hv(nvObj_t *nv); +stat_t hw_get_fbs(nvObj_t *nv); +stat_t hw_get_fbc(nvObj_t *nv); +stat_t hw_get_id(nvObj_t *nv); #ifdef __TEXT_MODE -void hw_print_fb(nvObj_t* nv); -void hw_print_fbs(nvObj_t* nv); -void hw_print_fbc(nvObj_t* nv); -void hw_print_fv(nvObj_t* nv); -void hw_print_cv(nvObj_t* nv); -void hw_print_hp(nvObj_t* nv); -void hw_print_hv(nvObj_t* nv); -void hw_print_id(nvObj_t* nv); + void hw_print_fb(nvObj_t *nv); + void hw_print_fv(nvObj_t *nv); + void hw_print_fbs(nvObj_t *nv); + void hw_print_fbc(nvObj_t *nv); + void hw_print_hp(nvObj_t *nv); + void hw_print_hv(nvObj_t *nv); + void hw_print_id(nvObj_t *nv); #else -#define hw_print_fb tx_print_stub -#define hw_print_fbs tx_print_stub -#define hw_print_fbc tx_print_stub -#define hw_print_fv tx_print_stub -#define hw_print_cv tx_print_stub -#define hw_print_hp tx_print_stub -#define hw_print_hv tx_print_stub -#define hw_print_id tx_print_stub + #define hw_print_fb tx_print_stub + #define hw_print_fv tx_print_stub + #define hw_print_fbs tx_print_stub + #define hw_print_fbc tx_print_stub + #define hw_print_hp tx_print_stub + #define hw_print_hv tx_print_stub + #define hw_print_id tx_print_stub #endif // __TEXT_MODE diff --git a/g2core/board/Archim/motate_pin_assignments.h b/g2core/board/Archim/motate_pin_assignments.h old mode 100755 new mode 100644 index 01226ae7..a4562464 --- a/g2core/board/Archim/motate_pin_assignments.h +++ b/g2core/board/Archim/motate_pin_assignments.h @@ -1,5 +1,6 @@ /* - * motate_pin_assignments.h - pin assignments for the g2core Archim boards + * motate_pin_assignments.h + * For: /board/Archim * This file is part of the g2core project * * Copyright (c) 2013-2016 Robert Giseburt diff --git a/g2core/board/ArduinoDue/0_hardware.cpp b/g2core/board/ArduinoDue/0_hardware.cpp old mode 100755 new mode 100644 index e4886882..3263acee --- a/g2core/board/ArduinoDue/0_hardware.cpp +++ b/g2core/board/ArduinoDue/0_hardware.cpp @@ -1,9 +1,10 @@ /* * hardware.cpp - general hardware support functions + * For: /board/g2v9 * This file is part of the g2core project * - * Copyright (c) 2010 - 2016 Alden S. Hart, Jr. - * Copyright (c) 2013 - 2016 Robert Giseburt + * Copyright (c) 2010 - 2018 Alden S. Hart, Jr. + * Copyright (c) 2013 - 2018 Robert Giseburt * * This file ("the software") is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License, version 2 as published by the @@ -95,15 +96,18 @@ void _get_id(char *id) ***********************************************************************************/ /* + * hw_get_fb() - get firmware build number + * hw_get_fv() - get firmware version number + * hw_get_hp() - get hardware platform string + * hw_get_hv() - get hardware version string * hw_get_fbs() - get firmware build string */ -stat_t hw_get_fbs(nvObj_t *nv) -{ - nv->valuetype = TYPE_STRING; - ritorno(nv_copy_string(nv, G2CORE_FIRMWARE_BUILD_STRING)); - return (STAT_OK); -} +stat_t hw_get_fb(nvObj_t *nv) { return (get_float(nv, cs.fw_build)); } +stat_t hw_get_fv(nvObj_t *nv) { return (get_float(nv, cs.fw_version)); } +stat_t hw_get_hp(nvObj_t *nv) { return (get_string(nv, G2CORE_HARDWARE_PLATFORM)); } +stat_t hw_get_hv(nvObj_t *nv) { return (get_string(nv, G2CORE_HARDWARE_VERSION)); } +stat_t hw_get_fbs(nvObj_t *nv) { return (get_string(nv, G2CORE_FIRMWARE_BUILD_STRING)); } /* * hw_get_fbc() - get configuration settings file @@ -112,15 +116,15 @@ stat_t hw_get_fbs(nvObj_t *nv) stat_t hw_get_fbc(nvObj_t *nv) { nv->valuetype = TYPE_STRING; - #ifdef SETTINGS_FILE - #define settings_file_string1(s) #s - #define settings_file_string2(s) settings_file_string1(s) +#ifdef SETTINGS_FILE +#define settings_file_string1(s) #s +#define settings_file_string2(s) settings_file_string1(s) ritorno(nv_copy_string(nv, settings_file_string2(SETTINGS_FILE))); - #undef settings_file_string1 - #undef settings_file_string2 - #else +#undef settings_file_string1 +#undef settings_file_string2 +#else ritorno(nv_copy_string(nv, "")); - #endif + #endif return (STAT_OK); } @@ -147,15 +151,6 @@ stat_t hw_flash(nvObj_t *nv) return(STAT_OK); } -/* - * hw_set_hv() - set hardware version number - */ -stat_t hw_set_hv(nvObj_t *nv) -{ - return (STAT_OK); -} - - /*********************************************************************************** * TEXT MODE SUPPORT * Functions to print variables from the cfgArray table @@ -163,22 +158,20 @@ stat_t hw_set_hv(nvObj_t *nv) #ifdef __TEXT_MODE -static const char fmt_fb[] = "[fb] firmware build %18.2f\n"; -static const char fmt_fbs[] = "[fbs] firmware build \"%s\"\n"; -static const char fmt_fbc[] = "[fbc] firmware config \"%s\"\n"; -static const char fmt_fv[] = "[fv] firmware version%16.2f\n"; -static const char fmt_cv[] = "[cv] configuration version%11.2f\n"; -static const char fmt_hp[] = "[hp] hardware platform%15.2f\n"; -static const char fmt_hv[] = "[hv] hardware version%16.2f\n"; -static const char fmt_id[] = "[id] g2core ID%21s\n"; + static const char fmt_fb[] = "[fb] firmware build%18.2f\n"; + static const char fmt_fv[] = "[fv] firmware version%16.2f\n"; + static const char fmt_fbs[] = "[fbs] firmware build%34s\n"; + static const char fmt_fbc[] = "[fbc] firmware config%33s\n"; + static const char fmt_hp[] = "[hp] hardware platform%15s\n"; + static const char fmt_hv[] = "[hv] hardware version%13s\n"; + static const char fmt_id[] = "[id] g2core ID%37s\n"; -void hw_print_fb(nvObj_t *nv) { text_print(nv, fmt_fb);} // TYPE_FLOAT -void hw_print_fbs(nvObj_t *nv) { text_print(nv, fmt_fbs);} // TYPE_STRING -void hw_print_fbc(nvObj_t *nv) { text_print(nv, fmt_fbc);} // TYPE_STRING -void hw_print_fv(nvObj_t *nv) { text_print(nv, fmt_fv);} // TYPE_FLOAT -void hw_print_cv(nvObj_t *nv) { text_print(nv, fmt_cv);} // TYPE_FLOAT -void hw_print_hp(nvObj_t *nv) { text_print(nv, fmt_hp);} // TYPE_FLOAT -void hw_print_hv(nvObj_t *nv) { text_print(nv, fmt_hv);} // TYPE_FLOAT -void hw_print_id(nvObj_t *nv) { text_print(nv, fmt_id);} // TYPE_STRING + void hw_print_fb(nvObj_t *nv) { text_print(nv, fmt_fb);} // TYPE_FLOAT + void hw_print_fv(nvObj_t *nv) { text_print(nv, fmt_fv);} // TYPE_FLOAT + void hw_print_fbs(nvObj_t *nv) { text_print(nv, fmt_fbs);} // TYPE_STRING + void hw_print_fbc(nvObj_t *nv) { text_print(nv, fmt_fbc);} // TYPE_STRING + void hw_print_hp(nvObj_t *nv) { text_print(nv, fmt_hp);} // TYPE_STRING + void hw_print_hv(nvObj_t *nv) { text_print(nv, fmt_hv);} // TYPE_STRING + void hw_print_id(nvObj_t *nv) { text_print(nv, fmt_id);} // TYPE_STRING #endif //__TEXT_MODE diff --git a/g2core/board/ArduinoDue/board_stepper.cpp b/g2core/board/ArduinoDue/board_stepper.cpp old mode 100755 new mode 100644 index 0583e4e7..c53539da --- a/g2core/board/ArduinoDue/board_stepper.cpp +++ b/g2core/board/ArduinoDue/board_stepper.cpp @@ -1,9 +1,10 @@ /* * board_stepper.cpp - board-specific code for stepper.cpp + * For: /board/ArduinoDue * This file is part of the g2core project * - * Copyright (c) 2016 Alden S. Hart, Jr. - * Copyright (c) 2016 Robert Giseburt + * Copyright (c) 2016 - 2018 Alden S. Hart, Jr. + * Copyright (c) 2016 - 2018 Robert Giseburt * * This file ("the software") is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License, version 2 as published by the @@ -28,7 +29,7 @@ #include "board_stepper.h" -// These are identical to board_stepper.h, except for the word "extern" +// These are identical to board_stepper.h, except for the word "extern", and they have the initialization parameters StepDirStepper - motor_1{}; + motor_1{M1_STEP_POLARITY, M1_ENABLE_POLARITY}; StepDirStepper - motor_2{}; + motor_2{M2_STEP_POLARITY, M2_ENABLE_POLARITY}; StepDirStepper - motor_3{}; + motor_3{M3_STEP_POLARITY, M3_ENABLE_POLARITY}; StepDirStepper - motor_4{}; + motor_4{M4_STEP_POLARITY, M4_ENABLE_POLARITY}; // StepDirStepper< // Motate::kSocket5_StepPinNumber, @@ -72,7 +73,8 @@ StepDirStepper motor_5 {}; +// Motate::kSocket5_VrefPinNumber> +// motor_5 {M5_STEP_POLARITY, M5_ENABLE_POLARITY}; // StepDirStepper< // Motate::kSocket6_StepPinNumber, @@ -81,7 +83,8 @@ StepDirStepper motor_6 {}; +// Motate::kSocket6_VrefPinNumber> +// motor_6 {M6_STEP_POLARITY, M6_ENABLE_POLARITY}; Stepper* Motors[MOTORS] = {&motor_1, &motor_2, &motor_3, &motor_4}; diff --git a/g2core/board/ArduinoDue/board_stepper.h b/g2core/board/ArduinoDue/board_stepper.h old mode 100755 new mode 100644 index 25c5d01b..cad66950 --- a/g2core/board/ArduinoDue/board_stepper.h +++ b/g2core/board/ArduinoDue/board_stepper.h @@ -1,9 +1,10 @@ /* * board_stepper.h - board-specific code for stepper.h + * For: /board/ArduinoDue * This file is part of the g2core project * - * Copyright (c) 2016 Alden S. Hart, Jr. - * Copyright (c) 2016 Robert Giseburt + * Copyright (c) 2016 - 2018 Alden S. Hart, Jr. + * Copyright (c) 2016 - 2018 Robert Giseburt * * This file ("the software") is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License, version 2 as published by the diff --git a/g2core/board/ArduinoDue/board_xio.cpp b/g2core/board/ArduinoDue/board_xio.cpp old mode 100755 new mode 100644 index 15639e6a..c056fa13 --- a/g2core/board/ArduinoDue/board_xio.cpp +++ b/g2core/board/ArduinoDue/board_xio.cpp @@ -1,5 +1,6 @@ /* * board_xio.cpp - extended IO functions that are board-specific + * For: /board/ArduinoDue * This file is part of the g2core project * * Copyright (c) 2016 Alden S. Hart Jr. diff --git a/g2core/board/ArduinoDue/board_xio.h b/g2core/board/ArduinoDue/board_xio.h old mode 100755 new mode 100644 index ea0a3fa7..5ab8cee5 --- a/g2core/board/ArduinoDue/board_xio.h +++ b/g2core/board/ArduinoDue/board_xio.h @@ -1,5 +1,6 @@ /* * board_xio.h - extended IO functions that are board-specific + * For: /board/ArduinoDue * This file is part of the g2core project * * Copyright (c) 2016 Alden S. Hart Jr. diff --git a/g2core/board/ArduinoDue/gShield-pinout.h b/g2core/board/ArduinoDue/gShield-pinout.h index 0f82c435..9440514a 100644 --- a/g2core/board/ArduinoDue/gShield-pinout.h +++ b/g2core/board/ArduinoDue/gShield-pinout.h @@ -1,9 +1,10 @@ /* - * motate_pin_assignments.h - pin assignments for g2 v9 boards + * motate_pin_assignments.h - pin assignments + * For: /board/ArduinoDue * This file is part of the g2core project * - * Copyright (c) 2013 - 2016 Robert Giseburt - * Copyright (c) 2013 - 2016 Alden S. Hart Jr. + * Copyright (c) 2013 - 2018 Robert Giseburt + * Copyright (c) 2013 - 2018 Alden S. Hart Jr. * * This file is part of the Motate Library. * @@ -267,4 +268,4 @@ pin_number kGRBL_CommonEnablePinNumber = 8; } // namespace Motate -#endif \ No newline at end of file +#endif diff --git a/g2core/board/ArduinoDue/hardware.h b/g2core/board/ArduinoDue/hardware.h index 79a1ad29..689e32ed 100644 --- a/g2core/board/ArduinoDue/hardware.h +++ b/g2core/board/ArduinoDue/hardware.h @@ -1,11 +1,12 @@ /* * hardware.h - system hardware configuration - * THIS FILE IS HARDWARE PLATFORM SPECIFIC - ARM version + * For: /board/ArduinoDue + * THIS FILE IS HARDWARE PLATFORM SPECIFIC - ARM version * * This file is part of the g2core project * - * Copyright (c) 2013 - 2016 Alden S. Hart, Jr. - * Copyright (c) 2013 - 2016 Robert Giseburt + * Copyright (c) 2013 - 2018 Alden S. Hart, Jr. + * Copyright (c) 2013 - 2018 Robert Giseburt * * This file ("the software") is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License, version 2 as published by the @@ -36,42 +37,31 @@ /*--- Hardware platform enumerations ---*/ -enum hwPlatform { - HM_PLATFORM_NONE = 0, - HW_PLATFORM_TINYG_XMEGA, // TinyG code base on Xmega boards. - HW_PLATFORM_G2_DUE, // G2 code base on native Arduino Due - HW_PLATFORM_V9 // G2 code base on v9 boards -}; +#define G2CORE_HARDWARE_PLATFORM "ArduinoDue" +#define G2CORE_HARDWARE_VERSION "na" -#define HW_VERSION_TINYGV6 6 -#define HW_VERSION_TINYGV7 7 -#define HW_VERSION_TINYGV8 8 - -#define HW_VERSION_TINYGV9I 4 -#define HW_VERSION_TINYGV9K 5 - - -/***** Axes, motors & PWM channels used by the application *****/ -// Axes, motors & PWM channels must be defines (not enums) so expressions like this: +/***** Motors & PWM channels supported by this hardware *****/ +// These must be defines (not enums) so expressions like this: // #if (MOTORS >= 6) will work -#define AXES 6 // number of axes supported in this version -#define HOMING_AXES 4 // number of axes that can be homed (assumes Zxyabc sequence) -#define MOTORS 4 // number of motors on the board -#define COORDS 6 // number of supported coordinate systems (index starts at 1) -#define PWMS 2 // number of supported PWM channels -#define TOOLS 32 // number of entries in tool table (index starts at 1) +#define MOTORS 4 // number of motors supported the hardware +#define PWMS 2 // number of PWM channels supported the hardware +/************************* + * Global System Defines * + *************************/ -//////////////////////////// -/////// ARM VERSION //////// -//////////////////////////// +#define MILLISECONDS_PER_TICK 1 // MS for system tick (systick * N) +#define SYS_ID_DIGITS 16 // actual digits in system ID (up to 16) +#define SYS_ID_LEN 24 // total length including dashes and NUL -// ARM specific code start here +/************************* + * Motate Setup * + *************************/ #include "MotatePins.h" -#include "MotateTimers.h" // for TimerChanel<> and related... -#include "MotateServiceCall.h" // for ServiceCall<> +#include "MotateTimers.h" // for TimerChanel<> and related... +#include "MotateServiceCall.h" // for ServiceCall<> using Motate::TimerChannel; using Motate::ServiceCall; @@ -81,14 +71,6 @@ using Motate::Pin; using Motate::PWMOutputPin; using Motate::OutputPin; -/************************* - * Global System Defines * - *************************/ - -#define MILLISECONDS_PER_TICK 1 // MS for system tick (systick * N) -#define SYS_ID_DIGITS 12 // actual digits in system ID (up to 16) -#define SYS_ID_LEN 24 // total length including dashes and NUL - /************************************************************************************ **** ARM SAM3X8E SPECIFIC HARDWARE ************************************************* ************************************************************************************/ @@ -175,18 +157,20 @@ stat_t hardware_periodic(); // callback from the main loop (time sensitive) void hw_hard_reset(void); stat_t hw_flash(nvObj_t *nv); +stat_t hw_get_fb(nvObj_t *nv); +stat_t hw_get_fv(nvObj_t *nv); +stat_t hw_get_hp(nvObj_t *nv); +stat_t hw_get_hv(nvObj_t *nv); stat_t hw_get_fbs(nvObj_t *nv); stat_t hw_get_fbc(nvObj_t *nv); -stat_t hw_set_hv(nvObj_t *nv); stat_t hw_get_id(nvObj_t *nv); #ifdef __TEXT_MODE void hw_print_fb(nvObj_t *nv); + void hw_print_fv(nvObj_t *nv); void hw_print_fbs(nvObj_t *nv); void hw_print_fbc(nvObj_t *nv); - void hw_print_fv(nvObj_t *nv); - void hw_print_cv(nvObj_t *nv); void hw_print_hp(nvObj_t *nv); void hw_print_hv(nvObj_t *nv); void hw_print_id(nvObj_t *nv); @@ -194,10 +178,9 @@ stat_t hw_get_id(nvObj_t *nv); #else #define hw_print_fb tx_print_stub + #define hw_print_fv tx_print_stub #define hw_print_fbs tx_print_stub #define hw_print_fbc tx_print_stub - #define hw_print_fv tx_print_stub - #define hw_print_cv tx_print_stub #define hw_print_hp tx_print_stub #define hw_print_hv tx_print_stub #define hw_print_id tx_print_stub diff --git a/g2core/board/ArduinoDue/motate_pin_assignments.h b/g2core/board/ArduinoDue/motate_pin_assignments.h old mode 100755 new mode 100644 index 0df31f5e..9a5ab0e6 --- a/g2core/board/ArduinoDue/motate_pin_assignments.h +++ b/g2core/board/ArduinoDue/motate_pin_assignments.h @@ -1,5 +1,6 @@ /* - * motate_pin_assignments.h - pin assignments for the g2core shopbotShield boards + * motate_pin_assignments.h - pin assignments + * For: /board/ArduinoDue * This file is part of the g2core project * * Copyright (c) 2013-2016 Robert Giseburt diff --git a/g2core/board/ArduinoDue/shopbotShield-pinout.h b/g2core/board/ArduinoDue/shopbotShield-pinout.h old mode 100755 new mode 100644 index 9b07c9bd..ec284451 --- a/g2core/board/ArduinoDue/shopbotShield-pinout.h +++ b/g2core/board/ArduinoDue/shopbotShield-pinout.h @@ -1,9 +1,10 @@ /* * shopbotShield-pinout.h - board pinout specification + * For: /board/ArduinoDue * This file is part of the g2core project * - * Copyright (c) 2013 - 2016 Robert Giseburt - * Copyright (c) 2013 - 2016 Alden S. Hart Jr. + * Copyright (c) 2013 - 2018 Robert Giseburt + * Copyright (c) 2013 - 2018 Alden S. Hart Jr. * * This file ("the software") is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License, version 2 as published by the diff --git a/g2core/board/G2v9/0_hardware.cpp b/g2core/board/G2v9/0_hardware.cpp old mode 100755 new mode 100644 index 45d63c37..d6405b60 --- a/g2core/board/G2v9/0_hardware.cpp +++ b/g2core/board/G2v9/0_hardware.cpp @@ -1,9 +1,10 @@ /* * hardware.cpp - general hardware support functions + * For: /board/g2v9 * This file is part of the g2core project * - * Copyright (c) 2010 - 2016 Alden S. Hart, Jr. - * Copyright (c) 2013 - 2016 Robert Giseburt + * Copyright (c) 2010 - 2018 Alden S. Hart, Jr. + * Copyright (c) 2013 - 2018 Robert Giseburt * * This file ("the software") is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License, version 2 as published by the @@ -63,12 +64,12 @@ stat_t hardware_periodic() void hw_hard_reset(void) { - Motate::System::reset(/*boootloader: */ false); // arg=0 resets the system + Motate::System::reset(/*bootloader: */ false); // arg=0 resets the system } void hw_flash_loader(void) { - Motate::System::reset(/*boootloader: */ true); // arg=1 erases FLASH and enters FLASH loader + Motate::System::reset(/*bootloader: */ true); // arg=1 erases FLASH and enters FLASH loader } /* @@ -94,15 +95,18 @@ void _get_id(char *id) ***********************************************************************************/ /* + * hw_get_fb() - get firmware build number + * hw_get_fv() - get firmware version number + * hw_get_hp() - get hardware platform string + * hw_get_hv() - get hardware version string * hw_get_fbs() - get firmware build string */ -stat_t hw_get_fbs(nvObj_t *nv) -{ - nv->valuetype = TYPE_STRING; - ritorno(nv_copy_string(nv, G2CORE_FIRMWARE_BUILD_STRING)); - return (STAT_OK); -} +stat_t hw_get_fb(nvObj_t *nv) { return (get_float(nv, cs.fw_build)); } +stat_t hw_get_fv(nvObj_t *nv) { return (get_float(nv, cs.fw_version)); } +stat_t hw_get_hp(nvObj_t *nv) { return (get_string(nv, G2CORE_HARDWARE_PLATFORM)); } +stat_t hw_get_hv(nvObj_t *nv) { return (get_string(nv, G2CORE_HARDWARE_VERSION)); } +stat_t hw_get_fbs(nvObj_t *nv) { return (get_string(nv, G2CORE_FIRMWARE_BUILD_STRING)); } /* * hw_get_fbc() - get configuration settings file @@ -111,15 +115,15 @@ stat_t hw_get_fbs(nvObj_t *nv) stat_t hw_get_fbc(nvObj_t *nv) { nv->valuetype = TYPE_STRING; - #ifdef SETTINGS_FILE - #define settings_file_string1(s) #s - #define settings_file_string2(s) settings_file_string1(s) +#ifdef SETTINGS_FILE +#define settings_file_string1(s) #s +#define settings_file_string2(s) settings_file_string1(s) ritorno(nv_copy_string(nv, settings_file_string2(SETTINGS_FILE))); - #undef settings_file_string1 - #undef settings_file_string2 - #else +#undef settings_file_string1 +#undef settings_file_string2 +#else ritorno(nv_copy_string(nv, "")); - #endif +#endif return (STAT_OK); } @@ -140,20 +144,13 @@ stat_t hw_get_id(nvObj_t *nv) /* * hw_flash() - invoke FLASH loader from command input */ + stat_t hw_flash(nvObj_t *nv) { hw_flash_loader(); return(STAT_OK); } -/* - * hw_set_hv() - set hardware version number - */ -stat_t hw_set_hv(nvObj_t *nv) -{ - return (STAT_OK); -} - /*********************************************************************************** * TEXT MODE SUPPORT @@ -162,22 +159,20 @@ stat_t hw_set_hv(nvObj_t *nv) #ifdef __TEXT_MODE -static const char fmt_fb[] = "[fb] firmware build%18.2f\n"; -static const char fmt_fbs[] = "[fbs] firmware build \"%32s\"\n"; -static const char fmt_fbc[] = "[fbc] firmware config \"%s\"\n"; -static const char fmt_fv[] = "[fv] firmware version%16.2f\n"; -static const char fmt_cv[] = "[cv] configuration version%11.2f\n"; -static const char fmt_hp[] = "[hp] hardware platform%15.2f\n"; -static const char fmt_hv[] = "[hv] hardware version%16.2f\n"; -static const char fmt_id[] = "[id] g2core ID%21s\n"; + static const char fmt_fb[] = "[fb] firmware build%18.2f\n"; + static const char fmt_fv[] = "[fv] firmware version%16.2f\n"; + static const char fmt_fbs[] = "[fbs] firmware build%34s\n"; + static const char fmt_fbc[] = "[fbc] firmware config%33s\n"; + static const char fmt_hp[] = "[hp] hardware platform%15s\n"; + static const char fmt_hv[] = "[hv] hardware version%13s\n"; + static const char fmt_id[] = "[id] g2core ID%37s\n"; -void hw_print_fb(nvObj_t *nv) { text_print(nv, fmt_fb);} // TYPE_FLOAT -void hw_print_fbs(nvObj_t *nv) { text_print(nv, fmt_fbs);} // TYPE_STRING -void hw_print_fbc(nvObj_t *nv) { text_print(nv, fmt_fbc);} // TYPE_STRING -void hw_print_fv(nvObj_t *nv) { text_print(nv, fmt_fv);} // TYPE_FLOAT -void hw_print_cv(nvObj_t *nv) { text_print(nv, fmt_cv);} // TYPE_FLOAT -void hw_print_hp(nvObj_t *nv) { text_print(nv, fmt_hp);} // TYPE_FLOAT -void hw_print_hv(nvObj_t *nv) { text_print(nv, fmt_hv);} // TYPE_FLOAT -void hw_print_id(nvObj_t *nv) { text_print(nv, fmt_id);} // TYPE_STRING + void hw_print_fb(nvObj_t *nv) { text_print(nv, fmt_fb);} // TYPE_FLOAT + void hw_print_fv(nvObj_t *nv) { text_print(nv, fmt_fv);} // TYPE_FLOAT + void hw_print_fbs(nvObj_t *nv) { text_print(nv, fmt_fbs);} // TYPE_STRING + void hw_print_fbc(nvObj_t *nv) { text_print(nv, fmt_fbc);} // TYPE_STRING + void hw_print_hp(nvObj_t *nv) { text_print(nv, fmt_hp);} // TYPE_STRING + void hw_print_hv(nvObj_t *nv) { text_print(nv, fmt_hv);} // TYPE_STRING + void hw_print_id(nvObj_t *nv) { text_print(nv, fmt_id);} // TYPE_STRING #endif //__TEXT_MODE diff --git a/g2core/board/G2v9/G2v9k-pinout.h b/g2core/board/G2v9/G2v9k-pinout.h index 8c89cc8f..593ec818 100644 --- a/g2core/board/G2v9/G2v9k-pinout.h +++ b/g2core/board/G2v9/G2v9k-pinout.h @@ -1,9 +1,10 @@ /* * g2v9k-pinout.h - board pinout specification + * For: /board/g2v9 * This file is part of the g2core project * - * Copyright (c) 2013 - 2016 Robert Giseburt - * Copyright (c) 2013 - 2016 Alden S. Hart Jr. + * Copyright (c) 2013 - 2018 Robert Giseburt + * Copyright (c) 2013 - 2018 Alden S. Hart Jr. * * This file ("the software") is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License, version 2 as published by the @@ -89,7 +90,7 @@ #define TEMPERATURE_OUTPUT_ON 0 // Some pins, if the PWM capability is turned on, it will cause timer conflicts. -// So we have to explicity enable them as PWM pins. +// So we have to explicitly enable them as PWM pins. // Generated with: // perl -e 'for($i=1;$i<14;$i++) { print "#define OUTPUT${i}_PWM 0\n";}' #define OUTPUT1_PWM 0 diff --git a/g2core/board/G2v9/board_stepper.cpp b/g2core/board/G2v9/board_stepper.cpp old mode 100755 new mode 100644 index 0583e4e7..1eb48b1d --- a/g2core/board/G2v9/board_stepper.cpp +++ b/g2core/board/G2v9/board_stepper.cpp @@ -1,9 +1,10 @@ /* * board_stepper.cpp - board-specific code for stepper.cpp + * For: /board/g2v9 * This file is part of the g2core project * - * Copyright (c) 2016 Alden S. Hart, Jr. - * Copyright (c) 2016 Robert Giseburt + * Copyright (c) 2016 - 2018 Alden S. Hart, Jr. + * Copyright (c) 2016 - 2018 Robert Giseburt * * This file ("the software") is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License, version 2 as published by the @@ -28,7 +29,7 @@ #include "board_stepper.h" -// These are identical to board_stepper.h, except for the word "extern" +// These are identical to board_stepper.h, except for the word "extern", and they have the initialization parameters StepDirStepper - motor_1{}; + motor_1{M1_STEP_POLARITY, M1_ENABLE_POLARITY}; StepDirStepper - motor_2{}; + motor_2{M2_STEP_POLARITY, M2_ENABLE_POLARITY}; StepDirStepper - motor_3{}; + motor_3{M3_STEP_POLARITY, M3_ENABLE_POLARITY}; StepDirStepper - motor_4{}; + motor_4{M4_STEP_POLARITY, M4_ENABLE_POLARITY}; // StepDirStepper< // Motate::kSocket5_StepPinNumber, @@ -72,7 +73,8 @@ StepDirStepper motor_5 {}; +// Motate::kSocket5_VrefPinNumber> +// motor_5 {M5_STEP_POLARITY, M5_ENABLE_POLARITY}; // StepDirStepper< // Motate::kSocket6_StepPinNumber, @@ -81,7 +83,8 @@ StepDirStepper motor_6 {}; +// Motate::kSocket6_VrefPinNumber> +// motor_6 {M6_STEP_POLARITY, M6_ENABLE_POLARITY}; Stepper* Motors[MOTORS] = {&motor_1, &motor_2, &motor_3, &motor_4}; diff --git a/g2core/board/G2v9/board_stepper.h b/g2core/board/G2v9/board_stepper.h old mode 100755 new mode 100644 index 25c5d01b..fde1f4f5 --- a/g2core/board/G2v9/board_stepper.h +++ b/g2core/board/G2v9/board_stepper.h @@ -1,9 +1,10 @@ /* * board_stepper.h - board-specific code for stepper.h + * For: /board/g2v9 * This file is part of the g2core project * - * Copyright (c) 2016 Alden S. Hart, Jr. - * Copyright (c) 2016 Robert Giseburt + * Copyright (c) 2016 - 2018 Alden S. Hart, Jr. + * Copyright (c) 2016 - 2018 Robert Giseburt * * This file ("the software") is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License, version 2 as published by the diff --git a/g2core/board/G2v9/board_xio.cpp b/g2core/board/G2v9/board_xio.cpp old mode 100755 new mode 100644 index 710433dc..70ff4599 --- a/g2core/board/G2v9/board_xio.cpp +++ b/g2core/board/G2v9/board_xio.cpp @@ -1,5 +1,6 @@ /* * board_xio.cpp - extended IO functions that are board-specific + * For: /board/g2v9 * This file is part of the g2core project * * Copyright (c) 2016 Alden S. Hart Jr. diff --git a/g2core/board/G2v9/board_xio.h b/g2core/board/G2v9/board_xio.h old mode 100755 new mode 100644 index ea0a3fa7..9fea5e0f --- a/g2core/board/G2v9/board_xio.h +++ b/g2core/board/G2v9/board_xio.h @@ -1,5 +1,6 @@ /* * board_xio.h - extended IO functions that are board-specific + * For: /board/g2v9 * This file is part of the g2core project * * Copyright (c) 2016 Alden S. Hart Jr. diff --git a/g2core/board/G2v9/hardware.h b/g2core/board/G2v9/hardware.h index 5c3e77a8..105e8e5d 100644 --- a/g2core/board/G2v9/hardware.h +++ b/g2core/board/G2v9/hardware.h @@ -1,11 +1,12 @@ /* * hardware.h - system hardware configuration - * THIS FILE IS HARDWARE PLATFORM SPECIFIC - ARM version + * For: /board/g2v9 + * THIS FILE IS HARDWARE PLATFORM SPECIFIC - ARM version * * This file is part of the g2core project * - * Copyright (c) 2013 - 2016 Alden S. Hart, Jr. - * Copyright (c) 2013 - 2016 Robert Giseburt + * Copyright (c) 2013 - 2018 Alden S. Hart, Jr. + * Copyright (c) 2013 - 2018 Robert Giseburt * * This file ("the software") is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License, version 2 as published by the @@ -36,42 +37,31 @@ /*--- Hardware platform enumerations ---*/ -enum hwPlatform { - HM_PLATFORM_NONE = 0, - HW_PLATFORM_TINYG_XMEGA, // TinyG code base on Xmega boards. - HW_PLATFORM_G2_DUE, // G2 code base on native Arduino Due - HW_PLATFORM_V9 // G2 code base on v9 boards -}; +#define G2CORE_HARDWARE_PLATFORM "g2v9" +#define G2CORE_HARDWARE_VERSION "k" -#define HW_VERSION_TINYGV6 6 -#define HW_VERSION_TINYGV7 7 -#define HW_VERSION_TINYGV8 8 - -#define HW_VERSION_TINYGV9I 4 -#define HW_VERSION_TINYGV9K 5 - - -/***** Axes, motors & PWM channels used by the application *****/ -// Axes, motors & PWM channels must be defines (not enums) so expressions like this: +/***** Motors & PWM channels supported by this hardware *****/ +// These must be defines (not enums) so expressions like this: // #if (MOTORS >= 6) will work -#define AXES 6 // number of axes supported in this version -#define HOMING_AXES 4 // number of axes that can be homed (assumes Zxyabc sequence) -#define MOTORS 4 // number of motors on the board -#define COORDS 6 // number of supported coordinate systems (index starts at 1) -#define PWMS 2 // number of supported PWM channels -#define TOOLS 32 // number of entries in tool table (index starts at 1) +#define MOTORS 4 // number of motors supported the hardware +#define PWMS 2 // number of PWM channels supported the hardware +/************************* + * Global System Defines * + *************************/ -//////////////////////////// -/////// ARM VERSION //////// -//////////////////////////// +#define MILLISECONDS_PER_TICK 1 // MS for system tick (systick * N) +#define SYS_ID_DIGITS 16 // actual digits in system ID (up to 16) +#define SYS_ID_LEN 24 // total length including dashes and NUL -// ARM specific code start here +/************************* + * Motate Setup * + *************************/ #include "MotatePins.h" -#include "MotateTimers.h" // for TimerChanel<> and related... -#include "MotateServiceCall.h" // for ServiceCall<> +#include "MotateTimers.h" // for TimerChanel<> and related... +#include "MotateServiceCall.h" // for ServiceCall<> using Motate::TimerChannel; using Motate::ServiceCall; @@ -81,14 +71,6 @@ using Motate::Pin; using Motate::PWMOutputPin; using Motate::OutputPin; -/************************* - * Global System Defines * - *************************/ - -#define MILLISECONDS_PER_TICK 1 // MS for system tick (systick * N) -#define SYS_ID_DIGITS 12 // actual digits in system ID (up to 16) -#define SYS_ID_LEN 24 // total length including dashes and NUL - /************************************************************************************ **** ARM SAM3X8E SPECIFIC HARDWARE ************************************************* ************************************************************************************/ @@ -175,32 +157,33 @@ stat_t hardware_periodic(); // callback from the main loop (time sensitive) void hw_hard_reset(void); stat_t hw_flash(nvObj_t *nv); +stat_t hw_get_fb(nvObj_t *nv); +stat_t hw_get_fv(nvObj_t *nv); +stat_t hw_get_hp(nvObj_t *nv); +stat_t hw_get_hv(nvObj_t *nv); stat_t hw_get_fbs(nvObj_t *nv); stat_t hw_get_fbc(nvObj_t *nv); -stat_t hw_set_hv(nvObj_t *nv); stat_t hw_get_id(nvObj_t *nv); #ifdef __TEXT_MODE - void hw_print_fb(nvObj_t *nv); + void hw_print_fb(nvObj_t *nv); + void hw_print_fv(nvObj_t *nv); void hw_print_fbs(nvObj_t *nv); void hw_print_fbc(nvObj_t *nv); - void hw_print_fv(nvObj_t *nv); - void hw_print_cv(nvObj_t *nv); - void hw_print_hp(nvObj_t *nv); - void hw_print_hv(nvObj_t *nv); - void hw_print_id(nvObj_t *nv); + void hw_print_hp(nvObj_t *nv); + void hw_print_hv(nvObj_t *nv); + void hw_print_id(nvObj_t *nv); #else - #define hw_print_fb tx_print_stub + #define hw_print_fb tx_print_stub + #define hw_print_fv tx_print_stub #define hw_print_fbs tx_print_stub #define hw_print_fbc tx_print_stub - #define hw_print_fv tx_print_stub - #define hw_print_cv tx_print_stub - #define hw_print_hp tx_print_stub - #define hw_print_hv tx_print_stub - #define hw_print_id tx_print_stub + #define hw_print_hp tx_print_stub + #define hw_print_hv tx_print_stub + #define hw_print_id tx_print_stub #endif // __TEXT_MODE diff --git a/g2core/board/G2v9/motate_pin_assignments.h b/g2core/board/G2v9/motate_pin_assignments.h index 28091bc4..94f98d42 100644 --- a/g2core/board/G2v9/motate_pin_assignments.h +++ b/g2core/board/G2v9/motate_pin_assignments.h @@ -1,9 +1,10 @@ /* - * motate_pin_assignments.h - pin assignments for the g2core g2v9 boards + * motate_pin_assignments.h - pin assignments + * For: /board/g2v9 * This file is part of the g2core project * - * Copyright (c) 2013 - 2016 Robert Giseburt - * Copyright (c) 2013 - 2016 Alden S. Hart Jr. + * Copyright (c) 2013 - 2018 Robert Giseburt + * Copyright (c) 2013 - 2018 Alden S. Hart Jr. * * This file ("the software") is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License, version 2 as published by the diff --git a/g2core/board/gquadratic/0_hardware.cpp b/g2core/board/gquadratic/0_hardware.cpp old mode 100755 new mode 100644 index 465c7175..de4b7d8a --- a/g2core/board/gquadratic/0_hardware.cpp +++ b/g2core/board/gquadratic/0_hardware.cpp @@ -1,8 +1,9 @@ /* * hardware.cpp - general hardware support functions + * For: /board/gQuadratic * This file is part of the g2core project * - * Copyright (c) 2010 - 2015 Alden S. Hart, Jr. + * Copyright (c) 2010 - 2018 Alden S. Hart, Jr. * * This file ("the software") is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License, version 2 as published by the @@ -106,7 +107,7 @@ void hardware_init() stat_t hardware_periodic() { #if EXPERIMENTAL_NEOPIXEL_SUPPORT == 1 - float x_pos = cm_get_work_position(ACTIVE_MODEL, AXIS_X); + float x_pos = cm_get_display_position(ACTIVE_MODEL, AXIS_X); if (std::abs(LEDs::old_x_pos - x_pos) > 0.01) { LEDs::old_x_pos = x_pos; @@ -178,15 +179,18 @@ void _get_id(char *id) ***********************************************************************************/ /* + * hw_get_fb() - get firmware build number + * hw_get_fv() - get firmware version number + * hw_get_hp() - get hardware platform string + * hw_get_hv() - get hardware version string * hw_get_fbs() - get firmware build string */ -stat_t hw_get_fbs(nvObj_t *nv) -{ - nv->valuetype = TYPE_STRING; - ritorno(nv_copy_string(nv, G2CORE_FIRMWARE_BUILD_STRING)); - return (STAT_OK); -} +stat_t hw_get_fb(nvObj_t *nv) { return (get_float(nv, cs.fw_build)); } +stat_t hw_get_fv(nvObj_t *nv) { return (get_float(nv, cs.fw_version)); } +stat_t hw_get_hp(nvObj_t *nv) { return (get_string(nv, G2CORE_HARDWARE_PLATFORM)); } +stat_t hw_get_hv(nvObj_t *nv) { return (get_string(nv, G2CORE_HARDWARE_VERSION)); } +stat_t hw_get_fbs(nvObj_t *nv) { return (get_string(nv, G2CORE_FIRMWARE_BUILD_STRING)); } /* * hw_get_fbc() - get configuration settings file @@ -230,15 +234,6 @@ stat_t hw_flash(nvObj_t *nv) return(STAT_OK); } -/* - * hw_set_hv() - set hardware version number - */ -stat_t hw_set_hv(nvObj_t *nv) -{ - return (STAT_OK); -} - - /*********************************************************************************** * TEXT MODE SUPPORT * Functions to print variables from the cfgArray table @@ -246,22 +241,21 @@ stat_t hw_set_hv(nvObj_t *nv) #ifdef __TEXT_MODE -static const char fmt_fb[] = "[fb] firmware build %18.2f\n"; -static const char fmt_fbs[] = "[fbs] firmware build \"%s\"\n"; -static const char fmt_fbc[] = "[fbc] firmware config \"%s\"\n"; -static const char fmt_fv[] = "[fv] firmware version%16.2f\n"; -static const char fmt_cv[] = "[cv] configuration version%11.2f\n"; -static const char fmt_hp[] = "[hp] hardware platform%15.2f\n"; -static const char fmt_hv[] = "[hv] hardware version%16.2f\n"; -static const char fmt_id[] = "[id] g2core ID%21s\n"; + static const char fmt_fb[] = "[fb] firmware build%18.2f\n"; + static const char fmt_fv[] = "[fv] firmware version%16.2f\n"; + static const char fmt_fbs[] = "[fbs] firmware build%34s\n"; + static const char fmt_fbc[] = "[fbc] firmware config%33s\n"; + static const char fmt_hp[] = "[hp] hardware platform%15s\n"; + static const char fmt_hv[] = "[hv] hardware version%13s\n"; + static const char fmt_id[] = "[id] g2core ID%37s\n"; -void hw_print_fb(nvObj_t *nv) { text_print(nv, fmt_fb);} // TYPE_FLOAT -void hw_print_fbs(nvObj_t *nv) { text_print(nv, fmt_fbs);} // TYPE_STRING -void hw_print_fbc(nvObj_t *nv) { text_print(nv, fmt_fbc);} // TYPE_STRING -void hw_print_fv(nvObj_t *nv) { text_print(nv, fmt_fv);} // TYPE_FLOAT -void hw_print_cv(nvObj_t *nv) { text_print(nv, fmt_cv);} // TYPE_FLOAT -void hw_print_hp(nvObj_t *nv) { text_print(nv, fmt_hp);} // TYPE_FLOAT -void hw_print_hv(nvObj_t *nv) { text_print(nv, fmt_hv);} // TYPE_FLOAT -void hw_print_id(nvObj_t *nv) { text_print(nv, fmt_id);} // TYPE_STRING + void hw_print_fb(nvObj_t *nv) { text_print(nv, fmt_fb);} // TYPE_FLOAT + void hw_print_fv(nvObj_t *nv) { text_print(nv, fmt_fv);} // TYPE_FLOAT + void hw_print_fbs(nvObj_t *nv) { text_print(nv, fmt_fbs);} // TYPE_STRING + void hw_print_fbc(nvObj_t *nv) { text_print(nv, fmt_fbc);} // TYPE_STRING + void hw_print_hp(nvObj_t *nv) { text_print(nv, fmt_hp);} // TYPE_STRING + void hw_print_hv(nvObj_t *nv) { text_print(nv, fmt_hv);} // TYPE_STRING + void hw_print_id(nvObj_t *nv) { text_print(nv, fmt_id);} // TYPE_STRING #endif //__TEXT_MODE + diff --git a/g2core/board/gquadratic/board_stepper.cpp b/g2core/board/gquadratic/board_stepper.cpp old mode 100755 new mode 100644 index cfe46144..b7908401 --- a/g2core/board/gquadratic/board_stepper.cpp +++ b/g2core/board/gquadratic/board_stepper.cpp @@ -1,9 +1,10 @@ /* * board_stepper.cpp - board-specific code for stepper.cpp + * For: /board/gQuadratic * This file is part of the g2core project * - * Copyright (c) 2016-2018 Alden S. Hart, Jr. - * Copyright (c) 2016-2018 Robert Giseburt + * Copyright (c) 2016 - 2019 Alden S. Hart, Jr. + * Copyright (c) 2016 - 2019 Robert Giseburt * * This file ("the software") is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License, version 2 as published by the @@ -50,7 +51,6 @@ HOT_DATA StepDirStepper motor_1_cs; HOT_DATA Trinamic2130= 6) will work +<<<<<<< HEAD #define AXES 6 // number of axes supported in this version #define HOMING_AXES 4 // number of axes that can be homed (assumes Zxyabc sequence) #define MOTORS 3 // number of motors on the board #define COORDS 6 // number of supported coordinate systems (index starts at 1) #define PWMS 2 // number of supported PWM channels #define TOOLS 32 // number of entries in tool table (index starts at 1) +======= +#define MOTORS 4 // number of motors supported the hardware +#define PWMS 2 // number of PWM channels supported the hardware +>>>>>>> edge -//////////////////////////// -/////// ARM VERSION //////// -//////////////////////////// +/************************* + * Global System Defines * + *************************/ -// ARM specific code start here +#define MILLISECONDS_PER_TICK 1 // MS for system tick (systick * N) +#define SYS_ID_DIGITS 16 // actual digits in system ID (up to 16) +#define SYS_ID_LEN 24 // total length including dashes and NUL + +/************************* + * Motate Setup * + *************************/ #include "MotatePins.h" +<<<<<<< HEAD #if QUADRATIC_REVISION == 'C' #define MOTOR_1_IS_TRINAMIC #define MOTOR_2_IS_TRINAMIC @@ -78,6 +79,10 @@ enum hwPlatform { #endif #include "MotateTimers.h" // for TimerChanel<> and related... #include "MotateServiceCall.h" // for ServiceCall<> +======= +#include "MotateTimers.h" // for TimerChanel<> and related... +#include "MotateServiceCall.h" // for ServiceCall<> +>>>>>>> edge using Motate::TimerChannel; using Motate::ServiceCall; @@ -87,6 +92,7 @@ using Motate::Pin; using Motate::PWMOutputPin; using Motate::OutputPin; +<<<<<<< HEAD /************************* * Global System Defines * *************************/ @@ -94,6 +100,36 @@ using Motate::OutputPin; #define MILLISECONDS_PER_TICK 1 // MS for system tick (systick * N) #define SYS_ID_DIGITS 12 // actual digits in system ID (up to 16) #define SYS_ID_LEN 24 // total length including dashes and NUL +======= +/************************************************************************************ + **** ARM SAM3X8E SPECIFIC HARDWARE ************************************************* + ************************************************************************************/ + +/**** Resource Assignment via Motate **** + * + * This section defines resource usage for pins, timers, PWM channels, communications + * and other resources. Please refer to /motate/utility/SamPins.h, SamTimers.h and + * other files for pinouts and other configuration details. + * + * Commenting out or #ifdef'ing out definitions below will cause the compiler to + * drop references to these resources from the compiled code. This will reduce + * compiled code size and runtime CPU cycles. E.g. if you are compiling for a 3 motor, + * XYZ axis config commenting out the higher motors and axes here will remove them + * from later code (using the motate .isNull() test). + */ + +/* Interrupt usage and priority + * + * The following interrupts are defined w/indicated priorities + * + * 0 DDA_TIMER (9) for step pulse generation + * 1 DWELL_TIMER (10) for dwell timing + * 2 LOADER software generated interrupt (STIR / SGI) + * 3 Serial read character interrupt + * 4 EXEC software generated interrupt (STIR / SGI) + * 5 Serial write character interrupt + */ +>>>>>>> edge /**** Stepper DDA and dwell timer settings ****/ @@ -103,7 +139,7 @@ using Motate::OutputPin; #define MIN_SEGMENT_MS ((float)0.125) // S70 can handle much much smaller segements -#define PLANNER_BUFFER_POOL_SIZE (60) +#define PLANNER_QUEUE_SIZE (60) /**** Motate Definitions ****/ @@ -151,33 +187,35 @@ stat_t hardware_periodic(); // callback from the main loop (time sensitive) void hw_hard_reset(void); stat_t hw_flash(nvObj_t* nv); -stat_t hw_get_fbs(nvObj_t* nv); -stat_t hw_get_fbc(nvObj_t* nv); -stat_t hw_set_hv(nvObj_t* nv); -stat_t hw_get_id(nvObj_t* nv); +stat_t hw_get_fb(nvObj_t *nv); +stat_t hw_get_fv(nvObj_t *nv); +stat_t hw_get_hp(nvObj_t *nv); +stat_t hw_get_hv(nvObj_t *nv); +stat_t hw_get_fbs(nvObj_t *nv); +stat_t hw_get_fbc(nvObj_t *nv); +stat_t hw_get_id(nvObj_t *nv); #ifdef __TEXT_MODE -void hw_print_fb(nvObj_t* nv); -void hw_print_fbs(nvObj_t* nv); -void hw_print_fbc(nvObj_t* nv); -void hw_print_fv(nvObj_t* nv); -void hw_print_cv(nvObj_t* nv); -void hw_print_hp(nvObj_t* nv); -void hw_print_hv(nvObj_t* nv); -void hw_print_id(nvObj_t* nv); + void hw_print_fb(nvObj_t *nv); + void hw_print_fv(nvObj_t *nv); + void hw_print_fbs(nvObj_t *nv); + void hw_print_fbc(nvObj_t *nv); + void hw_print_hp(nvObj_t *nv); + void hw_print_hv(nvObj_t *nv); + void hw_print_id(nvObj_t *nv); #else -#define hw_print_fb tx_print_stub -#define hw_print_fbs tx_print_stub -#define hw_print_fbc tx_print_stub -#define hw_print_fv tx_print_stub -#define hw_print_cv tx_print_stub -#define hw_print_hp tx_print_stub -#define hw_print_hv tx_print_stub -#define hw_print_id tx_print_stub + #define hw_print_fb tx_print_stub + #define hw_print_fv tx_print_stub + #define hw_print_fbs tx_print_stub + #define hw_print_fbc tx_print_stub + #define hw_print_hp tx_print_stub + #define hw_print_hv tx_print_stub + #define hw_print_id tx_print_stub + +#endif // __TEXT_MODE -#endif // __TEXT_MODE #endif // end of include guard: HARDWARE_H_ONCE diff --git a/g2core/board/gquadratic/motate_pin_assignments.h b/g2core/board/gquadratic/motate_pin_assignments.h old mode 100755 new mode 100644 index 57c099bb..bbed43b7 --- a/g2core/board/gquadratic/motate_pin_assignments.h +++ b/g2core/board/gquadratic/motate_pin_assignments.h @@ -1,9 +1,10 @@ /* - * motate_pin_assignments.h - pin assignments for the gQuadratic boards + * motate_pin_assignments.h - pin assignments + * For: /board/gQuadratic * This file is part of the g2core project * - * Copyright (c) 2013 - 2016 Robert Giseburt - * Copyright (c) 2013 - 2016 Alden S. Hart Jr. + * Copyright (c) 2013 - 2018 Robert Giseburt + * Copyright (c) 2013 - 2018 Alden S. Hart Jr. * * This file ("the software") is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License, version 2 as published by the diff --git a/g2core/board/gquintic/0_hardware.cpp b/g2core/board/gquintic/0_hardware.cpp old mode 100755 new mode 100644 index 9891e41a..5b1f9487 --- a/g2core/board/gquintic/0_hardware.cpp +++ b/g2core/board/gquintic/0_hardware.cpp @@ -1,5 +1,6 @@ /* * hardware.cpp - general hardware support functions + * For: /board/gQuintic * This file is part of the g2core project * * Copyright (c) 2010 - 2015 Alden S. Hart, Jr. @@ -42,13 +43,15 @@ Motate::OutputPin external_clk_pin {Motate::k HOT_DATA SPI_CS_PinMux_used_t spiCSPinMux; HOT_DATA SPIBus_used_t spiBus; -HOT_DATA TWIBus_used_t twiBus; -HOT_DATA plex0_t plex0{twiBus, 0x0070L}; -HOT_DATA plex1_t plex1{twiBus, 0x0071L}; +// HOT_DATA TWIBus_used_t twiBus; + +// // Define Multiplexers +// HOT_DATA plex0_t plex0{twiBus, 0x0070L}; +// HOT_DATA plex1_t plex1{twiBus, 0x0071L}; + // HOT_DATA I2C_EEPROM eeprom{twiBus, 0b01010000}; - // alignas(4) uint8_t eeprom_buffer[128] HOT_DATA = "TestinglyABCDEFGHIJKLmnop"; // alignas(4) uint8_t eeprom_in_buffer[128] HOT_DATA = ""; @@ -59,7 +62,7 @@ HOT_DATA plex1_t plex1{twiBus, 0x0071L}; void hardware_init() { spiBus.init(); - twiBus.init(); + // twiBus.init(); board_hardware_init(); external_clk_pin = 0; // Force external clock to 0 for now. } @@ -188,15 +191,18 @@ void _get_id(char *id) ***********************************************************************************/ /* + * hw_get_fb() - get firmware build number + * hw_get_fv() - get firmware version number + * hw_get_hp() - get hardware platform string + * hw_get_hv() - get hardware version string * hw_get_fbs() - get firmware build string */ -stat_t hw_get_fbs(nvObj_t *nv) -{ - nv->valuetype = TYPE_STRING; - ritorno(nv_copy_string(nv, G2CORE_FIRMWARE_BUILD_STRING)); - return (STAT_OK); -} +stat_t hw_get_fb(nvObj_t *nv) { return (get_float(nv, cs.fw_build)); } +stat_t hw_get_fv(nvObj_t *nv) { return (get_float(nv, cs.fw_version)); } +stat_t hw_get_hp(nvObj_t *nv) { return (get_string(nv, G2CORE_HARDWARE_PLATFORM)); } +stat_t hw_get_hv(nvObj_t *nv) { return (get_string(nv, G2CORE_HARDWARE_VERSION)); } +stat_t hw_get_fbs(nvObj_t *nv) { return (get_string(nv, G2CORE_FIRMWARE_BUILD_STRING)); } /* * hw_get_fbc() - get configuration settings file @@ -240,14 +246,6 @@ stat_t hw_flash(nvObj_t *nv) return(STAT_OK); } -/* - * hw_set_hv() - set hardware version number - */ -stat_t hw_set_hv(nvObj_t *nv) -{ - return (STAT_OK); -} - /*********************************************************************************** * TEXT MODE SUPPORT @@ -256,22 +254,20 @@ stat_t hw_set_hv(nvObj_t *nv) #ifdef __TEXT_MODE -static const char fmt_fb[] = "[fb] firmware build %18.2f\n"; -static const char fmt_fbs[] = "[fbs] firmware build \"%s\"\n"; -static const char fmt_fbc[] = "[fbc] firmware config \"%s\"\n"; -static const char fmt_fv[] = "[fv] firmware version%16.2f\n"; -static const char fmt_cv[] = "[cv] configuration version%11.2f\n"; -static const char fmt_hp[] = "[hp] hardware platform%15.2f\n"; -static const char fmt_hv[] = "[hv] hardware version%16.2f\n"; -static const char fmt_id[] = "[id] g2core ID%21s\n"; + static const char fmt_fb[] = "[fb] firmware build%18.2f\n"; + static const char fmt_fv[] = "[fv] firmware version%16.2f\n"; + static const char fmt_fbs[] = "[fbs] firmware build%34s\n"; + static const char fmt_fbc[] = "[fbc] firmware config%33s\n"; + static const char fmt_hp[] = "[hp] hardware platform%15s\n"; + static const char fmt_hv[] = "[hv] hardware version%13s\n"; + static const char fmt_id[] = "[id] g2core ID%37s\n"; -void hw_print_fb(nvObj_t *nv) { text_print(nv, fmt_fb);} // TYPE_FLOAT -void hw_print_fbs(nvObj_t *nv) { text_print(nv, fmt_fbs);} // TYPE_STRING -void hw_print_fbc(nvObj_t *nv) { text_print(nv, fmt_fbc);} // TYPE_STRING -void hw_print_fv(nvObj_t *nv) { text_print(nv, fmt_fv);} // TYPE_FLOAT -void hw_print_cv(nvObj_t *nv) { text_print(nv, fmt_cv);} // TYPE_FLOAT -void hw_print_hp(nvObj_t *nv) { text_print(nv, fmt_hp);} // TYPE_FLOAT -void hw_print_hv(nvObj_t *nv) { text_print(nv, fmt_hv);} // TYPE_FLOAT -void hw_print_id(nvObj_t *nv) { text_print(nv, fmt_id);} // TYPE_STRING + void hw_print_fb(nvObj_t *nv) { text_print(nv, fmt_fb);} // TYPE_FLOAT + void hw_print_fv(nvObj_t *nv) { text_print(nv, fmt_fv);} // TYPE_FLOAT + void hw_print_fbs(nvObj_t *nv) { text_print(nv, fmt_fbs);} // TYPE_STRING + void hw_print_fbc(nvObj_t *nv) { text_print(nv, fmt_fbc);} // TYPE_STRING + void hw_print_hp(nvObj_t *nv) { text_print(nv, fmt_hp);} // TYPE_STRING + void hw_print_hv(nvObj_t *nv) { text_print(nv, fmt_hv);} // TYPE_STRING + void hw_print_id(nvObj_t *nv) { text_print(nv, fmt_id);} // TYPE_STRING #endif //__TEXT_MODE diff --git a/g2core/board/gquintic/board_stepper.cpp b/g2core/board/gquintic/board_stepper.cpp old mode 100755 new mode 100644 index b8d02fdb..f9e4a2e0 --- a/g2core/board/gquintic/board_stepper.cpp +++ b/g2core/board/gquintic/board_stepper.cpp @@ -1,9 +1,10 @@ /* * board_stepper.cpp - board-specific code for stepper.cpp + * For: /board/gQuintic * This file is part of the g2core project * - * Copyright (c) 2016-2018 Alden S. Hart, Jr. - * Copyright (c) 2016-2018 Robert Giseburt + * Copyright (c) 2016 - 2019 Alden S. Hart, Jr. + * Copyright (c) 2016 - 2019 Robert Giseburt * * This file ("the software") is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License, version 2 as published by the diff --git a/g2core/board/gquintic/board_stepper.h b/g2core/board/gquintic/board_stepper.h old mode 100755 new mode 100644 index fb0dd913..37859058 --- a/g2core/board/gquintic/board_stepper.h +++ b/g2core/board/gquintic/board_stepper.h @@ -1,9 +1,10 @@ /* * board_stepper.h - board-specific code for stepper.h + * For: /board/gQuintic * This file is part of the g2core project * - * Copyright (c) 2016 Alden S. Hart, Jr. - * Copyright (c) 2016 Robert Giseburt + * Copyright (c) 2016 - 2018 Alden S. Hart, Jr. + * Copyright (c) 2016 - 2018 Robert Giseburt * * This file ("the software") is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License, version 2 as published by the @@ -63,21 +64,25 @@ extern Trinamic2130 motor_1; + extern Trinamic2130 motor_2; + extern Trinamic2130 motor_3; + extern Trinamic2130 motor_4; + extern Trinamic2130= 6) will work -#define AXES 6 // number of axes supported in this version -#define HOMING_AXES 4 // number of axes that can be homed (assumes Zxyabc sequence) #if QUINTIC_REVISION == 'C' -#define MOTORS 5 // number of motors on the board - 4 Trinamics + 1 servo +#define MOTORS 5 // number of motors on the board - 4 Trinamics + 1 servo #else -#define MOTORS 6 // number of motors on the board - 5 Trinamics + 1 servo +#define MOTORS 6 // number of motors on the board - 5 Trinamics + 1 servo #endif -#define COORDS 6 // number of supported coordinate systems (index starts at 1) -#define PWMS 2 // number of supported PWM channels -#define TOOLS 32 // number of entries in tool table (index starts at 1) +#define PWMS 2 // number of PWM channels supported the hardware #define MOTOR_1_IS_TRINAMIC #define MOTOR_2_IS_TRINAMIC @@ -75,19 +59,29 @@ enum hwPlatform { #if QUINTIC_REVISION == 'D' #define MOTOR_5_IS_TRINAMIC #endif -//////////////////////////// -/////// ARM VERSION //////// -//////////////////////////// + +/************************* + * Global System Defines * + *************************/ + +#define MILLISECONDS_PER_TICK 1 // MS for system tick (systick * N) +#define SYS_ID_DIGITS 16 // actual digits in system ID (up to 16) +#define SYS_ID_LEN 24 // total length including dashes and NUL + +/************************* + * Motate Setup * + *************************/ #include "MotatePins.h" #include "MotateSPI.h" #include "MotateTWI.h" -#include "MotateTimers.h" // for TimerChanel<> and related... -#include "MotateServiceCall.h" // for ServiceCall<> +#include "MotateTimers.h" // for TimerChanel<> and related... +#include "MotateServiceCall.h" // for ServiceCall<> -#include "i2c_eeprom.h" -#include "i2c_multiplexer.h" -#include "i2c_as5601.h" // For AS5601 +// Temporarily disabled: +// #include "i2c_eeprom.h" +// #include "i2c_multiplexer.h" +// #include "i2c_as5601.h" // For AS5601 using Motate::TimerChannel; using Motate::ServiceCall; @@ -97,14 +91,6 @@ using Motate::Pin; using Motate::PWMOutputPin; using Motate::OutputPin; -/************************* - * Global System Defines * - *************************/ - -#define MILLISECONDS_PER_TICK 1 // MS for system tick (systick * N) -#define SYS_ID_DIGITS 12 // actual digits in system ID (up to 16) -#define SYS_ID_LEN 24 // total length including dashes and NUL - /************************************************************************************ **** ARM SAM3X8E SPECIFIC HARDWARE ************************************************* ************************************************************************************/ @@ -143,14 +129,14 @@ using Motate::OutputPin; // #define MIN_SEGMENT_MS ((float)0.125) // S70 can handle much much smaller segements #define MIN_SEGMENT_MS ((float)0.5) // S70 can handle much much smaller segements -#define PLANNER_BUFFER_POOL_SIZE (60) +#define PLANNER_QUEUE_SIZE (60) /**** Motate Definitions ****/ // Timer definitions. See stepper.h and other headers for setup typedef TimerChannel<9, 0> dda_timer_type; // stepper pulse generation in stepper.cpp typedef TimerChannel<10, 0> exec_timer_type; // request exec timer in stepper.cpp -typedef TimerChannel<11, 0> fwd_plan_timer_type; // request exec timer in stepper.cpp +typedef TimerChannel<11, 0> fwd_plan_timer_type; // request forward planner in stepper.cpp /**** SPI Setup ****/ Motate::service_call_number kSPI_ServiceCallNumber = 3; @@ -162,15 +148,15 @@ typedef Motate::SPIChipSelectPinMux TWIBus_used_t; -extern TWIBus_used_t twiBus; +// typedef Motate::TWIBus TWIBus_used_t; +// extern TWIBus_used_t twiBus; -using plex0_t = decltype(I2C_Multiplexer{twiBus, 0x0070L}); -extern HOT_DATA plex0_t plex0; -using plex1_t = decltype(I2C_Multiplexer{twiBus, 0x0071L}); -extern HOT_DATA plex1_t plex1; +// using plex0_t = decltype(I2C_Multiplexer{twiBus, 0x0070L}); +// extern HOT_DATA plex0_t plex0; +// using plex1_t = decltype(I2C_Multiplexer{twiBus, 0x0071L}); +// extern HOT_DATA plex1_t plex1; // extern HOT_DATA I2C_EEPROM eeprom; @@ -205,18 +191,20 @@ stat_t hardware_periodic(); // callback from the main loop (time sensitive) void hw_hard_reset(void); stat_t hw_flash(nvObj_t *nv); +stat_t hw_get_fb(nvObj_t *nv); +stat_t hw_get_fv(nvObj_t *nv); +stat_t hw_get_hp(nvObj_t *nv); +stat_t hw_get_hv(nvObj_t *nv); stat_t hw_get_fbs(nvObj_t *nv); stat_t hw_get_fbc(nvObj_t *nv); -stat_t hw_set_hv(nvObj_t *nv); stat_t hw_get_id(nvObj_t *nv); #ifdef __TEXT_MODE void hw_print_fb(nvObj_t *nv); + void hw_print_fv(nvObj_t *nv); void hw_print_fbs(nvObj_t *nv); void hw_print_fbc(nvObj_t *nv); - void hw_print_fv(nvObj_t *nv); - void hw_print_cv(nvObj_t *nv); void hw_print_hp(nvObj_t *nv); void hw_print_hv(nvObj_t *nv); void hw_print_id(nvObj_t *nv); @@ -224,10 +212,9 @@ stat_t hw_get_id(nvObj_t *nv); #else #define hw_print_fb tx_print_stub + #define hw_print_fv tx_print_stub #define hw_print_fbs tx_print_stub #define hw_print_fbc tx_print_stub - #define hw_print_fv tx_print_stub - #define hw_print_cv tx_print_stub #define hw_print_hp tx_print_stub #define hw_print_hv tx_print_stub #define hw_print_id tx_print_stub diff --git a/g2core/board/gquintic/motate_pin_assignments.h b/g2core/board/gquintic/motate_pin_assignments.h old mode 100755 new mode 100644 index 02c75bf4..e1a9e5a0 --- a/g2core/board/gquintic/motate_pin_assignments.h +++ b/g2core/board/gquintic/motate_pin_assignments.h @@ -1,9 +1,10 @@ /* - * motate_pin_assignments.h - pin assignments for the gQuintic boards + * motate_pin_assignments.h - pin assignments + * For: /board/gQuintic * This file is part of the g2core project * - * Copyright (c) 2013 - 2016 Robert Giseburt - * Copyright (c) 2013 - 2016 Alden S. Hart Jr. + * Copyright (c) 2013 - 2018 Robert Giseburt + * Copyright (c) 2013 - 2018 Alden S. Hart Jr. * * This file ("the software") is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License, version 2 as published by the diff --git a/g2core/board/printrboardg2/0_hardware.cpp b/g2core/board/printrboardg2/0_hardware.cpp old mode 100755 new mode 100644 index 178fbfdd..0d1085b5 --- a/g2core/board/printrboardg2/0_hardware.cpp +++ b/g2core/board/printrboardg2/0_hardware.cpp @@ -1,9 +1,10 @@ /* * hardware.cpp - general hardware support functions + * For: /board/printrboardg2 * This file is part of the g2core project * - * Copyright (c) 2010 - 2016 Alden S. Hart, Jr. - * Copyright (c) 2013 - 2016 Robert Giseburt + * Copyright (c) 2010 - 2018 Alden S. Hart, Jr. + * Copyright (c) 2013 - 2018 Robert Giseburt * * This file ("the software") is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License, version 2 as published by the @@ -42,8 +43,6 @@ #include "neopixel.h" - - namespace LEDs { NeoPixel rgbw_leds {NeoPixelOrder::GRBW}; @@ -199,15 +198,18 @@ void _get_id(char *id) ***********************************************************************************/ /* + * hw_get_fb() - get firmware build number + * hw_get_fv() - get firmware version number + * hw_get_hp() - get hardware platform string + * hw_get_hv() - get hardware version string * hw_get_fbs() - get firmware build string */ -stat_t hw_get_fbs(nvObj_t *nv) -{ - nv->valuetype = TYPE_STRING; - ritorno(nv_copy_string(nv, G2CORE_FIRMWARE_BUILD_STRING)); - return (STAT_OK); -} +stat_t hw_get_fb(nvObj_t *nv) { return (get_float(nv, cs.fw_build)); } +stat_t hw_get_fv(nvObj_t *nv) { return (get_float(nv, cs.fw_version)); } +stat_t hw_get_hp(nvObj_t *nv) { return (get_string(nv, G2CORE_HARDWARE_PLATFORM)); } +stat_t hw_get_hv(nvObj_t *nv) { return (get_string(nv, G2CORE_HARDWARE_VERSION)); } +stat_t hw_get_fbs(nvObj_t *nv) { return (get_string(nv, G2CORE_FIRMWARE_BUILD_STRING)); } /* * hw_get_fbc() - get configuration settings file @@ -251,17 +253,9 @@ stat_t hw_flash(nvObj_t *nv) return(STAT_OK); } -/* - * hw_set_hv() - set hardware version number - */ -stat_t hw_set_hv(nvObj_t *nv) -{ - return (STAT_OK); -} - stat_t _get_leds(nvObj_t *nv) { - nv->valuetype = TYPE_INT; + nv->valuetype = TYPE_INTEGER; float red, green, blue; @@ -269,34 +263,34 @@ stat_t _get_leds(nvObj_t *nv) // black if (fp_EQ(red, 0.0) && fp_EQ(green, 0.0) && fp_EQ(blue, 0.0)) { - nv->value = 0; + nv->value_int = 0; // white } else if (fp_EQ(red, 1.0) && fp_EQ(green, 1.0) && fp_EQ(blue, 1.0)) { - nv->value = 1; + nv->value_int = 1; // red } else if (fp_EQ(red, 1.0) && fp_EQ(green, 0.0) && fp_EQ(blue, 0.0)) { - nv->value = 2; + nv->value_int = 2; // green } else if (fp_EQ(red, 0.0) && fp_EQ(green, 1.0) && fp_EQ(blue, 0.0)) { - nv->value = 3; + nv->value_int = 3; // blue } else if (fp_EQ(red, 0.0) && fp_EQ(green, 0.0) && fp_EQ(blue, 1.0)) { - nv->value = 4; + nv->value_int = 4; // orange } else if (fp_EQ(red, 1.0) && fp_EQ(green, 0.5) && fp_EQ(blue, 0.0)) { - nv->value = 5; + nv->value_int = 5; // yellow } else if (fp_EQ(red, 1.0) && fp_EQ(green, 1.0) && fp_EQ(blue, 0.0)) { - nv->value = 6; + nv->value_int = 6; } - return (STAT_OK); } stat_t _set_leds(nvObj_t *nv) { - uint32_t value = nv->value; - if ((nv->value < 0) || (nv->value > 6)) { + uint32_t value = nv->value_int; +// if ((nv->value_int < 0) || (nv->value_int > 6)) { + if ((value < 0) || (value > 6)) { return (STAT_INPUT_VALUE_RANGE_ERROR); } @@ -336,7 +330,6 @@ stat_t _set_leds(nvObj_t *nv) LEDs::display_color[pixel].startTransition(100, 1.0, 1.0, 0.0); } } - return (STAT_OK); } @@ -348,22 +341,20 @@ stat_t _set_leds(nvObj_t *nv) #ifdef __TEXT_MODE -static const char fmt_fb[] = "[fb] firmware build %18.2f\n"; -static const char fmt_fbs[] = "[fbs] firmware build \"%s\"\n"; -static const char fmt_fbc[] = "[fbc] firmware config \"%s\"\n"; -static const char fmt_fv[] = "[fv] firmware version%16.2f\n"; -static const char fmt_cv[] = "[cv] configuration version%11.2f\n"; -static const char fmt_hp[] = "[hp] hardware platform%15.2f\n"; -static const char fmt_hv[] = "[hv] hardware version%16.2f\n"; -static const char fmt_id[] = "[id] g2core ID%21s\n"; + static const char fmt_fb[] = "[fb] firmware build%18.2f\n"; + static const char fmt_fv[] = "[fv] firmware version%16.2f\n"; + static const char fmt_fbs[] = "[fbs] firmware build%34s\n"; + static const char fmt_fbc[] = "[fbc] firmware config%33s\n"; + static const char fmt_hp[] = "[hp] hardware platform%15s\n"; + static const char fmt_hv[] = "[hv] hardware version%13s\n"; + static const char fmt_id[] = "[id] g2core ID%37s\n"; -void hw_print_fb(nvObj_t *nv) { text_print(nv, fmt_fb);} // TYPE_FLOAT -void hw_print_fbs(nvObj_t *nv) { text_print(nv, fmt_fbs);} // TYPE_STRING -void hw_print_fbc(nvObj_t *nv) { text_print(nv, fmt_fbc);} // TYPE_STRING -void hw_print_fv(nvObj_t *nv) { text_print(nv, fmt_fv);} // TYPE_FLOAT -void hw_print_cv(nvObj_t *nv) { text_print(nv, fmt_cv);} // TYPE_FLOAT -void hw_print_hp(nvObj_t *nv) { text_print(nv, fmt_hp);} // TYPE_FLOAT -void hw_print_hv(nvObj_t *nv) { text_print(nv, fmt_hv);} // TYPE_FLOAT -void hw_print_id(nvObj_t *nv) { text_print(nv, fmt_id);} // TYPE_STRING + void hw_print_fb(nvObj_t *nv) { text_print(nv, fmt_fb);} // TYPE_FLOAT + void hw_print_fv(nvObj_t *nv) { text_print(nv, fmt_fv);} // TYPE_FLOAT + void hw_print_fbs(nvObj_t *nv) { text_print(nv, fmt_fbs);} // TYPE_STRING + void hw_print_fbc(nvObj_t *nv) { text_print(nv, fmt_fbc);} // TYPE_STRING + void hw_print_hp(nvObj_t *nv) { text_print(nv, fmt_hp);} // TYPE_STRING + void hw_print_hv(nvObj_t *nv) { text_print(nv, fmt_hv);} // TYPE_STRING + void hw_print_id(nvObj_t *nv) { text_print(nv, fmt_id);} // TYPE_STRING #endif //__TEXT_MODE diff --git a/g2core/board/printrboardg2/board_stepper.cpp b/g2core/board/printrboardg2/board_stepper.cpp old mode 100755 new mode 100644 index 0583e4e7..979c8fdc --- a/g2core/board/printrboardg2/board_stepper.cpp +++ b/g2core/board/printrboardg2/board_stepper.cpp @@ -1,9 +1,10 @@ /* * board_stepper.cpp - board-specific code for stepper.cpp + * For: /board/printrboardg2 * This file is part of the g2core project * - * Copyright (c) 2016 Alden S. Hart, Jr. - * Copyright (c) 2016 Robert Giseburt + * Copyright (c) 2016 - 2018 Alden S. Hart, Jr. + * Copyright (c) 2016 - 2018 Robert Giseburt * * This file ("the software") is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License, version 2 as published by the @@ -28,7 +29,7 @@ #include "board_stepper.h" -// These are identical to board_stepper.h, except for the word "extern" +// These are identical to board_stepper.h, except for the word "extern", and they have the initialization parameters StepDirStepper - motor_1{}; + motor_1{M1_STEP_POLARITY, M1_ENABLE_POLARITY}; StepDirStepper - motor_2{}; + motor_2{M2_STEP_POLARITY, M2_ENABLE_POLARITY}; StepDirStepper - motor_3{}; + motor_3{M3_STEP_POLARITY, M3_ENABLE_POLARITY}; StepDirStepper - motor_4{}; + motor_4{M4_STEP_POLARITY, M4_ENABLE_POLARITY}; // StepDirStepper< // Motate::kSocket5_StepPinNumber, @@ -72,7 +73,8 @@ StepDirStepper motor_5 {}; +// Motate::kSocket5_VrefPinNumber> +// motor_5 {M4_STEP_POLARITY, M4_ENABLE_POLARITY}; // StepDirStepper< // Motate::kSocket6_StepPinNumber, @@ -81,7 +83,8 @@ StepDirStepper motor_6 {}; +// Motate::kSocket6_VrefPinNumber> +// motor_6 {M4_STEP_POLARITY, M4_ENABLE_POLARITY}; Stepper* Motors[MOTORS] = {&motor_1, &motor_2, &motor_3, &motor_4}; diff --git a/g2core/board/printrboardg2/board_stepper.h b/g2core/board/printrboardg2/board_stepper.h old mode 100755 new mode 100644 index 25c5d01b..b511fa5a --- a/g2core/board/printrboardg2/board_stepper.h +++ b/g2core/board/printrboardg2/board_stepper.h @@ -1,9 +1,10 @@ /* * board_stepper.h - board-specific code for stepper.h + * For: /board/printrboardg2 * This file is part of the g2core project * - * Copyright (c) 2016 Alden S. Hart, Jr. - * Copyright (c) 2016 Robert Giseburt + * Copyright (c) 2016 - 2018 Alden S. Hart, Jr. + * Copyright (c) 2016 - 2018 Robert Giseburt * * This file ("the software") is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License, version 2 as published by the diff --git a/g2core/board/printrboardg2/board_xio.cpp b/g2core/board/printrboardg2/board_xio.cpp old mode 100755 new mode 100644 index 149dbc8d..f0d16379 --- a/g2core/board/printrboardg2/board_xio.cpp +++ b/g2core/board/printrboardg2/board_xio.cpp @@ -1,5 +1,6 @@ /* * board_xio.cpp - extended IO functions that are board-specific + * For: /board/printrboardg2 * This file is part of the g2core project * * Copyright (c) 2016 Alden S. Hart Jr. diff --git a/g2core/board/printrboardg2/board_xio.h b/g2core/board/printrboardg2/board_xio.h old mode 100755 new mode 100644 index 50b69107..0b6a1ec0 --- a/g2core/board/printrboardg2/board_xio.h +++ b/g2core/board/printrboardg2/board_xio.h @@ -1,5 +1,6 @@ /* * board_xio.h - extended IO functions that are board-specific + * For: /board/printrboardg2 * This file is part of the g2core project * * Copyright (c) 2016 Alden S. Hart Jr. diff --git a/g2core/board/printrboardg2/hardware.h b/g2core/board/printrboardg2/hardware.h index e62065ab..2425d1b9 100644 --- a/g2core/board/printrboardg2/hardware.h +++ b/g2core/board/printrboardg2/hardware.h @@ -1,11 +1,12 @@ /* * hardware.h - system hardware configuration - * THIS FILE IS HARDWARE PLATFORM SPECIFIC - ARM version + * For: /board/printrboardg2 + * THIS FILE IS HARDWARE PLATFORM SPECIFIC - ARM version * * This file is part of the g2core project * - * Copyright (c) 2013 - 2016 Alden S. Hart, Jr. - * Copyright (c) 2013 - 2016 Robert Giseburt + * Copyright (c) 2013 - 2018 Alden S. Hart, Jr. + * Copyright (c) 2013 - 2018 Robert Giseburt * * This file ("the software") is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License, version 2 as published by the @@ -36,40 +37,31 @@ /*--- Hardware platform enumerations ---*/ -enum hwPlatform { - HM_PLATFORM_NONE = 0, - HW_PLATFORM_TINYG_XMEGA, // TinyG code base on Xmega boards. - HW_PLATFORM_G2_DUE, // G2 code base on native Arduino Due - HW_PLATFORM_V9 // G2 code base on v9 boards -}; +#define G2CORE_HARDWARE_PLATFORM "printerboardg2" +#define G2CORE_HARDWARE_VERSION "c" -#define HW_VERSION_TINYGV6 6 -#define HW_VERSION_TINYGV7 7 -#define HW_VERSION_TINYGV8 8 - -#define HW_VERSION_TINYGV9I 4 -#define HW_VERSION_TINYGV9K 5 - - -/***** Axes, motors & PWM channels used by the application *****/ -// Axes, motors & PWM channels must be defines (not enums) so expressions like this: +/***** Motors & PWM channels supported by this hardware *****/ +// These must be defines (not enums) so expressions like this: // #if (MOTORS >= 6) will work -#define AXES 6 // number of axes supported in this version -#define HOMING_AXES 4 // number of axes that can be homed (assumes Zxyabc sequence) -#define MOTORS 4 // number of motors on the board -#define COORDS 6 // number of supported coordinate systems (index starts at 1) -#define PWMS 2 // number of supported PWM channels -#define TOOLS 32 // number of entries in tool table (index starts at 1) +#define MOTORS 4 // number of motors supported the hardware +#define PWMS 2 // number of PWM channels supported the hardware +/************************* + * Global System Defines * + *************************/ -//////////////////////////// -/////// ARM VERSION //////// -//////////////////////////// +#define MILLISECONDS_PER_TICK 1 // MS for system tick (systick * N) +#define SYS_ID_DIGITS 16 // actual digits in system ID (up to 16) +#define SYS_ID_LEN 24 // total length including dashes and NUL + +/************************* + * Motate Setup * + *************************/ #include "MotatePins.h" -#include "MotateTimers.h" // for TimerChanel<> and related... -#include "MotateServiceCall.h" // for ServiceCall<> +#include "MotateTimers.h" // for TimerChanel<> and related... +#include "MotateServiceCall.h" // for ServiceCall<> using Motate::TimerChannel; using Motate::ServiceCall; @@ -79,14 +71,6 @@ using Motate::Pin; using Motate::PWMOutputPin; using Motate::OutputPin; -/************************* - * Global System Defines * - *************************/ - -#define MILLISECONDS_PER_TICK 1 // MS for system tick (systick * N) -#define SYS_ID_DIGITS 12 // actual digits in system ID (up to 16) -#define SYS_ID_LEN 24 // total length including dashes and NUL - /************************************************************************************ **** ARM SAM3X8E SPECIFIC HARDWARE ************************************************* ************************************************************************************/ @@ -168,42 +152,44 @@ static OutputPin mist_enable_pin; * Function Prototypes (Common) * ********************************/ -void hardware_init(void); // master hardware init -stat_t hardware_periodic(); // callback from the main loop (time sensitive) +void hardware_init(void); // master hardware init +stat_t hardware_periodic(); // callback from the main loop (time sensitive) void hw_hard_reset(void); stat_t hw_flash(nvObj_t *nv); -stat_t hw_get_fbs(nvObj_t *nv); -stat_t hw_get_fbc(nvObj_t *nv); -stat_t hw_set_hv(nvObj_t *nv); -stat_t hw_get_id(nvObj_t *nv); - #define TEMPORARY_HAS_LEDS 1 stat_t _get_leds(nvObj_t *nv); stat_t _set_leds(nvObj_t *nv); +stat_t hw_get_fb(nvObj_t *nv); +stat_t hw_get_fv(nvObj_t *nv); +stat_t hw_get_hp(nvObj_t *nv); +stat_t hw_get_hv(nvObj_t *nv); +stat_t hw_get_fbs(nvObj_t *nv); +stat_t hw_get_fbc(nvObj_t *nv); +stat_t hw_get_id(nvObj_t *nv); + #ifdef __TEXT_MODE - void hw_print_fb(nvObj_t *nv); + void hw_print_fb(nvObj_t *nv); + void hw_print_fv(nvObj_t *nv); void hw_print_fbs(nvObj_t *nv); void hw_print_fbc(nvObj_t *nv); - void hw_print_fv(nvObj_t *nv); - void hw_print_cv(nvObj_t *nv); - void hw_print_hp(nvObj_t *nv); - void hw_print_hv(nvObj_t *nv); - void hw_print_id(nvObj_t *nv); + void hw_print_hp(nvObj_t *nv); + void hw_print_hv(nvObj_t *nv); + void hw_print_id(nvObj_t *nv); #else - #define hw_print_fb tx_print_stub + #define hw_print_fb tx_print_stub + #define hw_print_fv tx_print_stub #define hw_print_fbs tx_print_stub #define hw_print_fbc tx_print_stub - #define hw_print_fv tx_print_stub - #define hw_print_cv tx_print_stub - #define hw_print_hp tx_print_stub - #define hw_print_hv tx_print_stub - #define hw_print_id tx_print_stub + #define hw_print_hp tx_print_stub + #define hw_print_hv tx_print_stub + #define hw_print_id tx_print_stub #endif // __TEXT_MODE + #endif // end of include guard: HARDWARE_H_ONCE diff --git a/g2core/board/printrboardg2/motate_pin_assignments.h b/g2core/board/printrboardg2/motate_pin_assignments.h old mode 100755 new mode 100644 index 3ebdcc5a..bd7eb64d --- a/g2core/board/printrboardg2/motate_pin_assignments.h +++ b/g2core/board/printrboardg2/motate_pin_assignments.h @@ -1,9 +1,10 @@ /* - * motate_pin_assignments.h - pin assignments for the printrboardg2 boards + * motate_pin_assignments.h - pin assignments + * For: /board/printrboardg2 * This file is part of the g2core project * - * Copyright (c) 2013 - 2016 Robert Giseburt - * Copyright (c) 2013 - 2016 Alden S. Hart Jr. + * Copyright (c) 2013 - 2018 Robert Giseburt + * Copyright (c) 2013 - 2018 Alden S. Hart Jr. * * This file ("the software") is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License, version 2 as published by the diff --git a/g2core/board/printrboardg2/printrboardG2v3-pinout.h b/g2core/board/printrboardg2/printrboardG2v3-pinout.h old mode 100755 new mode 100644 index 429d3111..fa2b18c3 --- a/g2core/board/printrboardg2/printrboardG2v3-pinout.h +++ b/g2core/board/printrboardg2/printrboardG2v3-pinout.h @@ -1,9 +1,10 @@ /* * printrbaordG2v3-pinout.h - board pinout specification + * For: /board/printrboardg2 * This file is part of the g2core project * - * Copyright (c) 2013 - 2016 Robert Giseburt - * Copyright (c) 2013 - 2016 Alden S. Hart Jr. + * Copyright (c) 2013 - 2018 Robert Giseburt + * Copyright (c) 2013 - 2018 Alden S. Hart Jr. * * This file is part of the Motate Library. * diff --git a/g2core/board/sbv300/0_hardware.cpp b/g2core/board/sbv300/0_hardware.cpp old mode 100755 new mode 100644 index ee0cf7fd..d652ec60 --- a/g2core/board/sbv300/0_hardware.cpp +++ b/g2core/board/sbv300/0_hardware.cpp @@ -1,9 +1,10 @@ /* * hardware.cpp - general hardware support functions + * For: /board/sbv300 * This file is part of the g2core project * - * Copyright (c) 2010 - 2016 Alden S. Hart, Jr. - * Copyright (c) 2013 - 2016 Robert Giseburt + * Copyright (c) 2010 - 2018 Alden S. Hart, Jr. + * Copyright (c) 2013 - 2018 Robert Giseburt * * This file ("the software") is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License, version 2 as published by the @@ -94,15 +95,18 @@ void _get_id(char *id) ***********************************************************************************/ /* + * hw_get_fb() - get firmware build number + * hw_get_fv() - get firmware version number + * hw_get_hp() - get hardware platform string + * hw_get_hv() - get hardware version string * hw_get_fbs() - get firmware build string */ -stat_t hw_get_fbs(nvObj_t *nv) -{ - nv->valuetype = TYPE_STRING; - ritorno(nv_copy_string(nv, G2CORE_FIRMWARE_BUILD_STRING)); - return (STAT_OK); -} +stat_t hw_get_fb(nvObj_t *nv) { return (get_float(nv, cs.fw_build)); } +stat_t hw_get_fv(nvObj_t *nv) { return (get_float(nv, cs.fw_version)); } +stat_t hw_get_hp(nvObj_t *nv) { return (get_string(nv, G2CORE_HARDWARE_PLATFORM)); } +stat_t hw_get_hv(nvObj_t *nv) { return (get_string(nv, G2CORE_HARDWARE_VERSION)); } +stat_t hw_get_fbs(nvObj_t *nv) { return (get_string(nv, G2CORE_FIRMWARE_BUILD_STRING)); } /* * hw_get_fbc() - get configuration settings file @@ -146,15 +150,6 @@ stat_t hw_flash(nvObj_t *nv) return(STAT_OK); } -/* - * hw_set_hv() - set hardware version number - */ -stat_t hw_set_hv(nvObj_t *nv) -{ - return (STAT_OK); -} - - /*********************************************************************************** * TEXT MODE SUPPORT * Functions to print variables from the cfgArray table @@ -162,22 +157,20 @@ stat_t hw_set_hv(nvObj_t *nv) #ifdef __TEXT_MODE -static const char fmt_fb[] = "[fb] firmware build %18.2f\n"; -static const char fmt_fbs[] = "[fbs] firmware build \"%s\"\n"; -static const char fmt_fbc[] = "[fbc] firmware config \"%s\"\n"; -static const char fmt_fv[] = "[fv] firmware version%16.2f\n"; -static const char fmt_cv[] = "[cv] configuration version%11.2f\n"; -static const char fmt_hp[] = "[hp] hardware platform%15.2f\n"; -static const char fmt_hv[] = "[hv] hardware version%16.2f\n"; -static const char fmt_id[] = "[id] g2core ID%21s\n"; + static const char fmt_fb[] = "[fb] firmware build%18.2f\n"; + static const char fmt_fv[] = "[fv] firmware version%16.2f\n"; + static const char fmt_fbs[] = "[fbs] firmware build%34s\n"; + static const char fmt_fbc[] = "[fbc] firmware config%33s\n"; + static const char fmt_hp[] = "[hp] hardware platform%15s\n"; + static const char fmt_hv[] = "[hv] hardware version%13s\n"; + static const char fmt_id[] = "[id] g2core ID%37s\n"; -void hw_print_fb(nvObj_t *nv) { text_print(nv, fmt_fb);} // TYPE_FLOAT -void hw_print_fbs(nvObj_t *nv) { text_print(nv, fmt_fbs);} // TYPE_STRING -void hw_print_fbc(nvObj_t *nv) { text_print(nv, fmt_fbc);} // TYPE_STRING -void hw_print_fv(nvObj_t *nv) { text_print(nv, fmt_fv);} // TYPE_FLOAT -void hw_print_cv(nvObj_t *nv) { text_print(nv, fmt_cv);} // TYPE_FLOAT -void hw_print_hp(nvObj_t *nv) { text_print(nv, fmt_hp);} // TYPE_FLOAT -void hw_print_hv(nvObj_t *nv) { text_print(nv, fmt_hv);} // TYPE_FLOAT -void hw_print_id(nvObj_t *nv) { text_print(nv, fmt_id);} // TYPE_STRING + void hw_print_fb(nvObj_t *nv) { text_print(nv, fmt_fb);} // TYPE_FLOAT + void hw_print_fv(nvObj_t *nv) { text_print(nv, fmt_fv);} // TYPE_FLOAT + void hw_print_fbs(nvObj_t *nv) { text_print(nv, fmt_fbs);} // TYPE_STRING + void hw_print_fbc(nvObj_t *nv) { text_print(nv, fmt_fbc);} // TYPE_STRING + void hw_print_hp(nvObj_t *nv) { text_print(nv, fmt_hp);} // TYPE_STRING + void hw_print_hv(nvObj_t *nv) { text_print(nv, fmt_hv);} // TYPE_STRING + void hw_print_id(nvObj_t *nv) { text_print(nv, fmt_id);} // TYPE_STRING #endif //__TEXT_MODE diff --git a/g2core/board/sbv300/board_stepper.cpp b/g2core/board/sbv300/board_stepper.cpp old mode 100755 new mode 100644 index 0583e4e7..46a939d4 --- a/g2core/board/sbv300/board_stepper.cpp +++ b/g2core/board/sbv300/board_stepper.cpp @@ -1,9 +1,10 @@ /* * board_stepper.cpp - board-specific code for stepper.cpp + * For: /board/sbv300 * This file is part of the g2core project * - * Copyright (c) 2016 Alden S. Hart, Jr. - * Copyright (c) 2016 Robert Giseburt + * Copyright (c) 2016 - 2018 Alden S. Hart, Jr. + * Copyright (c) 2016 - 2018 Robert Giseburt * * This file ("the software") is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License, version 2 as published by the @@ -28,7 +29,7 @@ #include "board_stepper.h" -// These are identical to board_stepper.h, except for the word "extern" +// These are identical to board_stepper.h, except for the word "extern", and they have the initialization parameters StepDirStepper - motor_1{}; + motor_1{M1_STEP_POLARITY, M1_ENABLE_POLARITY}; StepDirStepper - motor_2{}; + motor_2{M2_STEP_POLARITY, M2_ENABLE_POLARITY}; StepDirStepper - motor_3{}; + motor_3{M3_STEP_POLARITY, M3_ENABLE_POLARITY}; StepDirStepper - motor_4{}; + motor_4{M4_STEP_POLARITY, M4_ENABLE_POLARITY}; // StepDirStepper< // Motate::kSocket5_StepPinNumber, @@ -72,7 +73,8 @@ StepDirStepper motor_5 {}; +// Motate::kSocket5_VrefPinNumber> +// motor_5 {M5_STEP_POLARITY, M5_ENABLE_POLARITY}; // StepDirStepper< // Motate::kSocket6_StepPinNumber, @@ -81,7 +83,8 @@ StepDirStepper motor_6 {}; +// Motate::kSocket6_VrefPinNumber> +// motor_6 {M6_STEP_POLARITY, M6_ENABLE_POLARITY}; Stepper* Motors[MOTORS] = {&motor_1, &motor_2, &motor_3, &motor_4}; diff --git a/g2core/board/sbv300/board_stepper.h b/g2core/board/sbv300/board_stepper.h old mode 100755 new mode 100644 index 25c5d01b..f8e13687 --- a/g2core/board/sbv300/board_stepper.h +++ b/g2core/board/sbv300/board_stepper.h @@ -1,9 +1,10 @@ /* * board_stepper.h - board-specific code for stepper.h + * For: /board/sbv300 * This file is part of the g2core project * - * Copyright (c) 2016 Alden S. Hart, Jr. - * Copyright (c) 2016 Robert Giseburt + * Copyright (c) 2016 - 2018 Alden S. Hart, Jr. + * Copyright (c) 2016 - 2018 Robert Giseburt * * This file ("the software") is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License, version 2 as published by the diff --git a/g2core/board/sbv300/board_xio.cpp b/g2core/board/sbv300/board_xio.cpp old mode 100755 new mode 100644 index c48356be..bf7d0b86 --- a/g2core/board/sbv300/board_xio.cpp +++ b/g2core/board/sbv300/board_xio.cpp @@ -1,5 +1,6 @@ /* * board_xio.cpp - extended IO functions that are board-specific + * For: /board/sbv300 * This file is part of the g2core project * * Copyright (c) 2016 Alden S. Hart Jr. diff --git a/g2core/board/sbv300/board_xio.h b/g2core/board/sbv300/board_xio.h old mode 100755 new mode 100644 index 04af83fd..aa6d5d38 --- a/g2core/board/sbv300/board_xio.h +++ b/g2core/board/sbv300/board_xio.h @@ -1,5 +1,6 @@ /* * board_xio.h - extended IO functions that are board-specific + * For: /board/sbv300 * This file is part of the g2core project * * Copyright (c) 2016 Alden S. Hart Jr. diff --git a/g2core/board/sbv300/hardware.h b/g2core/board/sbv300/hardware.h index 7936eb34..5e84848b 100644 --- a/g2core/board/sbv300/hardware.h +++ b/g2core/board/sbv300/hardware.h @@ -1,11 +1,12 @@ /* * hardware.h - system hardware configuration - * THIS FILE IS HARDWARE PLATFORM SPECIFIC - ARM version + * For: /board/sbv300 + * THIS FILE IS HARDWARE PLATFORM SPECIFIC - ARM version * * This file is part of the g2core project * - * Copyright (c) 2013 - 2016 Alden S. Hart, Jr. - * Copyright (c) 2013 - 2016 Robert Giseburt + * Copyright (c) 2013 - 2018 Alden S. Hart, Jr. + * Copyright (c) 2013 - 2018 Robert Giseburt * * This file ("the software") is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License, version 2 as published by the @@ -36,39 +37,31 @@ /*--- Hardware platform enumerations ---*/ -enum hwPlatform { - HM_PLATFORM_NONE = 0, - HW_PLATFORM_TINYG_XMEGA, // TinyG code base on Xmega boards. - HW_PLATFORM_G2_DUE, // G2 code base on native Arduino Due - HW_PLATFORM_V9 // G2 code base on v9 boards -}; +#define G2CORE_HARDWARE_PLATFORM "sbv300" +#define G2CORE_HARDWARE_VERSION "k" -#define HW_VERSION_TINYGV6 6 -#define HW_VERSION_TINYGV7 7 -#define HW_VERSION_TINYGV8 8 - -#define HW_VERSION_TINYGV9I 4 -#define HW_VERSION_TINYGV9K 5 - - -/***** Axes, motors & PWM channels used by the application *****/ -// Axes, motors & PWM channels must be defines (not enums) so expressions like this: +/***** Motors & PWM channels supported by this hardware *****/ +// These must be defines (not enums) so expressions like this: // #if (MOTORS >= 6) will work -#define AXES 6 // number of axes supported in this version -#define HOMING_AXES 4 // number of axes that can be homed (assumes Zxyabc sequence) -#define MOTORS 4 // number of motors on the board -#define COORDS 6 // number of supported coordinate systems (index starts at 1) -#define PWMS 2 // number of supported PWM channels -#define TOOLS 32 // number of entries in tool table (index starts at 1) +#define MOTORS 4 // number of motors supported the hardware +#define PWMS 2 // number of PWM channels supported the hardware -//////////////////////////// -/////// ARM SETTINGS /////// -//////////////////////////// +/************************* + * Global System Defines * + *************************/ + +#define MILLISECONDS_PER_TICK 1 // MS for system tick (systick * N) +#define SYS_ID_DIGITS 16 // actual digits in system ID (up to 16) +#define SYS_ID_LEN 24 // total length including dashes and NUL + +/************************* + * Motate Setup * + *************************/ #include "MotatePins.h" -#include "MotateTimers.h" // for TimerChanel<> and related... -#include "MotateServiceCall.h" // for ServiceCall<> +#include "MotateTimers.h" // for TimerChanel<> and related... +#include "MotateServiceCall.h" // for ServiceCall<> using Motate::TimerChannel; using Motate::ServiceCall; @@ -78,14 +71,6 @@ using Motate::Pin; using Motate::PWMOutputPin; using Motate::OutputPin; -/************************* - * Global System Defines * - *************************/ - -#define MILLISECONDS_PER_TICK 1 // MS for system tick (systick * N) -#define SYS_ID_DIGITS 12 // actual digits in system ID (up to 16) -#define SYS_ID_LEN 24 // total length including dashes and NUL - /************************************************************************************ **** ARM SAM3X8E SPECIFIC HARDWARE ************************************************* ************************************************************************************/ @@ -172,18 +157,20 @@ stat_t hardware_periodic(); // callback from the main loop (time sensitive) void hw_hard_reset(void); stat_t hw_flash(nvObj_t *nv); +stat_t hw_get_fb(nvObj_t *nv); +stat_t hw_get_fv(nvObj_t *nv); +stat_t hw_get_hp(nvObj_t *nv); +stat_t hw_get_hv(nvObj_t *nv); stat_t hw_get_fbs(nvObj_t *nv); stat_t hw_get_fbc(nvObj_t *nv); -stat_t hw_set_hv(nvObj_t *nv); stat_t hw_get_id(nvObj_t *nv); #ifdef __TEXT_MODE void hw_print_fb(nvObj_t *nv); + void hw_print_fv(nvObj_t *nv); void hw_print_fbs(nvObj_t *nv); void hw_print_fbc(nvObj_t *nv); - void hw_print_fv(nvObj_t *nv); - void hw_print_cv(nvObj_t *nv); void hw_print_hp(nvObj_t *nv); void hw_print_hv(nvObj_t *nv); void hw_print_id(nvObj_t *nv); @@ -191,10 +178,9 @@ stat_t hw_get_id(nvObj_t *nv); #else #define hw_print_fb tx_print_stub + #define hw_print_fv tx_print_stub #define hw_print_fbs tx_print_stub #define hw_print_fbc tx_print_stub - #define hw_print_fv tx_print_stub - #define hw_print_cv tx_print_stub #define hw_print_hp tx_print_stub #define hw_print_hv tx_print_stub #define hw_print_id tx_print_stub diff --git a/g2core/board/sbv300/motate_pin_assignments.h b/g2core/board/sbv300/motate_pin_assignments.h old mode 100755 new mode 100644 index a77176a4..8d61f4d5 --- a/g2core/board/sbv300/motate_pin_assignments.h +++ b/g2core/board/sbv300/motate_pin_assignments.h @@ -1,9 +1,10 @@ /* - * motate_pin_assignments.h - pin assignments for the shopbot sbv300 boards + * motate_pin_assignments.h - pin assignments + * For: /board/sbv300 * This file is part of the g2core project * - * Copyright (c) 2013 - 2016 Robert Giseburt - * Copyright (c) 2013 - 2016 Alden S. Hart Jr. + * Copyright (c) 2013 - 2018 Robert Giseburt + * Copyright (c) 2013 - 2018 Alden S. Hart Jr. * * This file is part of the Motate Library. * diff --git a/g2core/board/sbv300/sbv300-pinout.h b/g2core/board/sbv300/sbv300-pinout.h old mode 100755 new mode 100644 index f2667ec5..07b0bf41 --- a/g2core/board/sbv300/sbv300-pinout.h +++ b/g2core/board/sbv300/sbv300-pinout.h @@ -1,9 +1,10 @@ /* * sbv300-pinout.h - board pinout specification + * For: /board/sbv300 * This file is part of the g2core project * - * Copyright (c) 2013 - 2016 Robert Giseburt - * Copyright (c) 2013 - 2016 Alden S. Hart Jr. + * Copyright (c) 2013 - 2018 Robert Giseburt + * Copyright (c) 2013 - 2018 Alden S. Hart Jr. * * This file ("the software") is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License, version 2 as published by the diff --git a/g2core/boards.mk b/g2core/boards.mk index 5cb8ba64..5fbb8bda 100644 --- a/g2core/boards.mk +++ b/g2core/boards.mk @@ -24,7 +24,6 @@ # To choose a CONFIG but apply it to a different BOARD: # make CONFIG=PrintrbotPlus BOARD=g2ref-a - ########## # V9-based configs: @@ -109,6 +108,12 @@ ifeq ("$(CONFIG)","sbv300") SETTINGS_FILE="settings_shopbot_sbv300.h" endif +ifeq ("$(CONFIG)","ShopbotTestV9") + ifeq ("$(BOARD)","NONE") + BOARD=g2v9k + endif + SETTINGS_FILE="settings_shopbot_test.h" +endif ########## # PrintrBot configs: diff --git a/g2core/canonical_machine.cpp b/g2core/canonical_machine.cpp index 3d354a56..7585c95e 100644 --- a/g2core/canonical_machine.cpp +++ b/g2core/canonical_machine.cpp @@ -2,8 +2,8 @@ * canonical_machine.cpp - rs274/ngc canonical machine. * This file is part of the g2core project * - * Copyright (c) 2010 - 2017 Alden S Hart, Jr. - * Copyright (c) 2014 - 2017 Robert Giseburt + * Copyright (c) 2010 - 2019 Alden S Hart, Jr. + * Copyright (c) 2014 - 2019 Robert Giseburt * * This file ("the software") is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License, version 2 as published by the @@ -37,9 +37,9 @@ * * Useful reference for doing C callbacks http://www.newty.de/fpt/fpt.html * - * There are 3 temporal contexts for system state: - * - The gcode model in the canonical machine (the MODEL context, held in gm) - * - The gcode model used by the planner (PLANNER context, held in bf's and mm) + * There are 3 layered contexts for dynamic system state ("gcode model"): + * - The gcode model in the canonical machine (the MODEL context, held in cm->gm) + * - The gcode model used by the planner (PLANNER context, held in mp & its buffers) * - The gcode model used during motion for reporting (RUNTIME context, held in mr) * * It's a bit more complicated than this. The 'gm' struct contains the core Gcode model @@ -86,8 +86,9 @@ * and have no buffer. */ -#include "g2core.h" // #1 -#include "config.h" // #2 +#include "g2core.h" // #1 +#include "config.h" // #2 +#include "gcode.h" // #3 #include "canonical_machine.h" #include "hardware.h" #include "controller.h" @@ -111,11 +112,20 @@ #include "xio.h" // for serial queue flush #include "kinematics.h" // for forward kinematics in cm_cycle_start -/*********************************************************************************** - **** STRUCTURE ALLOCATIONS ******************************************************** - ***********************************************************************************/ +/**************************************************************************************** + **** CM GLOBALS & STRUCTURE ALLOCATIONS ************************************************ + ****************************************************************************************/ -cmSingleton_t cm; // canonical machine controller singleton +cmMachine_t *cm; // pointer to active canonical machine +cmMachine_t cm1; // canonical machine primary machine +cmMachine_t cm2; // canonical machine secondary machine +cmToolTable_t tt; // global tool table + +/**************************************************************************************** + **** GENERIC STATIC FUNCTIONS AND VARIABLES ******************************************** + ****************************************************************************************/ + +static int8_t _axis(const nvObj_t *nv); // return axis number from token/group in nv /* * _hold_input_handler - a gpioDigitalInputHandler to capture pin change events @@ -125,7 +135,15 @@ gpioDigitalInputHandler _hold_input_handler { [&](const bool state, const inputEdgeFlag edge, const uint8_t triggering_pin_number) { if (edge != INPUT_EDGE_LEADING) { return false; } - cm_start_hold(); + // any action = d_in[triggering_pin_number]->getAction(); + // if (action == INPUT_ACTION_STOP) { + // cm_request_feedhold(FEEDHOLD_TYPE_HOLD, FEEDHOLD_EXIT_STOP); + // } + // if (action == INPUT_ACTION_FAST_STOP) { + // cm_request_feedhold(FEEDHOLD_TYPE_HOLD, FEEDHOLD_EXIT_STOP); + // } + + cm_request_feedhold(FEEDHOLD_TYPE_HOLD, FEEDHOLD_EXIT_STOP); return false; // allow others to see this notice }, @@ -141,7 +159,7 @@ gpioDigitalInputHandler _halt_input_handler { [&](const bool state, const inputEdgeFlag edge, const uint8_t triggering_pin_number) { if (edge != INPUT_EDGE_LEADING) { return false; } - cm_halt_all(); + cm_halt(); // hard stop, including spindle, coolant and heaters return false; // allow others to see this notice }, @@ -202,115 +220,199 @@ gpioDigitalInputHandler _reset_input_handler { nullptr // next - nullptr to start with }; -/*********************************************************************************** - **** GENERIC STATIC FUNCTIONS AND VARIABLES *************************************** - ***********************************************************************************/ +/**************************************************************************************** + **** CODE ****************************************************************************** + ****************************************************************************************/ -// command execution callbacks from planner queue -static void _exec_offset(float *value, bool *flag); -static void _exec_change_tool(float *value, bool *flag); -static void _exec_select_tool(float *value, bool *flag); -static void _exec_absolute_origin(float *value, bool *flag); -static void _exec_program_finalize(float *value, bool *flag); +/**************************************************************************************** + **** Initialization ******************************************************************** + ****************************************************************************************/ +/* + * canonical_machine_inits() - combined cm inits + * canonical_machine_init() - initialize cm struct + * canonical_machine_reset() - apply startup settings or reset to startup + * canonical_machine_reset_rotation() + */ -static int8_t _get_axis(const index_t index); +void canonical_machine_inits() +{ + planner_init(&mp1, &mr1, mp1_queue, PLANNER_QUEUE_SIZE); + planner_init(&mp2, &mr2, mp2_queue, SECONDARY_QUEUE_SIZE); + canonical_machine_init(&cm1, &mp1); // primary canonical machine + canonical_machine_init(&cm2, &mp2); // secondary canonical machine + cm = &cm1; // set global canonical machine pointer to primary machine + mp = &mp1; // set global pointer to the primary planner + mr = &mr1; // and primary runtime -/*********************************************************************************** - **** CODE ************************************************************************* - ***********************************************************************************/ + din_handlers[INPUT_ACTION_STOP].registerHandler(&_hold_input_handler); + din_handlers[INPUT_ACTION_FAST_STOP].registerHandler(&_hold_input_handler); + din_handlers[INPUT_ACTION_HALT].registerHandler(&_halt_input_handler); + din_handlers[INPUT_ACTION_ALARM].registerHandler(&_alarm_input_handler); + din_handlers[INPUT_ACTION_PANIC].registerHandler(&_panic_input_handler); + din_handlers[INPUT_ACTION_RESET].registerHandler(&_reset_input_handler); +} -/************************************* - * Internal getters and setters * - * Canonical Machine State functions * - *************************************/ +void canonical_machine_init(cmMachine_t *_cm, void *_mp) +{ + // Note cm* was assignd in main() + // If you can assume all memory has been zeroed by a hard reset you don't need this code: + memset(_cm, 0, sizeof(cmMachine_t)); // do not reset canonicalMachine once it's been initialized + memset(&_cm->gm, 0, sizeof(GCodeState_t)); // clear all values, pointers and status + + canonical_machine_init_assertions(_cm); // establish assertions + cm_arc_init(_cm); // setup arcs. Note: spindle and coolant inits are independent + _cm->mp = _mp; // point to associated planner + _cm->am = MODEL; // setup initial Gcode model pointer +} + +// *** Note: Run canonical_machine_init and profile initializations beforehand *** +void canonical_machine_reset(cmMachine_t *_cm) +{ + // reset canonical machine assertions + canonical_machine_init_assertions(_cm); + + // set canonical machine gcode defaults + cm_set_units_mode(cm->default_units_mode); + cm_set_coord_system(cm->default_coord_system); // NB: queues a block to the planner with the coordinates + cm_select_plane(cm->default_select_plane); + cm_set_path_control(MODEL, cm->default_path_control); + cm_set_distance_mode(cm->default_distance_mode); + cm_set_arc_distance_mode(INCREMENTAL_DISTANCE_MODE); // always the default + cm_set_feed_rate_mode(UNITS_PER_MINUTE_MODE); // always the default + cm_reset_overrides(); // set overrides to initial conditions + + // NOTE: Should unhome axes here + _cm->homing_state = HOMING_NOT_HOMED; + + // reset request state and flags + _cm->queue_flush_state = QUEUE_FLUSH_OFF; + _cm->cycle_start_state = CYCLE_START_OFF; + _cm->job_kill_state = JOB_KILL_OFF; + _cm->limit_requested = 0; // resets switch closures that occurred during initialization + _cm->safety_interlock_disengaged = 0; // ditto + _cm->safety_interlock_reengaged = 0; // ditto + _cm->shutdown_requested = 0; // ditto + _cm->request_interlock = false; + _cm->request_interlock_exit = false; + + // set initial state and signal that the machine is ready for action + _cm->cycle_type = CYCLE_NONE; + _cm->motion_state = MOTION_STOP; + _cm->hold_state = FEEDHOLD_OFF; + // _cm->esc_boot_timer.set(???); // no longer used? + _cm->gmx.block_delete_switch = true; + _cm->gm.motion_mode = MOTION_MODE_CANCEL_MOTION_MODE; // never start in a motion mode + _cm->machine_state = MACHINE_READY; + + cm_operation_init(); // reset operations runner + + canonical_machine_reset_rotation(_cm); + memset(&_cm->probe_state, 0, sizeof(cmProbeState)*PROBES_STORED); + memset(&_cm->probe_results, 0, sizeof(float)*PROBES_STORED*AXES); +} + +void canonical_machine_reset_rotation(cmMachine_t *_cm) { + + // Make it an identity matrix for no rotation + memset(&_cm->rotation_matrix, 0, sizeof(float)*3*3); + _cm->rotation_matrix[0][0] = 1.0; + _cm->rotation_matrix[1][1] = 1.0; + _cm->rotation_matrix[2][2] = 1.0; + + // Separately handle a z-offset so that the new plane maintains a consistent + // distance from the old one. We only need z, since we are rotating to the z axis. + _cm->rotation_z_offset = 0.0; +} + +/**************************************************************************************** + * canonical_machine_init_assertions() + * canonical_machine_test_assertions() - test assertions, return error code if violation exists + */ + +void canonical_machine_init_assertions(cmMachine_t *_cm) +{ + _cm->magic_start = MAGICNUM; + _cm->magic_end = MAGICNUM; + _cm->gmx.magic_start = MAGICNUM; + _cm->gmx.magic_end = MAGICNUM; + _cm->arc.magic_start = MAGICNUM; + _cm->arc.magic_end = MAGICNUM; +} + +stat_t canonical_machine_test_assertions(cmMachine_t *_cm) +{ + if ((BAD_MAGIC(_cm->magic_start)) || (BAD_MAGIC(_cm->magic_end)) || + (BAD_MAGIC(_cm->gmx.magic_start)) || (BAD_MAGIC(_cm->gmx.magic_end)) || + (BAD_MAGIC(_cm->arc.magic_start)) || (BAD_MAGIC(_cm->arc.magic_end))) { + return(cm_panic(STAT_CANONICAL_MACHINE_ASSERTION_FAILURE, "canonical_machine_test_assertions()")); + } + return (STAT_OK); +} + +/**************************************************************************************** + **** Canonical Machine State Management ************************************************ + ****************************************************************************************/ /* * cm_set_motion_state() - adjusts active model pointer as well */ void cm_set_motion_state(const cmMotionState motion_state) { - cm.motion_state = motion_state; - - switch (motion_state) { - case (MOTION_STOP): { ACTIVE_MODEL = MODEL; break; } - case (MOTION_PLANNING): { ACTIVE_MODEL = RUNTIME; break; } - case (MOTION_RUN): { ACTIVE_MODEL = RUNTIME; break; } - case (MOTION_HOLD): { ACTIVE_MODEL = RUNTIME; break; } - } + cm->motion_state = motion_state; + ACTIVE_MODEL = ((motion_state == MOTION_STOP) ? MODEL : RUNTIME); } /* * cm_get_machine_state() * cm_get_motion_state() - * cm_get_cycle_state() + * cm_get_cycle_type() * cm_get_hold_state() * cm_get_homing_state() + * cm_get_probe_state() */ -cmMachineState cm_get_machine_state() { return cm.machine_state;} -cmCycleState cm_get_cycle_state() { return cm.cycle_state;} -cmMotionState cm_get_motion_state() { return cm.motion_state;} -cmFeedholdState cm_get_hold_state() { return cm.hold_state;} -cmHomingState cm_get_homing_state() { return cm.homing_state;} +cmMachineState cm_get_machine_state() { return cm->machine_state;} +cmCycleType cm_get_cycle_type() { return cm->cycle_type;} +cmMotionState cm_get_motion_state() { return cm->motion_state;} +cmFeedholdState cm_get_hold_state() { return cm->hold_state;} +cmHomingState cm_get_homing_state() { return cm->homing_state;} +cmProbeState cm_get_probe_state() { return cm->probe_state[0];} /* * cm_get_combined_state() - combines raw states into something a user might want to see - * - * Note: - * On issuing a gcode command we call cm_cycle_start() before the motion gets queued. We don't go - * to MOTION_RUN until the command is executed by mp_exec_aline(), planned, queued, and started. - * So MOTION_STOP must actually return COMBINED_RUN to address this case, even though under some - * circumstances it might actually be an exception case. Therefore this assertion isn't valid: - * cm_panic(STAT_STATE_MANAGEMENT_ASSERTION_FAILURE, "mots2"));//"mots is stop but machine is in cycle" - * return (COMBINED_PANIC); */ -cmCombinedState cm_get_combined_state() +cmCombinedState cm_get_combined_state(cmMachine_t *_cm) { - if (cm.machine_state <= MACHINE_PROGRAM_END) { // replaces first 5 cm.machine_state cases - return ((cmCombinedState)cm.machine_state); //...where MACHINE_xxx == COMBINED_xxx - } - switch(cm.machine_state) { - case MACHINE_INTERLOCK: { return (COMBINED_INTERLOCK); } - case MACHINE_SHUTDOWN: { return (COMBINED_SHUTDOWN); } - case MACHINE_PANIC: { return (COMBINED_PANIC); } + switch(_cm->machine_state) { + case MACHINE_INITIALIZING: + case MACHINE_READY: + case MACHINE_ALARM: + case MACHINE_PROGRAM_STOP: + case MACHINE_PROGRAM_END: { return ((cmCombinedState)_cm->machine_state); } + case MACHINE_INTERLOCK: { return (COMBINED_INTERLOCK); } + case MACHINE_SHUTDOWN: { return (COMBINED_SHUTDOWN); } + case MACHINE_PANIC: { return (COMBINED_PANIC); } case MACHINE_CYCLE: { - switch(cm.cycle_state) { - case CYCLE_HOMING: { return (COMBINED_HOMING); } - case CYCLE_PROBE: { return (COMBINED_PROBE); } - case CYCLE_JOG: { return (COMBINED_JOG); } - case CYCLE_MACHINING: case CYCLE_OFF: { - switch(cm.motion_state) { - case MOTION_STOP: { return (COMBINED_RUN); } // See NOTE_1, above - case MOTION_PLANNING: { return (COMBINED_RUN); } - case MOTION_RUN: { return (COMBINED_RUN); } - case MOTION_HOLD: { return (COMBINED_HOLD); } - default: { - cm_panic(STAT_STATE_MANAGEMENT_ASSERTION_FAILURE, "cm_get_combined_state() mots bad");// "mots has impossible value" - return (COMBINED_PANIC); - } - } - } - default: { - cm_panic(STAT_STATE_MANAGEMENT_ASSERTION_FAILURE, "cm_get_combined_state() cycs bad"); // "cycs has impossible value" - return (COMBINED_PANIC); - } + switch(_cm->cycle_type) { + case CYCLE_NONE: { break; } // CYCLE_NONE cannot ever get here + case CYCLE_MACHINING: { return (_cm->hold_state == FEEDHOLD_OFF ? COMBINED_RUN : COMBINED_HOLD); } + case CYCLE_HOMING: { return (COMBINED_HOMING); } + case CYCLE_PROBE: { return (COMBINED_PROBE); } + case CYCLE_JOG: { return (COMBINED_JOG); } } } - default: { - cm_panic(STAT_STATE_MANAGEMENT_ASSERTION_FAILURE, "cm_get_combined_state() macs bad"); // "macs has impossible value" - return (COMBINED_PANIC); - } } + cm_panic(STAT_STATE_MANAGEMENT_ASSERTION_FAILURE, "cm_get_combined_state() undefined state"); + return (COMBINED_PANIC); } -/*********************************** - * Model State Getters and Setters * - ***********************************/ - +/**************************************************************************************** + **** Model State Getters and Setters *************************************************** + ****************************************************************************************/ /* These getters and setters will work on any gm model with inputs: - * MODEL (GCodeState_t *)&cm.gm // absolute pointer from canonical machine gm model - * PLANNER (GCodeState_t *)&bf->gm // relative to buffer *bf is currently pointing to - * RUNTIME (GCodeState_t *)&mr.gm // absolute pointer from runtime mm struct - * ACTIVE_MODEL cm.am // active model pointer is maintained by state management + * MODEL (GCodeState_t *)&cm->gm // absolute pointer from canonical machine gm model + * RUNTIME (GCodeState_t *)&mr->gm // absolute pointer from runtime mm struct + * ACTIVE_MODEL cm->am // active model pointer is maintained by state management */ + uint32_t cm_get_linenum(const GCodeState_t *gcode_state) { return gcode_state->linenum;} cmMotionMode cm_get_motion_mode(const GCodeState_t *gcode_state) { return gcode_state->motion_mode;} uint8_t cm_get_coord_system(const GCodeState_t *gcode_state) { return gcode_state->coord_system;} @@ -321,7 +423,7 @@ uint8_t cm_get_distance_mode(const GCodeState_t *gcode_state) { return gcode_sta uint8_t cm_get_arc_distance_mode(const GCodeState_t *gcode_state) { return gcode_state->arc_distance_mode;} uint8_t cm_get_feed_rate_mode(const GCodeState_t *gcode_state) { return gcode_state->feed_rate_mode;} uint8_t cm_get_tool(const GCodeState_t *gcode_state) { return gcode_state->tool;} -uint8_t cm_get_block_delete_switch() { return cm.gmx.block_delete_switch;} +uint8_t cm_get_block_delete_switch() { return cm->gmx.block_delete_switch;} uint8_t cm_get_runtime_busy() { return (mp_get_runtime_busy());} float cm_get_feed_rate(const GCodeState_t *gcode_state) { return gcode_state->feed_rate;} @@ -338,145 +440,143 @@ void cm_set_tool_number(GCodeState_t *gcode_state, const uint8_t tool) void cm_set_absolute_override(GCodeState_t *gcode_state, const uint8_t absolute_override) { gcode_state->absolute_override = (cmAbsoluteOverride)absolute_override; - cm_set_work_offsets(MODEL); // must reset offsets if you change absolute override + cm_set_display_offsets(MODEL); // must reset offsets if you change absolute override } -void cm_set_model_linenum(const uint32_t linenum) +void cm_set_model_linenum(int32_t linenum) { - cm.gm.linenum = linenum; // you must first set the model line number, + if ((linenum < 0) || (linenum > MAX_LINENUM)) { + linenum = 0; + rpt_exception(STAT_INPUT_VALUE_RANGE_ERROR, "line number > 2B or negative; set to zero"); + } + cm->gm.linenum = linenum; // you must first set the model line number, nv_add_object((const char *)"n"); // then add the line number to the nv list } +/* + * cm_check_linenum() - Check line number for Marlin protocol + */ + stat_t cm_check_linenum() { - if (cm.gmx.last_line_number != cm.gm.linenum) { - _debug_trap("line number out of sequence"); + if (cm->gmx.last_line_number != cm->gm.linenum) { + debug_trap("line number out of sequence"); return STAT_LINE_NUMBER_OUT_OF_SEQUENCE; } - cm.gmx.last_line_number = cm.gm.linenum; + cm->gmx.last_line_number = cm->gm.linenum; return STAT_OK; } -/*********************************************************************************** +/**************************************************************************************** * COORDINATE SYSTEMS AND OFFSETS * Functions to get, set and report coordinate systems and work offsets * These functions are not part of the NIST defined functions - ***********************************************************************************/ + ****************************************************************************************/ /* + * cm_get_combined_offset() - return the combined offsets for an axis (G53-G59, G92, Tools) + * cm_get_display_offset() - return the current display offset from pecified Gcode model + * cm_set_display_offsets() - capture combined offsets from the model into absolute values + * in the active Gcode dynamic model + * * Notes on Coordinate System and Offset functions * * All positional information in the canonical machine is kept as absolute coords and in - * canonical units (mm). The offsets are only used to translate in and out of canonical form - * during interpretation and response. + * canonical units (mm, mm/min). The offsets are only used to translate in and out of + * canonical form during incoming processing, and for displays in responses. * - * Managing the coordinate systems & offsets is somewhat complicated. The following affect offsets: - * - coordinate system selected. 1-9 correspond to G54-G59 - * - absolute override: forces current move to be interpreted in machine coordinates: G53 (system 0) - * - G92 offsets are added "on top of" the coord system offsets -- if origin_offset_enable == true - * - G28 and G30 moves; these are run in absolute coordinates + * Managing coordinate systems & offsets is somewhat complicated. The following affect offsets: + * - coordinate system selected. 1-6 correspond to G54-G59 + * - G92 offsets are added "on top of" coord system offsets -- if g92_offset_enable == true + * - tool offsets are also accounted for + * - absolute override (G53) * - * The offsets themselves are considered static, are kept in cm, and are supposed to be persistent. + * Position displays {pos:n} are always in work coordinates aka using 'display' offsets + * - position displays are assembled by applying all active offsets to the current machine position + * - an absolute override forces current move to be interpreted in machine coordinates: G53 (system 0) + * - G53 is an explicit absolute override requested by the program, so displays in absolute coords + * - G28 and G30 moves are run in absolute coordinates but display using current offsets + * - Probing also has a very short move that behaves this way * - * To reduce complexity and data load the following is done: - * - Full data for coordinates/offsets is only accessible by the canonical machine, not the downstream - * - A fully resolved set of coord and G92 offsets, with per-move exceptions can be captured as "work_offsets" - * - The core gcode context (gm) only knows about the active coord system and the work offsets + * The offsets themselves are "the truth". These are: + * - cm.coord_offset[coord][axis] coordinate offsets for G53-G59, by axis - persistent + * - cm.tool_offset[axis] offsets for currently selected and active tool - persistent + * - cm.gmx.g92_offset[axis] G92 origin offset. Not persistent + * + * - cm_get_combined_offset() puts the above together to provide a combined, active offset. + * G92 offsets are only included if g92 is active (gmx.g92_offset_enable == true) + * + * Display offsets + * *** Display offsets are for display only and CANNOT be used to set positions *** + * - cm_set_display_offsets() writes combined offsets to cm.gm.display_offset[] + * - cm_set_display_offsets() should be called every time underlying data would cause a change + * - cm_set_display_offsets() takes absolute override display rules into account + * - Use cm_get_display_offset() to return the display offset value + */ +/* Absolute Override is the Gcode G53 convention to allow one and only one Gcode block + * to be run in absolute coordinates, regardless of coordinate offsets, G92 offsets, and + * tool offsets. See cmAbsoluteOverride for enumerations. + * + * - If absolute_override is set to ABSOLUTE_OVERRIDE_ON_DISPLAY_WITH_OFFSETS + * move will run in absolute coordinates but POS will display using current offsets. + * This is to support G28 and G30 return moves and other moves that run in absolute + * override mode but may want position to be reported using all current offsets + * + * - If absolute_override is set to ABSOLUTE_OVERRIDE_ON_DISPLAY_WITH_NO_OFFSETS + * move will run in absolute coordinates and POS will display using no offsets. */ -/* - * cm_get_active_coord_offset() - return the currently active coordinate offset for an axis - * - * Takes G5x, G92 and absolute override into account to return the active offset for this move - * - * This function is typically used to evaluate and set offsets, as opposed to cm_get_work_offset() - * which merely returns what's in the work_offset[] array. - */ - -float cm_get_active_coord_offset(const uint8_t axis) +float cm_get_combined_offset(const uint8_t axis) { - if (cm.gm.absolute_override == ABSOLUTE_OVERRIDE_ON) { // no offset if in absolute override mode - return (0.0); + if (cm->gm.absolute_override >= ABSOLUTE_OVERRIDE_ON_DISPLAY_WITH_OFFSETS) { + return (0); } - float offset = cm.offset[cm.gm.coord_system][axis] + cm.tl_offset[axis]; - if (cm.gmx.origin_offset_enable == true) { - offset += cm.gmx.origin_offset[axis]; // includes G5x and G92 components + float offset = cm->coord_offset[cm->gm.coord_system][axis] + cm->tool_offset[axis]; + if (cm->gmx.g92_offset_enable == true) { + offset += cm->gmx.g92_offset[axis]; } return (offset); } -/* - * cm_get_work_offset() - return a coord offset from the gcode_state - * - * MODEL (GCodeState_t *)&cm.gm // absolute pointer from canonical machine gm model - * PLANNER (GCodeState_t *)&bf->gm // relative to buffer *bf is currently pointing to - * RUNTIME (GCodeState_t *)&mr.gm // absolute pointer from runtime mm struct - * ACTIVE_MODEL cm.am // active model pointer is maintained by state management - */ - -float cm_get_work_offset(const GCodeState_t *gcode_state, const uint8_t axis) +float cm_get_display_offset(const GCodeState_t *gcode_state, const uint8_t axis) { - return (gcode_state->work_offset[axis]); + return (gcode_state->display_offset[axis]); } -/* - * cm_set_work_offsets() - capture coord offsets from the model into absolute values in the gcode_state - * - * MODEL (GCodeState_t *)&cm.gm // absolute pointer from canonical machine gm model - * PLANNER (GCodeState_t *)&bf->gm // relative to buffer *bf is currently pointing to - * RUNTIME (GCodeState_t *)&mr.gm // absolute pointer from runtime mm struct - * ACTIVE_MODEL cm.am // active model pointer is maintained by state management - */ - -void cm_set_work_offsets(GCodeState_t *gcode_state) +void cm_set_display_offsets(GCodeState_t *gcode_state) { for (uint8_t axis = AXIS_X; axis < AXES; axis++) { - gcode_state->work_offset[axis] = cm_get_active_coord_offset(axis); + + // if absolute override is on for G53 so position should be displayed with no offsets + if (cm->gm.absolute_override == ABSOLUTE_OVERRIDE_ON_DISPLAY_WITH_NO_OFFSETS) { + gcode_state->display_offset[axis] = 0; + } + + // all other cases: position should be displayed with currently active offsets + else { + gcode_state->display_offset[axis] = cm->coord_offset[cm->gm.coord_system][axis] + + cm->tool_offset[axis]; + if (cm->gmx.g92_offset_enable == true) { + gcode_state->display_offset[axis] += cm->gmx.g92_offset[axis]; + } + } } } /* - * cm_get_absolute_position() - get position of axis in absolute coordinates - * - * This function accepts as input: - * MODEL (GCodeState_t *)&cm.gm // absolute pointer from canonical machine gm model - * RUNTIME (GCodeState_t *)&mr.gm // absolute pointer from runtime mm struct - * - * NOTE: Only MODEL and RUNTIME are supported (no PLANNER or bf's) - * NOTE: Machine position is always returned in mm mode. No units conversion is performed - */ - -float cm_get_absolute_position(const GCodeState_t *gcode_state, const uint8_t axis) -{ - if (gcode_state == MODEL) { - return (cm.gmx.position[axis]); - } - return (mp_get_runtime_absolute_position(axis)); -} - -/* - * cm_get_work_position() - return work position in external form + * cm_get_display_position() - return position in external form from the active Gcode dynamic model * * ... that means in prevailing units (mm/inch) and with all offsets applied - * - * NOTE: This function only works after the gcode_state struct as had the work_offsets setup by - * calling cm_get_model_coord_offset_vector() first. - * - * This function accepts as input: - * MODEL (GCodeState_t *)&cm.gm // absolute pointer from canonical machine gm model - * RUNTIME (GCodeState_t *)&mr.gm // absolute pointer from runtime mm struct - * - * NOTE: Only MODEL and RUNTIME are supported (no PLANNER or bf's) */ -float cm_get_work_position(const GCodeState_t *gcode_state, const uint8_t axis) +float cm_get_display_position(const GCodeState_t *gcode_state, const uint8_t axis) { float position; if (gcode_state == MODEL) { - position = cm.gmx.position[axis] - cm_get_active_coord_offset(axis); + position = cm->gmx.position[axis] - cm_get_display_offset(MODEL, axis); } else { - position = mp_get_runtime_work_position(axis); + position = mp_get_runtime_display_position(axis); } - if (axis <= AXIS_Z) { + if (axis <= AXIS_W) { // linears if (gcode_state->units_mode == INCHES) { position /= MM_PER_INCH; } @@ -484,196 +584,190 @@ float cm_get_work_position(const GCodeState_t *gcode_state, const uint8_t axis) return (position); } -/*********************************************************************************** - * CRITICAL HELPERS - * Core functions supporting the canonical machining functions - * These functions are not part of the NIST defined functions - ***********************************************************************************/ /* - * cm_finalize_move() - perform final operations for a traverse or feed - * cm_update_model_position_from_runtime() - set endpoint position from final runtime position + * cm_get_absolute_position() - get position of axis in absolute coordinates from the active Gcode dynamic model * - * These routines set the point position in the gcode model. - * - * Note: As far as the canonical machine is concerned the final position of a Gcode block (move) - * is achieved as soon as the move is planned and the move target becomes the new model position. - * In reality the planner will (in all likelihood) have only just queued the move for later - * execution, and the real tool position is still close to the starting point. + * ... machine position is always returned in mm mode. No units conversion is performed */ -void cm_finalize_move() +float cm_get_absolute_position(const GCodeState_t *gcode_state, const uint8_t axis) { - copy_vector(cm.gmx.position, cm.gm.target); // update model position + if (gcode_state == MODEL) { + return (cm->gmx.position[axis]); + } + return (mp_get_runtime_absolute_position(mr, axis)); } -void cm_update_model_position_from_runtime() +/**************************************************************************************** + **** CRITICAL HELPERS ****************************************************************** + **************************************************************************************** + * Core functions supporting the canonical machining functions + * These functions are not part of the NIST defined functions + ****************************************************************************************/ + +/**************************************************************************************** + * cm_update_model_position() - set gmx endpoint position for a traverse, feed or arc + * + * Note: As far as the canonical machine is concerned the final position of a Gcode block + * (move) is achieved as soon as the move is planned and the move target becomes the new + * model position. In reality the planner will (in all likelihood) have only just queued + * the move for later execution, and the real tool position is still close to the starting + * point. + */ + +void cm_update_model_position() { - copy_vector(cm.gmx.position, mr.gm.target); + copy_vector(cm->gmx.position, cm->gm.target); // would be mr->gm.target if from runtime } -/* +/**************************************************************************************** * cm_deferred_write_callback() - write any changed G10 values back to persistence * - * Only runs if there is G10 data to write, there is no movement, and the serial queues are quiescent - * This could be made tighter by issuing an XOFF or ~CTS beforehand and releasing it afterwards. + * Only runs if there is G10 data to write, there is no movement, and the serial queues + * are quiescent. */ stat_t cm_deferred_write_callback() { - if ((cm.cycle_state == CYCLE_OFF) && (cm.deferred_write_flag == true)) { - cm.deferred_write_flag = false; + if ((cm->cycle_type == CYCLE_NONE) && (cm->deferred_write_flag == true)) { + cm->deferred_write_flag = false; nvObj_t nv; for (uint8_t i=1; i<=COORDS; i++) { for (uint8_t j=0; jcoord_offset[i][j]; + nv_persist(&nv); // Note: nv_persist() only writes values that have changed } } } return (STAT_OK); } -/* - * cm_set_tram() - JSON command to trigger computing the rotation matrix +/**************************************************************************************** * cm_get_tram() - JSON query to determine if the rotation matrix is set (non-identity) + * cm_set_tram() - JSON command to trigger computing the rotation matrix * - * There MUST be three valid probes stored. + * For set_tram there MUST be three valid probes stored. */ -stat_t cm_set_tram(nvObj_t *nv) -{ - if ((nv->valuetype == TYPE_BOOL) || - (nv->valuetype == TYPE_INT) || - (nv->valuetype == TYPE_FLOAT)) - { - bool do_set = !!((uint32_t)nv->value); - - // if passed false/0, we will clear the rotation matrix - if (!do_set) { - canonical_machine_reset_rotation(); - return (STAT_OK); - } - - // check to make sure we have three valid probes in a row - if ((cm.probe_state[0] == PROBE_SUCCEEDED) && - (cm.probe_state[1] == PROBE_SUCCEEDED) && - (cm.probe_state[2] == PROBE_SUCCEEDED)) - { - - // Step 1: Get the normal of the plane formed by the three probes. Naming: - // d0_{xyz} is the delta between point 0 and point 1 - // d2_{xyz} is the delta between point 2 and point 1 - // n_{xyz} is the unit normal - - // Step 1a: get the deltas - float d0_x = cm.probe_results[0][0] - cm.probe_results[1][0]; - float d0_y = cm.probe_results[0][1] - cm.probe_results[1][1]; - float d0_z = cm.probe_results[0][2] - cm.probe_results[1][2]; - float d2_x = cm.probe_results[2][0] - cm.probe_results[1][0]; - float d2_y = cm.probe_results[2][1] - cm.probe_results[1][1]; - float d2_z = cm.probe_results[2][2] - cm.probe_results[1][2]; - - // Step 1b: compute the combined magnitude - // since sqrt(a)*sqrt(b) = sqrt(a*b), we can save a sqrt in making the unit normal - float combined_magnitude_inv = 1.0/sqrt( - (d0_x*d0_x + d0_y*d0_y + d0_z*d0_z)* - (d2_x*d2_x + d2_y*d2_y + d2_z*d2_z) - ); - - // Step 1c: compute the cross product and normalize - float n_x = (d0_z*d2_y - d0_y*d2_z)*combined_magnitude_inv; - float n_y = (d0_x*d2_z - d0_z*d2_x)*combined_magnitude_inv; - float n_z = (d0_y*d2_x - d0_x*d2_y)*combined_magnitude_inv; - - // Step 1d: flip the normal if it's negative - if (n_z < 0.0) { - n_x = -n_x; - n_y = -n_y; - n_z = -n_z; - } - - // Step 2: make the quaternion for the rotation to {0,0,1} - float p = sqrt(n_x*n_x + n_y*n_y + n_z*n_z); - float m = sqrt(2.0)*sqrt(p*(p+n_z)); - float q_w = (n_z + p) / m; - float q_x = -n_y / m; - float q_y = n_x / m; - //float q_z = 0; // already optimized out - - // Step 3: compute the rotation matrix - float q_wx_2 = q_w * q_x * 2.0; - float q_wy_2 = q_w * q_y * 2.0; - float q_xx_2 = q_x * q_x * 2.0; - float q_xy_2 = q_x * q_y * 2.0; - float q_yy_2 = q_y * q_y * 2.0; - - /* - matrix = { - {1 - q_yy_2, q_xy_2, q_wy_2, 0}, - {q_xy_2, 1 - q_xx_2, -q_wx_2, 0}, - {-q_wy_2, q_wx_2, 1 - q_xx_2 - q_yy_2, i}, - {0, 0, 0, 1} - } - */ - cm.rotation_matrix[0][0] = 1 - q_yy_2; - cm.rotation_matrix[0][1] = q_xy_2; - cm.rotation_matrix[0][2] = q_wy_2; - - cm.rotation_matrix[1][0] = q_xy_2; - cm.rotation_matrix[1][1] = 1 - q_xx_2; - cm.rotation_matrix[1][2] = -q_wx_2; - - cm.rotation_matrix[2][0] = -q_wy_2; - cm.rotation_matrix[2][1] = q_wx_2; - cm.rotation_matrix[2][2] = 1 - q_xx_2 - q_yy_2; - - // Step 4: compute the z-offset - cm.rotation_z_offset = (n_x*cm.probe_results[1][0] + n_y*cm.probe_results[1][1]) / n_z + cm.probe_results[1][2]; - } else { - return (STAT_COMMAND_NOT_ACCEPTED); - } - } else { - return (STAT_INPUT_VALUE_RANGE_ERROR); - } - return (STAT_OK); -} - stat_t cm_get_tram(nvObj_t *nv) { - nv->value = true; + nv->value_int = true; // believe it or not, the compiler likes this form best - most efficient code - if (fp_NOT_ZERO(cm.rotation_z_offset) || - fp_NOT_ZERO(cm.rotation_matrix[0][1]) || - fp_NOT_ZERO(cm.rotation_matrix[0][2]) || - fp_NOT_ZERO(cm.rotation_matrix[1][0]) || - fp_NOT_ZERO(cm.rotation_matrix[1][2]) || - fp_NOT_ZERO(cm.rotation_matrix[2][0]) || - fp_NOT_ZERO(cm.rotation_matrix[2][1]) || - fp_NE(1.0, cm.rotation_matrix[0][0]) || - fp_NE(1.0, cm.rotation_matrix[1][1]) || - fp_NE(1.0, cm.rotation_matrix[2][2])) + if (fp_NOT_ZERO(cm->rotation_z_offset) || + fp_NOT_ZERO(cm->rotation_matrix[0][1]) || + fp_NOT_ZERO(cm->rotation_matrix[0][2]) || + fp_NOT_ZERO(cm->rotation_matrix[1][0]) || + fp_NOT_ZERO(cm->rotation_matrix[1][2]) || + fp_NOT_ZERO(cm->rotation_matrix[2][0]) || + fp_NOT_ZERO(cm->rotation_matrix[2][1]) || + fp_NE(1.0, cm->rotation_matrix[0][0]) || + fp_NE(1.0, cm->rotation_matrix[1][1]) || + fp_NE(1.0, cm->rotation_matrix[2][2])) { - nv->value = false; + nv->value_int = false; } - nv->valuetype = TYPE_BOOL; + nv->valuetype = TYPE_BOOLEAN; return (STAT_OK); } +stat_t cm_set_tram(nvObj_t *nv) +{ + if (!nv->value_int) { // if false, reset the matrix and return + canonical_machine_reset_rotation(cm); + return (STAT_OK); + } -/* + // check to make sure we have three valid probes in a row + if (!((cm->probe_state[0] == PROBE_SUCCEEDED) && + (cm->probe_state[1] == PROBE_SUCCEEDED) && + (cm->probe_state[2] == PROBE_SUCCEEDED))) { + return (STAT_COMMAND_NOT_ACCEPTED); // do not have 3 valid probes + } + + // Step 1: Get the normal of the plane formed by the three probes. Naming: + // d0_{xyz} is the delta between point 0 and point 1 + // d2_{xyz} is the delta between point 2 and point 1 + // n_{xyz} is the unit normal + + // Step 1a: get the deltas + float d0_x = cm->probe_results[0][0] - cm->probe_results[1][0]; + float d0_y = cm->probe_results[0][1] - cm->probe_results[1][1]; + float d0_z = cm->probe_results[0][2] - cm->probe_results[1][2]; + float d2_x = cm->probe_results[2][0] - cm->probe_results[1][0]; + float d2_y = cm->probe_results[2][1] - cm->probe_results[1][1]; + float d2_z = cm->probe_results[2][2] - cm->probe_results[1][2]; + + // Step 1b: compute the combined magnitude + // since sqrt(a)*sqrt(b) = sqrt(a*b), we can save a sqrt in making the unit normal + float combined_magnitude_inv = 1.0/sqrt((d0_x*d0_x + d0_y*d0_y + d0_z*d0_z) * + (d2_x*d2_x + d2_y*d2_y + d2_z*d2_z)); + + // Step 1c: compute the cross product and normalize + float n_x = (d0_z*d2_y - d0_y*d2_z) * combined_magnitude_inv; + float n_y = (d0_x*d2_z - d0_z*d2_x) * combined_magnitude_inv; + float n_z = (d0_y*d2_x - d0_x*d2_y) * combined_magnitude_inv; + + // Step 1d: flip the normal if it's negative + if (n_z < 0.0) { + n_x = -n_x; + n_y = -n_y; + n_z = -n_z; + } + + // Step 2: make the quaternion for the rotation to {0,0,1} + float p = sqrt(n_x*n_x + n_y*n_y + n_z*n_z); + float m = sqrt(2.0)*sqrt(p*(p+n_z)); + float q_w = (n_z + p) / m; + float q_x = -n_y / m; + float q_y = n_x / m; + //float q_z = 0; // already optimized out + + // Step 3: compute the rotation matrix + float q_wx_2 = q_w * q_x * 2.0; + float q_wy_2 = q_w * q_y * 2.0; + float q_xx_2 = q_x * q_x * 2.0; + float q_xy_2 = q_x * q_y * 2.0; + float q_yy_2 = q_y * q_y * 2.0; + + /* + matrix = { + {1 - q_yy_2, q_xy_2, q_wy_2, 0}, + {q_xy_2, 1 - q_xx_2, -q_wx_2, 0}, + {-q_wy_2, q_wx_2, 1 - q_xx_2 - q_yy_2, i}, + {0, 0, 0, 1} + } + */ + cm->rotation_matrix[0][0] = 1 - q_yy_2; + cm->rotation_matrix[0][1] = q_xy_2; + cm->rotation_matrix[0][2] = q_wy_2; + + cm->rotation_matrix[1][0] = q_xy_2; + cm->rotation_matrix[1][1] = 1 - q_xx_2; + cm->rotation_matrix[1][2] = -q_wx_2; + + cm->rotation_matrix[2][0] = -q_wy_2; + cm->rotation_matrix[2][1] = q_wx_2; + cm->rotation_matrix[2][2] = 1 - q_xx_2 - q_yy_2; + + // Step 4: compute the z-offset + cm->rotation_z_offset = (n_x*cm->probe_results[1][0] + + n_y*cm->probe_results[1][1]) / + n_z + cm->probe_results[1][2]; + return (STAT_OK); +} + +/**************************************************************************************** * cm_set_nxt_line() - JSON command to set the next line number * cm_get_nxt_line() - JSON query to get the next expected line number - * - * There MUST be three valid probes stored. */ stat_t cm_set_nxln(nvObj_t *nv) { - if (nv->valuetype == TYPE_INT || nv->valuetype == TYPE_FLOAT) + if (nv->valuetype == TYPE_INTEGER || nv->valuetype == TYPE_FLOAT) { - cm.gmx.last_line_number = ((int32_t)nv->value) - 1; + cm->gmx.last_line_number = nv->value_int - 1; return (STAT_OK); } return (STAT_INPUT_VALUE_RANGE_ERROR); @@ -681,13 +775,12 @@ stat_t cm_set_nxln(nvObj_t *nv) stat_t cm_get_nxln(nvObj_t *nv) { - nv->value = cm.gmx.last_line_number+1; - nv->valuetype = TYPE_INT; + nv->value_int = cm->gmx.last_line_number+1; + nv->valuetype = TYPE_INTEGER; return (STAT_OK); } - -/* +/**************************************************************************************** * cm_set_model_target() - set target vector in GM model * * This is a core routine. It handles: @@ -713,13 +806,11 @@ stat_t cm_get_nxln(nvObj_t *nv) static float _calc_ABC(const uint8_t axis, const float target[]) { - if ((cm.a[axis].axis_mode == AXIS_STANDARD) || (cm.a[axis].axis_mode == AXIS_INHIBITED)) { + if ((cm->a[axis].axis_mode == AXIS_STANDARD) || (cm->a[axis].axis_mode == AXIS_INHIBITED)) { return(target[axis]); // no mm conversion - it's in degrees } - // radius mode - - return (_to_millimeters(target[axis]) * 360.0 / (2.0 * M_PI * cm.a[axis].radius)); + return (_to_millimeters(target[axis]) * 360.0 / (2 * M_PI * cm->a[axis].radius)); } void cm_set_model_target(const float target[], const bool flags[]) @@ -728,54 +819,58 @@ void cm_set_model_target(const float target[], const bool flags[]) float tmp = 0; // copy position to target so it always starts correctly - copy_vector(cm.gm.target, cm.gmx.position); + copy_vector(cm->gm.target, cm->gmx.position); - // process XYZABC for lower modes - for (axis=AXIS_X; axis<=AXIS_Z; axis++) { - if (!flags[axis] || cm.a[axis].axis_mode == AXIS_DISABLED) { + // process linear axes (XYZUVW) first + for (axis=AXIS_X; axis<=AXIS_W; axis++) { + if (!flags[axis] || cm->a[axis].axis_mode == AXIS_DISABLED) { continue; // skip axis if not flagged for update or its disabled - } else if ((cm.a[axis].axis_mode == AXIS_STANDARD) || (cm.a[axis].axis_mode == AXIS_INHIBITED)) { - if (cm.gm.distance_mode == ABSOLUTE_DISTANCE_MODE) { - cm.gm.target[axis] = cm_get_active_coord_offset(axis) + _to_millimeters(target[axis]); + } else if ((cm->a[axis].axis_mode == AXIS_STANDARD) || (cm->a[axis].axis_mode == AXIS_INHIBITED)) { + if (cm->gm.distance_mode == ABSOLUTE_DISTANCE_MODE) { + cm->gm.target[axis] = cm_get_combined_offset(axis) + _to_millimeters(target[axis]); } else { - cm.gm.target[axis] += _to_millimeters(target[axis]); + cm->gm.target[axis] += _to_millimeters(target[axis]); } + cm->return_flags[axis] = true; // used to make a synthetic G28/G30 intermediate move } } - // FYI: The ABC loop below relies on the XYZ loop having been run first + // FYI: The ABC loop below relies on the XYZUVW loop having been run first for (axis=AXIS_A; axis<=AXIS_C; axis++) { - if (!flags[axis] || cm.a[axis].axis_mode == AXIS_DISABLED) { + if (!flags[axis] || cm->a[axis].axis_mode == AXIS_DISABLED) { continue; // skip axis if not flagged for update or its disabled } else { tmp = _calc_ABC(axis, target); } + #if MARLIN_COMPAT_ENABLED == true // If we are in absolute mode (generally), but the extruder is relative, // then we adjust the extruder to a relative position - if (mst.marlin_flavor && (cm.a[axis].axis_mode == AXIS_RADIUS)) { - if ((cm.gm.distance_mode == INCREMENTAL_DISTANCE_MODE) || (mst.extruder_mode == EXTRUDER_MOVES_RELATIVE)) { - cm.gm.target[axis] += tmp; + if (mst.marlin_flavor && (cm->a[axis].axis_mode == AXIS_RADIUS)) { + if ((cm->gm.distance_mode == INCREMENTAL_DISTANCE_MODE) || (mst.extruder_mode == EXTRUDER_MOVES_RELATIVE)) { + cm->gm.target[axis] += tmp; } else { // if (cm.gmx.extruder_mode == EXTRUDER_MOVES_NORMAL) - cm.gm.target[axis] = tmp + cm_get_active_coord_offset(axis); + cm->gm.target[axis] = tmp + cm_get_combined_offset(axis); } - // TODO + // TODO - volumetric filament conversion // else { -// cm.gm.target[axis] += tmp * cm.gmx.volume_to_filament_length[axis-3]; +// cm->gm.target[axis] += tmp * cm.gmx.volume_to_filament_length[axis-3]; // } } else #endif // MARLIN_COMPAT_ENABLED - if (cm.gm.distance_mode == ABSOLUTE_DISTANCE_MODE) { - cm.gm.target[axis] = tmp + cm_get_active_coord_offset(axis); // sacidu93's fix to Issue #22 + + if (cm->gm.distance_mode == ABSOLUTE_DISTANCE_MODE) { + cm->gm.target[axis] = tmp + cm_get_combined_offset(axis); // sacidu93's fix to Issue #22 } else { - cm.gm.target[axis] += tmp; + cm->gm.target[axis] += tmp; } + cm->return_flags[axis] = true; } } -/* +/**************************************************************************************** * cm_get_soft_limits() * cm_set_soft_limits() * cm_test_soft_limits() - return error code if soft limit is exceeded @@ -788,29 +883,29 @@ void cm_set_model_target(const float target[], const bool flags[]) * This allows a single end to be tested w/the other disabled, should that requirement ever arise. */ -bool cm_get_soft_limits() { return (cm.soft_limit_enable); } -void cm_set_soft_limits(bool enable) { cm.soft_limit_enable = enable; } +bool cm_get_soft_limits() { return (cm->soft_limit_enable); } +void cm_set_soft_limits(bool enable) { cm->soft_limit_enable = enable; } static stat_t _finalize_soft_limits(const stat_t status) { - cm.gm.motion_mode = MOTION_MODE_CANCEL_MOTION_MODE; // cancel motion - copy_vector(cm.gm.target, cm.gmx.position); // reset model target + cm->gm.motion_mode = MOTION_MODE_CANCEL_MOTION_MODE; // cancel motion + copy_vector(cm->gm.target, cm->gmx.position); // reset model target return (cm_alarm(status, "soft_limits")); // throw an alarm } stat_t cm_test_soft_limits(const float target[]) { - if (cm.soft_limit_enable == true) { + if (cm->soft_limit_enable == true) { for (uint8_t axis = AXIS_X; axis < AXES; axis++) { - if (cm.homed[axis] != true) { continue; } // skip axis if not homed - if (fp_EQ(cm.a[axis].travel_min, cm.a[axis].travel_max)) { continue; } // skip axis if identical - if (std::abs(cm.a[axis].travel_min) > DISABLE_SOFT_LIMIT) { continue; } // skip min test if disabled - if (std::abs(cm.a[axis].travel_max) > DISABLE_SOFT_LIMIT) { continue; } // skip max test if disabled + if (cm->homed[axis] != true) { continue; } // skip axis if not homed + if (fp_EQ(cm->a[axis].travel_min, cm->a[axis].travel_max)) { continue; } // skip axis if identical + if (fabs(cm->a[axis].travel_min) > DISABLE_SOFT_LIMIT) { continue; } // skip min test if disabled + if (fabs(cm->a[axis].travel_max) > DISABLE_SOFT_LIMIT) { continue; } // skip max test if disabled - if (target[axis] < cm.a[axis].travel_min) { + if (target[axis] < cm->a[axis].travel_min) { return (_finalize_soft_limits(STAT_SOFT_LIMIT_EXCEEDED_XMIN + 2*axis)); } - if (target[axis] > cm.a[axis].travel_max) { + if (target[axis] > cm->a[axis].travel_max) { return (_finalize_soft_limits(STAT_SOFT_LIMIT_EXCEEDED_XMAX + 2*axis)); } } @@ -818,335 +913,17 @@ stat_t cm_test_soft_limits(const float target[]) return (STAT_OK); } -/************************************************************************* - * CANONICAL MACHINING FUNCTIONS - * Values are passed in pre-unit_converted state (from gn structure) - * All operations occur on gm (current model state) - * - * These are organized by section number (x.x.x) in the order they are - * found in NIST RS274 NGCv3 - ************************************************************************/ +/**************************************************************************************** + **** CANONICAL MACHINING FUNCTIONS ***************************************************** + **************************************************************************************** + * Organized by section number in the order they are found in NIST RS274 NGCv3 + ***************************************************************************************/ -/****************************************** - * Initialization and Termination (4.3.2) * - ******************************************/ -/* - * canonical_machine_init() - initialize cm struct - * canonical_machine_reset() - apply startup settings or reset to startup - * run profile initialization beforehand -*/ +/**************************************************************************************** + **** Representation (4.3.3) ************************************************************ + ****************************************************************************************/ -void canonical_machine_init() -{ -// If you can assume all memory has been zeroed by a hard reset you don't need this code: -// memset(&cm, 0, sizeof(cm)); // do not reset canonicalMachineSingleton once it's been initialized - memset(&cm, 0, sizeof(cmSingleton_t)); // do not reset canonicalMachineSingleton once it's been initialized - cm.gm.reset(); // clear all values, pointers and status -- not ALL to zero, however - - canonical_machine_init_assertions(); // establish assertions - ACTIVE_MODEL = MODEL; // setup initial Gcode model pointer - cm_arc_init(); // Note: spindle and coolant inits are independent - - din_handlers[INPUT_ACTION_STOP].registerHandler(&_hold_input_handler); - din_handlers[INPUT_ACTION_FAST_STOP].registerHandler(&_hold_input_handler); - din_handlers[INPUT_ACTION_HALT].registerHandler(&_halt_input_handler); - din_handlers[INPUT_ACTION_ALARM].registerHandler(&_alarm_input_handler); - din_handlers[INPUT_ACTION_PANIC].registerHandler(&_panic_input_handler); - din_handlers[INPUT_ACTION_RESET].registerHandler(&_reset_input_handler); -} - -void canonical_machine_reset_rotation() { - memset(&cm.rotation_matrix, 0, sizeof(float)*3*3); - // We must make it an identity matrix for no rotation - cm.rotation_matrix[0][0] = 1.0; - cm.rotation_matrix[1][1] = 1.0; - cm.rotation_matrix[2][2] = 1.0; - cm.rotation_z_offset = 0.0; -} - -void canonical_machine_reset() -{ - // set gcode defaults - cm_set_units_mode(cm.default_units_mode); - cm_set_coord_system(cm.default_coord_system); // NB: queues a block to the planner with the coordinates - cm_select_plane(cm.default_select_plane); - cm_set_path_control(MODEL, cm.default_path_control); - cm_set_distance_mode(cm.default_distance_mode); - cm_set_arc_distance_mode(INCREMENTAL_DISTANCE_MODE);// always the default - cm_set_feed_rate_mode(UNITS_PER_MINUTE_MODE); // always the default - cm_reset_overrides(); // set overrides to initial conditions - - // NOTE: Should unhome axes here - - // reset requests and flags - cm.queue_flush_state = FLUSH_OFF; - cm.end_hold_requested = false; - cm.limit_requested = 0; // resets switch closures that occurred during initialization - cm.safety_interlock_disengaged = 0; // ditto - cm.safety_interlock_reengaged = 0; // ditto - cm.shutdown_requested = 0; // ditto - cm.probe_report_enable = PROBE_REPORT_ENABLE; - - // set initial state and signal that the machine is ready for action - cm.cycle_state = CYCLE_OFF; - cm.motion_state = MOTION_STOP; - cm.hold_state = FEEDHOLD_OFF; - cm.esc_boot_timer = SysTickTimer.getValue(); - cm.gmx.block_delete_switch = true; - cm.gm.motion_mode = MOTION_MODE_CANCEL_MOTION_MODE; // never start in a motion mode - cm.machine_state = MACHINE_READY; - - canonical_machine_reset_rotation(); - - memset(&cm.probe_state, 0, sizeof(cmProbeState)*PROBES_STORED); - memset(&cm.probe_results, 0, sizeof(float)*PROBES_STORED*AXES); -} - -/* - * canonical_machine_init_assertions() - * canonical_machine_test_assertions() - test assertions, return error code if violation exists - */ - -void canonical_machine_init_assertions(void) -{ - cm.magic_start = MAGICNUM; - cm.magic_end = MAGICNUM; - cm.gmx.magic_start = MAGICNUM; - cm.gmx.magic_end = MAGICNUM; - arc.magic_start = MAGICNUM; - arc.magic_end = MAGICNUM; -} - -stat_t canonical_machine_test_assertions(void) -{ - if ((BAD_MAGIC(cm.magic_start)) || (BAD_MAGIC(cm.magic_end)) || - (BAD_MAGIC(cm.gmx.magic_start)) || (BAD_MAGIC(cm.gmx.magic_end)) || - (BAD_MAGIC(arc.magic_start)) || (BAD_MAGIC(arc.magic_end))) { - return(cm_panic(STAT_CANONICAL_MACHINE_ASSERTION_FAILURE, "canonical_machine_test_assertions()")); - } - return (STAT_OK); -} - -/************************** - * Alarms * - **************************/ - -/******************************************************************************** - * ALARM, SHUTDOWN, and PANIC are nested dolls. - * - * cm_alrm() - invoke alarm from command - * cm_shutd() - invoke shutdown from command - * cm_pnic() - invoke panic from command - * cm_clr() - clear alarm or shutdown from command - * - * The alarm states can be invoked from the above commands for testing and clearing - */ -stat_t cm_alrm(nvObj_t *nv) // invoke alarm from command -{ - cm_alarm(STAT_ALARM, "sent by host"); - return (STAT_OK); -} - -stat_t cm_shutd(nvObj_t *nv) // invoke shutdown from command -{ - cm_shutdown(STAT_SHUTDOWN, "sent by host"); - return (STAT_OK); -} - -stat_t cm_pnic(nvObj_t *nv) // invoke panic from command -{ - cm_panic(STAT_PANIC, "sent by host"); - return (STAT_OK); -} - -stat_t cm_clr(nvObj_t *nv) // clear alarm or shutdown from command line -{ - cm_clear(); - return (STAT_OK); -} - -/* - * cm_clear() - clear ALARM and SHUTDOWN states - * cm_parse_clear() - parse incoming gcode for M30 or M2 clears if in ALARM state - * - * Parse clear interprets an M30 or M2 PROGRAM_END as a $clear condition and clear ALARM - * but not SHUTDOWN or PANIC. Assumes Gcode string has no leading or embedded whitespace - */ - -void cm_clear() -{ - if (cm.machine_state == MACHINE_ALARM) { - cm.machine_state = MACHINE_PROGRAM_STOP; - xio_flush_to_command(); - } else if (cm.machine_state == MACHINE_SHUTDOWN) { - cm.machine_state = MACHINE_READY; - } -} - -void cm_parse_clear(const char *s) -{ - if (cm.machine_state == MACHINE_ALARM) { - if (toupper(s[0]) == 'M') { - if (( (s[1]=='3') && (s[2]=='0') && (s[3]==NUL)) || ((s[1]=='2') && (s[2]==NUL) )) { - cm_clear(); - } - } - } -} - -/* - * cm_is_alarmed() - return alarm status code or OK if no alarms - */ - -stat_t cm_is_alarmed() -{ - if (cm.machine_state == MACHINE_ALARM) { return (STAT_COMMAND_REJECTED_BY_ALARM); } - if (cm.machine_state == MACHINE_SHUTDOWN) { return (STAT_COMMAND_REJECTED_BY_SHUTDOWN); } - if (cm.machine_state == MACHINE_PANIC) { return (STAT_COMMAND_REJECTED_BY_PANIC); } - return (STAT_OK); -} - -/* - * cm_halt_all() - stop, spindle and coolant immediately - * cm_halt_motion() - stop motion immediately. Does not affect spindle, coolant, or other IO - * - * Stop motors and reset all system states accordingly. - * Does not de-energize motors as in some cases the motors must remain energized - * in order to prevent an axis from crashing. - */ - -void cm_halt_all(void) -{ - cm_halt_motion(); - cm_spindle_off_immediate(); - cm_coolant_off_immediate(); -} - -void cm_halt_motion(void) -{ - mp_halt_runtime(); // stop the runtime. Do this immediately. (Reset is in cm_clear) - canonical_machine_reset(); // reset Gcode model - cm.cycle_state = CYCLE_OFF; // Note: leaves machine_state alone - cm.motion_state = MOTION_STOP; - cm.hold_state = FEEDHOLD_OFF; -} - - -/* - * cm_alarm() - enter ALARM state - * - * An ALARM sets the ALARM machine state, starts a feedhold to stop motion, stops the - * spindle, turns off coolant, clears out queued planner moves and serial input, - * and rejects new action commands (gcode blocks, SET commands, and other actions) - * until the alarm is cleared. - * - * ALARM is typically entered by a soft limit or a limit switch being hit. In the - * limit switch case the INPUT_ACTION will override the feedhold - i.e. if the - * input action is "FAST_STOP" or "HALT" that setting will take precedence over - * the feedhold native to the alarm function. - * - * Gcode and machine state is preserved. It may be possible to recover the job from - * an alarm, but in many cases this is not possible. Since ALARM attempts to preserve - * Gcode and machine state it does not END the job. - * - * ALARM may also be invoked from the command line using {alarm:n} or $alarm - * ALARM can be manually cleared by entering: {clear:n}, {clr:n}, $clear, or $clr - * ALARMs will also clear on receipt of an M30 or M2 command if one is received - * while draining the host command queue. - */ - -stat_t cm_alarm(const stat_t status, const char *msg) -{ - if ((cm.machine_state == MACHINE_ALARM) || (cm.machine_state == MACHINE_SHUTDOWN) || - (cm.machine_state == MACHINE_PANIC)) { - return (STAT_OK); // don't alarm if already in an alarm state - } - cm.machine_state = MACHINE_ALARM; - cm_request_feedhold(); // stop motion - cm_request_queue_flush(); // do a queue flush once runtime is not busy - -// TBD - these functions should probably be called - See cm_shutdown() -// cm_spindle_control_immediate(SPINDLE_OFF); -// cm_coolant_off_immediate(); -// cm_spindle_optional_pause(spindle.pause_on_hold); -// cm_coolant_optional_pause(coolant.pause_on_hold); - rpt_exception(status, msg); // send alarm message - - // If "stat" is in the status report, we need to poke it to send. - sr_request_status_report(SR_REQUEST_TIMED); - return (status); -} -/* - * cm_shutdown() - enter shutdown state - * - * SHUTDOWN stops all motion, spindle and coolant immediately, sets a SHUTDOWN machine - * state, clears out queued moves and serial input, and rejects new action commands - * (gcode blocks, SET commands, and some others). - * - * Shutdown is typically invoked as an electrical input signal sent to the board as - * part of an external emergency stop (Estop). Shutdown is meant to augment but not - * replace the external Estop functions that shut down power to motors, spindles and - * other moving parts. - * - * Shutdown may also be invoked from the command line using {shutd:n} or $shutd - * Shutdown must be manually cleared by entering: {clear:n}, {clr:n}, $clear, or $clr - * Shutdown does not clear on M30 or M2 Gcode commands - */ - -stat_t cm_shutdown(const stat_t status, const char *msg) -{ - if ((cm.machine_state == MACHINE_SHUTDOWN) || (cm.machine_state == MACHINE_PANIC)) { - return (STAT_OK); // don't shutdown if shutdown or panic'd - } - cm_halt_motion(); // halt motors (may have already been done from GPIO) - spindle_reset(); // stop spindle immediately and set speed to 0 RPM - coolant_reset(); // stop coolant immediately - temperature_reset(); // turn off heaters and fans - cm_queue_flush(); // flush all queues and reset positions - - for (uint8_t i = 0; i < HOMING_AXES; i++) { // unhome axes and the machine - cm.homed[i] = false; - } - cm.homing_state = HOMING_NOT_HOMED; - - cm.machine_state = MACHINE_SHUTDOWN; // do this after all other activity - rpt_exception(status, msg); // send exception report - return (status); -} - -/* - * cm_panic() - enter panic state - * - * PANIC occurs if the firmware has detected an unrecoverable internal error - * such as an assertion failure or a code condition that should never occur. - * It sets PANIC machine state, and leaves the system inspect able (if possible). - * - * PANIC can only be exited by a hardware reset or soft reset (^x) - */ - -stat_t cm_panic(const stat_t status, const char *msg) -{ - _debug_trap(msg); - - if (cm.machine_state == MACHINE_PANIC) { // only do this once - return (STAT_OK); - } - cm_halt_motion(); // halt motors (may have already been done from GPIO) - spindle_reset(); // stop spindle immediately and set speed to 0 RPM - coolant_reset(); // stop coolant immediately - temperature_reset(); // turn off heaters and fans - cm_queue_flush(); // flush all queues and reset positions - - cm.machine_state = MACHINE_PANIC; // don't reset anything. Panics are not recoverable - rpt_exception(status, msg); // send panic report - return (status); -} - -/************************** - * Representation (4.3.3) * - **************************/ - -/************************************************************************** +/***************************************************************************************** * Representation functions that affect the Gcode model only (asynchronous) * * cm_select_plane() - G17,G18,G19 select axis plane @@ -1155,42 +932,40 @@ stat_t cm_panic(const stat_t status, const char *msg) * cm_set_arc_distance_mode() - G90.1, G91.1 * cm_set_g10_data() - G10 (delayed persistence) * - * These functions assume input validation occurred upstream. + * These functions assume input validation occurred upstream, most likely in gcode parser. */ stat_t cm_select_plane(const uint8_t plane) { - cm.gm.select_plane = (cmCanonicalPlane)plane; + cm->gm.select_plane = (cmCanonicalPlane)plane; return (STAT_OK); } stat_t cm_set_units_mode(const uint8_t mode) { - cm.gm.units_mode = (cmUnitsMode)mode; // 0 = inches, 1 = mm. + cm->gm.units_mode = (cmUnitsMode)mode; // 0 = inches, 1 = mm. return(STAT_OK); } stat_t cm_set_distance_mode(const uint8_t mode) { - cm.gm.distance_mode = (cmDistanceMode)mode; // 0 = absolute mode, 1 = incremental + cm->gm.distance_mode = (cmDistanceMode)mode; // 0 = absolute mode, 1 = incremental return (STAT_OK); } stat_t cm_set_arc_distance_mode(const uint8_t mode) { - cm.gm.arc_distance_mode = (cmDistanceMode)mode; // 0 = absolute mode, 1 = incremental + cm->gm.arc_distance_mode = (cmDistanceMode)mode; // 0 = absolute mode, 1 = incremental return (STAT_OK); } -/* +/**************************************************************************************** * cm_set_g10_data() - G10 L1/L2/L10/L20 Pn (affects MODEL only) * * This function applies the offset to the GM model but does not persist the offsets * during the Gcode cycle. The persist flag is used to persist offsets once the cycle * has ended. You can also use $g54x - $g59c config functions to change offsets. - * - * It also does not reset the work_offsets which may be accomplished by calling - * cm_set_work_offsets() immediately afterwards. + * It also does resets the display offsets to reflect the new values. */ stat_t cm_set_g10_data(const uint8_t P_word, const bool P_flag, @@ -1204,62 +979,67 @@ stat_t cm_set_g10_data(const uint8_t P_word, const bool P_flag, if ((L_word == 2) || (L_word == 20)) { // coordinate system offset command if ((P_word < G54) || (P_word > COORD_SYSTEM_MAX)) { - // you can't set G53 - return (STAT_P_WORD_IS_INVALID); + return (STAT_P_WORD_IS_INVALID); // you can't set G53 } for (uint8_t axis = AXIS_X; axis < AXES; axis++) { if (flag[axis]) { if (L_word == 2) { - cm.offset[P_word][axis] = _to_millimeters(offset[axis]); + cm->coord_offset[P_word][axis] = _to_millimeters(offset[axis]); } else { // Should L20 take into account G92 offsets? - cm.offset[P_word][axis] = - cm.gmx.position[axis] - + cm->coord_offset[P_word][axis] = cm->gmx.position[axis] - _to_millimeters(offset[axis]) - - cm.tl_offset[axis]; + cm->tool_offset[axis]; } - // persist offsets once machining cycle is over - cm.deferred_write_flag = true; + cm->deferred_write_flag = true; // persist offsets once machining cycle is over } } } else if ((L_word == 1) || (L_word == 10)) { - // tool table offset command. L11 not supported atm. - if ((P_word < 1) || (P_word > TOOLS)) { + if ((P_word < 1) || (P_word > TOOLS)) { // tool table offset command. L11 not supported atm. return (STAT_P_WORD_IS_INVALID); } for (uint8_t axis = AXIS_X; axis < AXES; axis++) { if (flag[axis]) { if (L_word == 1) { - cm.tt_offset[P_word][axis] = _to_millimeters(offset[axis]); - } else { - // L10 should also take into account G92 offset - cm.tt_offset[P_word][axis] = - cm.gmx.position[axis] - _to_millimeters(offset[axis]) - - cm.offset[cm.gm.coord_system][axis] - - (cm.gmx.origin_offset[axis] * cm.gmx.origin_offset_enable); + tt.tt_offset[P_word][axis] = _to_millimeters(offset[axis]); + } else { // L10 should also take into account G92 offset + tt.tt_offset[P_word][axis] = + cm->gmx.position[axis] - _to_millimeters(offset[axis]) - + cm->coord_offset[cm->gm.coord_system][axis] - + (cm->gmx.g92_offset[axis] * cm->gmx.g92_offset_enable); } - // persist offsets once machining cycle is over - cm.deferred_write_flag = true; + cm->deferred_write_flag = true; // persist offsets once machining cycle is over } } } else { return (STAT_L_WORD_IS_INVALID); } + cm_set_display_offsets(MODEL); return (STAT_OK); } /****************************************************************************************** * Representation functions that affect gcode model and are queued to planner (synchronous) - */ -/* + * * cm_set_tl_offset() - G43 * cm_cancel_tl_offset() - G49 * cm_set_coord_system() - G54-G59 - * _exec_offset() - callback from planner + * _exec_offset() - callback from planner command */ +static void _exec_offset(float *value, bool *flag) +{ + uint8_t coord_system = ((uint8_t)value[0]); // coordinate system is passed in value[0] element + float offsets[AXES]; + for (uint8_t axis = AXIS_X; axis < AXES; axis++) { + offsets[axis] = cm->coord_offset[coord_system][axis] + cm->tool_offset[axis] + + (cm->gmx.g92_offset[axis] * cm->gmx.g92_offset_enable); + } + mp_set_runtime_display_offset(offsets); +} + stat_t cm_set_tl_offset(const uint8_t H_word, const bool H_flag, const bool apply_additional) { uint8_t tool; @@ -1268,93 +1048,91 @@ stat_t cm_set_tl_offset(const uint8_t H_word, const bool H_flag, const bool appl return (STAT_H_WORD_IS_INVALID); } if (H_word == 0) { // interpret H0 as "current tool", just like no H at all. - tool = cm.gm.tool; + tool = cm->gm.tool; } else { tool = H_word; } - } else { - tool = cm.gm.tool; + } else { + tool = cm->gm.tool; } if (apply_additional) { for (uint8_t axis = AXIS_X; axis < AXES; axis++) { - cm.tl_offset[axis] += cm.tt_offset[tool][axis]; + cm->tool_offset[axis] += tt.tt_offset[tool][axis]; } } else { for (uint8_t axis = AXIS_X; axis < AXES; axis++) { - cm.tl_offset[axis] = cm.tt_offset[tool][axis]; + cm->tool_offset[axis] = tt.tt_offset[tool][axis]; } } - float value[] = { (float)cm.gm.coord_system,0,0,0,0,0 };// pass coordinate system in value[0] element - bool flags[] = { 1,0,0,0,0,0 }; - mp_queue_command(_exec_offset, value, flags); // second vector (flags) is not used, so fake it + cm_set_display_offsets(MODEL); // display new offsets in the model right now + + float value[] = { (float)cm->gm.coord_system }; // pass coordinate system in value[0] element + mp_queue_command(_exec_offset, value, nullptr); // second vector (flags) is not used, so fake it return (STAT_OK); } stat_t cm_cancel_tl_offset() { for (uint8_t axis = AXIS_X; axis < AXES; axis++) { - cm.tl_offset[axis] = 0; + cm->tool_offset[axis] = 0; } - float value[] = { (float)cm.gm.coord_system,0,0,0,0,0 };// pass coordinate system in value[0] element - bool flags[] = { 1,0,0,0,0,0 }; - mp_queue_command(_exec_offset, value, flags); // second vector (flags) is not used, so fake it + cm_set_display_offsets(MODEL); // display new offsets in the model right now + + float value[] = { (float)cm->gm.coord_system }; + mp_queue_command(_exec_offset, value, nullptr); // changes it in the runtime when executed return (STAT_OK); } -stat_t cm_set_coord_system(const uint8_t coord_system) // set coordinate system sync'd with planner +stat_t cm_set_coord_system(const uint8_t coord_system) // set coordinate system sync'd with planner { - cm.gm.coord_system = (cmCoordSystem)coord_system; + cm->gm.coord_system = (cmCoordSystem)coord_system; + cm_set_display_offsets(MODEL); // must reset display offsets if you change coordinate system - float value[] = { (float)coord_system,0,0,0,0,0 }; // pass coordinate system in value[0] element - bool flags[] = { 1,0,0,0,0,0 }; - mp_queue_command(_exec_offset, value, flags); // second vector (flags) is not used, so fake it + float value[] = { (float)coord_system }; + mp_queue_command(_exec_offset, value, nullptr); return (STAT_OK); } -static void _exec_offset(float *value, bool *flag) -{ - uint8_t coord_system = ((uint8_t)value[0]); // coordinate system is passed in value[0] element - float offsets[AXES]; - for (uint8_t axis = AXIS_X; axis < AXES; axis++) { - - offsets[axis] = cm.offset[coord_system][axis] + cm.tl_offset[axis] + - (cm.gmx.origin_offset[axis] * cm.gmx.origin_offset_enable); - } - mp_set_runtime_work_offset(offsets); - cm_set_work_offsets(MODEL); // set work offsets in the Gcode model -} - -/* - * cm_set_position() - set the position of a single axis in the model, planner and runtime +/****************************************************************************************** + * cm_set_position_by_axis() - set the position of a single axis in the model, planner and runtime + * cm_reset_position_to_absolute_position() - set all positions to current absolute position in mr * - * This command sets an axis/axes to a position provided as an argument. - * This is useful for setting origins for homing, probing, and other operations. + * This command sets an axis/axes to a position provided as an argument. + * This is useful for setting origins for homing, probing, and other operations. * - * !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! - * !!!!! DO NOT CALL THIS FUNCTION WHILE IN A MACHINING CYCLE !!!!! - * !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! + * !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! + * !!!!! DO NOT CALL THESE FUNCTIONS WHILE IN A MACHINING CYCLE !!!!! + * !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! * - * More specifically, do not call this function if there are any moves in the planner or - * if the runtime is moving. The system must be quiescent or you will introduce positional - * errors. This is true because the planned / running moves have a different reference frame - * than the one you are now going to set. These functions should only be called during - * initialization sequences and during cycles (such as homing cycles) when you know there - * are no more moves in the planner and that all motion has stopped. - * Use cm_get_runtime_busy() to be sure the system is quiescent. + * More specifically, do not call these functions if there are any moves in the planner or + * if the runtime is moving. The system must be quiescent or you will introduce positional + * errors. This is true because the planned / running moves have a different reference frame + * than the one you are now going to set. These functions should only be called during + * initialization sequences and during cycles (such as homing cycles) when you know there + * are no more moves in the planner and that all motion has stopped. + * You can use cm_get_runtime_busy() to be sure the system is quiescent. + * + * TODO: Turn this into a queued command so it executes from the planner */ -void cm_set_position(const uint8_t axis, const float position) +void cm_set_position_by_axis(const uint8_t axis, const float position) { - // TODO: Interlock involving runtime_busy test - cm.gmx.position[axis] = position; - cm.gm.target[axis] = position; + cm->gmx.position[axis] = position; + cm->gm.target[axis] = position; mp_set_planner_position(axis, position); mp_set_runtime_position(axis, position); mp_set_steps_to_runtime_position(); } -/*** G28.3 functions and support *** - * +void cm_reset_position_to_absolute_position(cmMachine_t *_cm) +{ + mpPlanner_t *_mp = (mpPlanner_t *)_cm->mp; + for (uint8_t axis = AXIS_X; axis < AXES; axis++) { + cm_set_position_by_axis(axis, mp_get_runtime_absolute_position(_mp->mr, axis)); + } +} + +/* * cm_set_absolute_origin() - G28.3 - model, planner and queue to runtime * _exec_absolute_origin() - callback from planner * @@ -1367,118 +1145,119 @@ void cm_set_position(const uint8_t axis, const float position) * as homed. */ +static void _exec_absolute_origin(float *value, bool *flag) +{ + for (uint8_t axis = AXIS_X; axis < AXES; axis++) { + if (flag[axis]) { + mp_set_runtime_position(axis, value[axis]); + cm->homed[axis] = true; // G28.3 is not considered homed until you get here + } + } + mp_set_steps_to_runtime_position(); +} + stat_t cm_set_absolute_origin(const float origin[], bool flag[]) { float value[AXES]; for (uint8_t axis = AXIS_X; axis < AXES; axis++) { if (flag[axis]) { -// REMOVED value[axis] = cm.offset[cm.gm.coord_system][axis] + _to_millimeters(origin[axis]); // G2 Issue #26 - value[axis] = _to_millimeters(origin[axis]); - cm.gmx.position[axis] = value[axis]; // set model position - cm.gm.target[axis] = value[axis]; // reset model target - mp_set_planner_position(axis, value[axis]); // set mm position +// REMOVED value[axis] = cm->offset[cm->gm.coord_system][axis] + _to_millimeters(origin[axis]); // G2 Issue #26 + value[axis] = _to_millimeters(origin[axis]); // replaced the above + cm->gmx.position[axis] = value[axis]; // set model position + cm->gm.target[axis] = value[axis]; // reset model target + mp_set_planner_position(axis, value[axis]); // set mm position } } mp_queue_command(_exec_absolute_origin, value, flag); return (STAT_OK); } -static void _exec_absolute_origin(float *value, bool *flag) -{ - for (uint8_t axis = AXIS_X; axis < AXES; axis++) { - if (flag[axis]) { - mp_set_runtime_position(axis, value[axis]); - cm.homed[axis] = true; // G28.3 is not considered homed until you get here - } - } - mp_set_steps_to_runtime_position(); -} - -/* - * cm_set_origin_offsets() - G92 - * cm_reset_origin_offsets() - G92.1 - * cm_suspend_origin_offsets() - G92.2 - * cm_resume_origin_offsets() - G92.3 +/****************************************************************************************** + * cm_set_g92_offsets() - G92 + * cm_reset_g92_offsets() - G92.1 + * cm_suspend_g92_offsets() - G92.2 + * cm_resume_g92_offsets() - G92.3 * * G92's behave according to NIST 3.5.18 & LinuxCNC G92 * http://linuxcnc.org/docs/html/gcode/gcode.html#sec:G92-G92.1-G92.2-G92.3 */ -stat_t cm_set_origin_offsets(const float offset[], const bool flag[]) +stat_t cm_set_g92_offsets(const float offset[], const bool flag[]) { // set offsets in the Gcode model extended context - cm.gmx.origin_offset_enable = true; + cm->gmx.g92_offset_enable = true; for (uint8_t axis = AXIS_X; axis < AXES; axis++) { if (flag[axis]) { - cm.gmx.origin_offset[axis] = cm.gmx.position[axis] - - cm.offset[cm.gm.coord_system][axis] - - cm.tl_offset[axis] - - _to_millimeters(offset[axis]); + cm->gmx.g92_offset[axis] = cm->gmx.position[axis] - + cm->coord_offset[cm->gm.coord_system][axis] - + cm->tool_offset[axis] - + _to_millimeters(offset[axis]); } } // now pass the offset to the callback - setting the coordinate system also applies the offsets - float value[] = { (float)cm.gm.coord_system,0,0,0,0,0 }; // pass coordinate system in value[0] element - bool flags[] = { 1,0,0,0,0,0 }; - mp_queue_command(_exec_offset, value, flags); + float value[] = { (float)cm->gm.coord_system }; // pass coordinate system in value[0] element + mp_queue_command(_exec_offset, value, nullptr); + cm_set_display_offsets(MODEL); return (STAT_OK); } -stat_t cm_reset_origin_offsets() +stat_t cm_reset_g92_offsets() { - cm.gmx.origin_offset_enable = false; + cm->gmx.g92_offset_enable = false; for (uint8_t axis = AXIS_X; axis < AXES; axis++) { - cm.gmx.origin_offset[axis] = 0; + cm->gmx.g92_offset[axis] = 0; } - float value[] = { (float)cm.gm.coord_system,0,0,0,0,0 }; - bool flags[] = { 1,0,0,0,0,0 }; - mp_queue_command(_exec_offset, value, flags); + float value[] = { (float)cm->gm.coord_system }; + mp_queue_command(_exec_offset, value, nullptr); + cm_set_display_offsets(MODEL); return (STAT_OK); } -stat_t cm_suspend_origin_offsets() +stat_t cm_suspend_g92_offsets() { - cm.gmx.origin_offset_enable = false; - float value[] = { (float)cm.gm.coord_system,0,0,0,0,0 }; - bool flags[] = { 1,0,0,0,0,0 }; - mp_queue_command(_exec_offset, value, flags); + cm->gmx.g92_offset_enable = false; + float value[] = { (float)cm->gm.coord_system }; + mp_queue_command(_exec_offset, value, nullptr); + cm_set_display_offsets(MODEL); return (STAT_OK); } -stat_t cm_resume_origin_offsets() +stat_t cm_resume_g92_offsets() { - cm.gmx.origin_offset_enable = true; - float value[] = { (float)cm.gm.coord_system,0,0,0,0,0 }; - bool flags[] = { 1,0,0,0,0,0 }; - mp_queue_command(_exec_offset, value, flags); + cm->gmx.g92_offset_enable = true; + float value[] = { (float)cm->gm.coord_system }; + mp_queue_command(_exec_offset, value, nullptr); + cm_set_display_offsets(MODEL); return (STAT_OK); } -/***************************** - * Free Space Motion (4.3.4) * - *****************************/ +/**************************************************************************************** + **** Free Space Motion (4.3.4) ********************************************************* + ****************************************************************************************/ /* * cm_straight_traverse() - G0 linear rapid */ -stat_t cm_straight_traverse(const float target[], const bool flags[]) +stat_t cm_straight_traverse(const float *target, const bool *flags, const uint8_t motion_profile) { - cm.gm.motion_mode = MOTION_MODE_STRAIGHT_TRAVERSE; + cm->gm.motion_mode = MOTION_MODE_STRAIGHT_TRAVERSE; // it's legal for a G0 to have no axis words but we don't want to process it - if (!(flags[AXIS_X] | flags[AXIS_Y] | flags[AXIS_Z] | flags[AXIS_A] | flags[AXIS_B] | flags[AXIS_C])) { + if (!(flags[AXIS_X] | flags[AXIS_Y] | flags[AXIS_Z] | + flags[AXIS_U] | flags[AXIS_V] | flags[AXIS_W] | + flags[AXIS_A] | flags[AXIS_B] | flags[AXIS_C])) { return(STAT_OK); } - cm_set_model_target(target, flags); - ritorno (cm_test_soft_limits(cm.gm.target)); // test soft limits; exit if thrown - cm_set_work_offsets(&cm.gm); // capture the fully resolved offsets to the state - cm_cycle_start(); // required for homing & other cycles - stat_t status = mp_aline(&cm.gm); // send the move to the planner - cm_finalize_move(); + ritorno (cm_test_soft_limits(cm->gm.target)); // test soft limits; exit if thrown + cm_set_display_offsets(&cm->gm); // capture the fully resolved offsets to the state + cm_cycle_start(); // required here for homing & other cycles + stat_t status = mp_aline(&cm->gm); // send the move to the planner + cm_update_model_position(); // update gmx.position to ready for next incoming move if (status == STAT_MINIMUM_LENGTH_MOVE) { - if (!mp_has_runnable_buffer()) { // handle condition where zero-length move is last or only move + if (!mp_has_runnable_buffer(mp)) { // handle condition where zero-length move is last or only move cm_cycle_end(); // ...otherwise cycle will not end properly } status = STAT_OK; @@ -1486,11 +1265,11 @@ stat_t cm_straight_traverse(const float target[], const bool flags[]) return (status); } -/* - * cm_set_g28_position() - G28.1 +/**************************************************************************************** * cm_goto_g28_position() - G28 - * cm_set_g30_position() - G30.1 + * cm_set_g28_position() - G28.1 * cm_goto_g30_position() - G30 + * cm_set_g30_position() - G30.1 * _goto_stored_position() - helper */ @@ -1499,27 +1278,28 @@ stat_t _goto_stored_position(const float stored_position[], // always in mm const bool flags[]) // all false if no intermediate move { // Go through intermediate point if one is provided - while (mp_planner_is_full()); // Make sure you have available buffers - ritorno(cm_straight_traverse(intermediate_target, flags)); // w/no action if no axis flags + while (mp_planner_is_full(mp)); // Make sure you have available buffers + ritorno(cm_straight_traverse(intermediate_target, flags, PROFILE_NORMAL)); // w/no action if no axis flags // If G20 adjust stored position (always in mm) to inches so traverse will be correct float target[AXES]; // make a local stored position as it may be modified copy_vector(target, stored_position); - if (cm.gm.units_mode == INCHES) { - for (uint8_t i=0; igm.units_mode == INCHES) { + for (uint8_t i=0; igmx.g28_position, cm->gmx.position); // in MM and machine coordinates return (STAT_OK); } stat_t cm_goto_g28_position(const float target[], const bool flags[]) { - return (_goto_stored_position(cm.gmx.g28_position, target, flags)); + return (_goto_stored_position(cm->gmx.g28_position, target, flags)); } stat_t cm_set_g30_position(void) { - copy_vector(cm.gmx.g30_position, cm.gmx.position); // in MM and machine coordinates + copy_vector(cm->gmx.g30_position, cm->gmx.position); // in MM and machine coordinates return (STAT_OK); } stat_t cm_goto_g30_position(const float target[], const bool flags[]) { - return (_goto_stored_position(cm.gmx.g30_position, target, flags)); + return (_goto_stored_position(cm->gmx.g30_position, target, flags)); } -/******************************** - * Machining Attributes (4.3.5) * - ********************************/ +/**************************************************************************************** + **** Machining Attributes (4.3.5) ****************************************************** + ****************************************************************************************/ /* * cm_set_feed_rate() - F parameter (affects MODEL only) * @@ -1558,18 +1338,18 @@ stat_t cm_goto_g30_position(const float target[], const bool flags[]) stat_t cm_set_feed_rate(const float feed_rate) { - if (cm.gm.feed_rate_mode == INVERSE_TIME_MODE) { + if (cm->gm.feed_rate_mode == INVERSE_TIME_MODE) { if (fp_ZERO(feed_rate)) { - return (STAT_GCODE_FEEDRATE_NOT_SPECIFIED); + return (STAT_FEEDRATE_NOT_SPECIFIED); } - cm.gm.feed_rate = 1/feed_rate; // normalize to minutes (NB: active for this gcode block only) + cm->gm.feed_rate = 1/feed_rate; // normalize to minutes (NB: active for this gcode block only) } else { - cm.gm.feed_rate = _to_millimeters(feed_rate); + cm->gm.feed_rate = _to_millimeters(feed_rate); } return (STAT_OK); } -/* +/**************************************************************************************** * cm_set_feed_rate_mode() - G93, G94 (affects MODEL only) * * INVERSE_TIME_MODE = 0, // G93 @@ -1579,11 +1359,11 @@ stat_t cm_set_feed_rate(const float feed_rate) stat_t cm_set_feed_rate_mode(const uint8_t mode) { - cm.gm.feed_rate_mode = (cmFeedRateMode)mode; + cm->gm.feed_rate_mode = (cmFeedRateMode)mode; return (STAT_OK); } -/* +/**************************************************************************************** * cm_set_path_control() - G61, G61.1, G64 */ @@ -1593,50 +1373,51 @@ stat_t cm_set_path_control(GCodeState_t *gcode_state, const uint8_t mode) return (STAT_OK); } - -/******************************* - * Machining Functions (4.3.6) * - *******************************/ +/**************************************************************************************** + **** Machining Functions (4.3.6) ******************************************************* + ****************************************************************************************/ /* * cm_arc_feed() - SEE plan_arc.cpp */ - -/* +/**************************************************************************************** * cm_dwell() - G4, P parameter (seconds) */ stat_t cm_dwell(const float seconds) { - cm.gm.parameter = seconds; + cm->gm.P_word = seconds; mp_dwell(seconds); return (STAT_OK); } -/* +/**************************************************************************************** * cm_straight_feed() - G1 */ -stat_t cm_straight_feed(const float target[], const bool flags[]) + +stat_t cm_straight_feed(const float *target, const bool *flags, const uint8_t motion_profile) { // trap zero feed rate condition - if (fp_ZERO(cm.gm.feed_rate)) { - return (STAT_GCODE_FEEDRATE_NOT_SPECIFIED); + if (fp_ZERO(cm->gm.feed_rate)) { + return (STAT_FEEDRATE_NOT_SPECIFIED); } - cm.gm.motion_mode = MOTION_MODE_STRAIGHT_FEED; + cm->gm.motion_mode = MOTION_MODE_STRAIGHT_FEED; - if (!(flags[AXIS_X] | flags[AXIS_Y] | flags[AXIS_Z] | flags[AXIS_A] | flags[AXIS_B] | flags[AXIS_C])) { + // it's legal for a G0 to have no axis words but we don't want to process it + if (!(flags[AXIS_X] | flags[AXIS_Y] | flags[AXIS_Z] | + flags[AXIS_U] | flags[AXIS_V] | flags[AXIS_W] | + flags[AXIS_A] | flags[AXIS_B] | flags[AXIS_C])) { return(STAT_OK); } cm_set_model_target(target, flags); - ritorno (cm_test_soft_limits(cm.gm.target)); // test soft limits; exit if thrown - cm_set_work_offsets(&cm.gm); // capture the fully resolved offsets to the state + ritorno (cm_test_soft_limits(cm->gm.target)); // test soft limits; exit if thrown + cm_set_display_offsets(&cm->gm); // capture the fully resolved offsets to the state cm_cycle_start(); // required for homing & other cycles - stat_t status = mp_aline(&cm.gm); // send the move to the planner - - cm_finalize_move(); // <-- ONLY safe because we don't care about status... + stat_t status = mp_aline(&cm->gm); // send the move to the planner + cm_update_model_position(); // <-- ONLY safe because we don't care about status... if (status == STAT_MINIMUM_LENGTH_MOVE) { - if (!mp_has_runnable_buffer()) { // handle condition where zero-length move is last or only move + if (!mp_has_runnable_buffer(mp)) { // handle condition where zero-length move is last or only move cm_cycle_end(); // ...otherwise cycle will not end properly } status = STAT_OK; @@ -1644,14 +1425,14 @@ stat_t cm_straight_feed(const float target[], const bool flags[]) return (status); } -/***************************** - * Spindle Functions (4.3.7) * - *****************************/ +/**************************************************************************************** + **** Spindle Functions (4.3.7) ********************************************************* + ****************************************************************************************/ // see spindle.cpp/.h -/************************** - * Tool Functions (4.3.8) * - **************************/ +/**************************************************************************************** + **** Tool Functions (4.3.8) ************************************************************ + ****************************************************************************************/ /* * cm_select_tool() - T parameter * _exec_select_tool() - execution callback @@ -1662,41 +1443,40 @@ stat_t cm_straight_feed(const float target[], const bool flags[]) * Note: These functions don't actually do anything for now, and there's a bug * where T and M in different blocks don't work correctly */ +static void _exec_select_tool(float *value, bool *flag) +{ + cm->gm.tool_select = (uint8_t)value[0]; +} + stat_t cm_select_tool(const uint8_t tool_select) { if (tool_select > TOOLS) { return (STAT_T_WORD_IS_INVALID); } - float value[] = { (float)tool_select, 0,0,0,0,0 }; - bool flags[] = { 1,0,0,0,0,0 }; - mp_queue_command(_exec_select_tool, value, flags); - return (STAT_OK); -} - -static void _exec_select_tool(float *value, bool *flag) -{ - cm.gm.tool_select = (uint8_t)value[0]; -} - -stat_t cm_change_tool(const uint8_t tool_change) -{ - float value[] = { (float)cm.gm.tool_select,0,0,0,0,0 }; - bool flags[] = { 1,0,0,0,0,0 }; - mp_queue_command(_exec_change_tool, value, flags); + float value[] = { (float)tool_select }; + mp_queue_command(_exec_select_tool, value, nullptr); return (STAT_OK); } static void _exec_change_tool(float *value, bool *flag) { - cm.gm.tool = (uint8_t)value[0]; + cm->gm.tool = (uint8_t)value[0]; + // TODO - change tool offsets and update display offsets } -/*********************************** - * Miscellaneous Functions (4.3.9) * - ***********************************/ +stat_t cm_change_tool(const uint8_t tool_change) +{ + float value[] = { (float)cm->gm.tool_select }; + mp_queue_command(_exec_change_tool, value, nullptr); + return (STAT_OK); +} + +/**************************************************************************************** + **** Miscellaneous Functions (4.3.9) *************************************************** + ****************************************************************************************/ // see coolant.cpp/.h -/* +/**************************************************************************************** * cm_message() - queue a RAM string as a message in the response (unconditionally) */ @@ -1705,31 +1485,29 @@ void cm_message(const char *message) nv_add_string((const char *)"msg", message); // add message to the response object } -/* +/**************************************************************************************** + **** Overrides ************************************************************************* + ****************************************************************************************/ + +/**************************************************************************************** * cm_reset_overrides() - reset manual feedrate and spindle overrides to initial conditions */ void cm_reset_overrides() { - cm.gmx.m48_enable = true; - cm.gmx.mfo_enable = false; // feed rate overrides - cm.gmx.mfo_factor = 1.0; - cm.gmx.mto_enable = false; // traverse overrides - cm.gmx.mto_factor = 1.0; + cm->gmx.m48_enable = true; + cm->gmx.mfo_enable = false; // feed rate overrides + cm->gmx.mfo_factor = 1.0; + cm->gmx.mto_enable = false; // traverse overrides + cm->gmx.mto_factor = 1.0; } -/* -static void _exec_feed_override(const bool m48_enable, const bool m50_enable, const float override_factor) -{ -} -*/ - -/* +/**************************************************************************************** * cm_m48_enable() - M48, M49 * * M48 is the master enable for manual feedrate override and spindle override - * If M48 is asserted M50 (mfo), M50.1 (mto) and M51 (sso) settings are in effect - * If M49 is asserted M50 (mfo), M501. (mto) and M51 (sso) settings are in ignored + * If M48 is asserted M50 (mfo), M50.1 (mto) and M51 (spo) settings are in effect + * If M49 is asserted M50 (mfo), M501. (mto) and M51 (spo) settings are in ignored * * See http://linuxcnc.org/docs/html/gcode/m-code.html#sec:M48,-M49-Speed-and-Feed-Override-Control */ @@ -1750,12 +1528,13 @@ stat_t cm_m48_enable(uint8_t enable) // M48, M49 { // handle changes to feed override given new state of m48/m49 - cm.gmx.m48_enable = enable; // update state + cm->gmx.m48_enable = enable; // update state return (STAT_OK); } -/* - * cm_mfo_control() - M50 manual feed rate override comtrol +/**************************************************************************************** + * cm_fro_control() - M50 feed rate override comtrol + * cm_tro_control() - M50.1 traverse override comtrol * * M50 enables manual feedrate override and the optional P override parameter. * P is expressed as M% to N% of programmed feedrate, typically a value from 0.05 to 2.000. @@ -1795,7 +1574,7 @@ stat_t cm_m48_enable(uint8_t enable) // M48, M49 * (Note: new ramp will supercede any existing ramp) */ -stat_t cm_mfo_control(const float P_word, const bool P_flag) // M50 +stat_t cm_fro_control(const float P_word, const bool P_flag) // M50 { bool new_enable = true; bool new_override = false; @@ -1809,22 +1588,22 @@ stat_t cm_mfo_control(const float P_word, const bool P_flag) // M50 if (P_word > FEED_OVERRIDE_MAX) { return (STAT_INPUT_EXCEEDS_MAX_VALUE); } - cm.gmx.mfo_factor = P_word; // P word is valid, store it. + cm->gmx.mfo_factor = P_word; // P word is valid, store it. new_override = true; } } - if (cm.gmx.m48_enable) { // if master enable is ON - if (new_enable && (new_override || !cm.gmx.mfo_enable)) { // 3 cases to start a ramp - mp_start_feed_override(FEED_OVERRIDE_RAMP_TIME, cm.gmx.mfo_factor); - } else if (cm.gmx.mfo_enable && !new_enable) { // case to turn off the ramp + if (cm->gmx.m48_enable) { // if master enable is ON + if (new_enable && (new_override || !cm->gmx.mfo_enable)) { // 3 cases to start a ramp + mp_start_feed_override(FEED_OVERRIDE_RAMP_TIME, cm->gmx.mfo_factor); + } else if (cm->gmx.mfo_enable && !new_enable) { // case to turn off the ramp mp_end_feed_override(FEED_OVERRIDE_RAMP_TIME); } } - cm.gmx.mfo_enable = new_enable; // always update the enable state + cm->gmx.mfo_enable = new_enable; // always update the enable state return (STAT_OK); } -stat_t cm_mto_control(const float P_word, const bool P_flag) // M50.1 +stat_t cm_tro_control(const float P_word, const bool P_flag) // M50.1 { bool new_enable = true; bool new_override = false; @@ -1838,237 +1617,50 @@ stat_t cm_mto_control(const float P_word, const bool P_flag) // M50.1 if (P_word > TRAVERSE_OVERRIDE_MAX) { return (STAT_INPUT_EXCEEDS_MAX_VALUE); } - cm.gmx.mto_factor = P_word; // P word is valid, store it. + cm->gmx.mto_factor = P_word; // P word is valid, store it. new_override = true; } } - if (cm.gmx.m48_enable) { // if master enable is ON - if (new_enable && (new_override || !cm.gmx.mfo_enable)) { // 3 cases to start a ramp - mp_start_traverse_override(FEED_OVERRIDE_RAMP_TIME, cm.gmx.mto_factor); - } else if (cm.gmx.mto_enable && !new_enable) { // case to turn off the ramp + if (cm->gmx.m48_enable) { // if master enable is ON + if (new_enable && (new_override || !cm->gmx.mfo_enable)) { // 3 cases to start a ramp + mp_start_traverse_override(FEED_OVERRIDE_RAMP_TIME, cm->gmx.mto_factor); + } else if (cm->gmx.mto_enable && !new_enable) { // case to turn off the ramp mp_end_traverse_override(FEED_OVERRIDE_RAMP_TIME); } } - cm.gmx.mto_enable = new_enable; // always update the enable state + cm->gmx.mto_enable = new_enable; // always update the enable state return (STAT_OK); } -/************************************************ - * Feedhold and Related Functions (no NIST ref) * - ************************************************/ -/* - * Feedholds, queue flushes and end_holds are all related. The request functions set flags - * or change state to "REQUESTED". The sequencing callback interprets the flags as so: - * - A feedhold request received during motion should be honored - * - A feedhold request received during a feedhold should be ignored - * - A feedhold request received during a motion stop should be ignored - * - * - A queue flush request should only be honored while in a feedhold - * - Said queue flush request received during a feedhold should be deferred until - * the feedhold enters a HOLD state (i.e. until deceleration is complete and motors stop). - * - A queue flush request received during a motion stop should be honored - * - * - An end_hold (cycle start) request should only be honored while in a feedhold - * - Said end_hold request received during a feedhold should be deferred until the - * feedhold enters a HOLD state (i.e. until deceleration is complete). - * If a queue flush request is also present the queue flush should be done first - * - * Below the request level, feedholds work like this: - * - The hold is initiated by calling cm_start_hold(). cm.hold_state is set to - * FEEDHOLD_SYNC, motion_state is set to MOTION_HOLD, and the spindle is turned off - * (if it it on). The remainder of feedhold - * processing occurs in plan_exec.c in the mp_exec_aline() function. - * - * - MOTION_HOLD and FEEDHOLD_SYNC tells mp_exec_aline() to begin feedhold processing - * after the current move segment is finished (< 5 ms later). (Cases handled by - * feedhold processing are listed in plan_exec.c). - * - * - FEEDHOLD_SYNC causes the current move in mr to be replanned into a deceleration. - * If the distance remaining in the executing move is sufficient for a full deceleration - * then motion will stop in the current block. Otherwise the deceleration phase - * will extend across as many blocks necessary until one will stop. - * - * - Once deceleration is complete hold state transitions to FEEDHOLD_HOLD and the - * distance remaining in the bf last block is replanned up from zero velocity. - * The move in the bf block is NOT released (unlike normal operation), as it - * will be used again to restart from hold. - * - * - When cm_end_hold() is called it releases the hold, restarts the move and restarts - * the spindle if the spindle is active. - */ -/* Queue Flush operation - * - * This one's complicated. See here first: - * https://github.com/synthetos/g2/wiki/Alarm-Processing - * https://github.com/synthetos/g2/wiki/Job-Exception-Handling - * - * We want to use queue flush for a few different use cases, as per the above wiki pages. - * The % behavior implements Exception Handling cases 1 and 2 - Stop a Single Move and - * Stop Multiple Moves. This is complicated further by the processing in single USB and - * dual USB being different. Also, the state handling is located in xio.cpp / readline(), - * controller.cpp _dispatch_kernel() and cm_request_queue_flush(), below. - * So it's documented here. - * - * Single or Dual USB Channels: - * - If a % is received outside of a feed hold or ALARM state, ignore it. - * Change the % to a ; comment symbol (xio) - * - * Single USB Channel Operation: - * - Enter a feedhold (!) - * - Receive a queue flush (%) Both dispatch it and store a marker (ACK) in the input - * buffer in place of the the % (xio) - * - Execute the feedhold to a hold condition (plan_exec) - * - Execute the dispatched % to flush queues (canonical_machine) - * - Silently reject any commands up to the % in the input queue (controller) - * - When ETX is encountered transition to STOP state (controller/canonical_machine) - * - * Dual USB Channel Operation: - * - Same as above except that we expect the % to arrive on the control channel - * - The system will read and dump all commands in the data channel until either a - * clear is encountered ({clear:n} or $clear), or an ETX is encountered on either - * channel, but it really should be on the data channel to ensure all queued commands - * are dumped. It is the host's responsibility to both write the clear (or ETX), and - * to ensure that it either arrives on the data channel or that the data channel is - * empty before writing it to the control channel. - */ -/* - * cm_request_feedhold() - * cm_request_end_hold() - cycle restart - * cm_request_queue_flush() - */ -void cm_request_feedhold(void) { - // honor request if not already in a feedhold and you are moving - if ((cm.hold_state == FEEDHOLD_OFF) && (cm.motion_state != MOTION_STOP)) { - cm.hold_state = FEEDHOLD_REQUESTED; - } -} - -void cm_request_end_hold(void) -{ - if (cm.hold_state != FEEDHOLD_OFF) { - cm.end_hold_requested = true; - } -} - -void cm_request_queue_flush() -{ - if ((cm.hold_state != FEEDHOLD_OFF) && // don't honor request unless you are in a feedhold - (cm.queue_flush_state == FLUSH_OFF)) { // ...and only once - cm.queue_flush_state = FLUSH_REQUESTED; // request planner flush once motion has stopped - - // NOTE: we used to flush the input buffers, but this is handled in xio *prior* to queue flush now - } -} - -/* - * cm_feedhold_sequencing_callback() - sequence feedhold, queue_flush, and end_hold requests - */ -stat_t cm_feedhold_sequencing_callback() -{ - if (cm.hold_state == FEEDHOLD_REQUESTED) { - cm_start_hold(); // feed won't run unless the machine is moving - } - if (cm.queue_flush_state == FLUSH_REQUESTED) { - cm_queue_flush(); // queue flush won't run until runtime is idle - } - if (cm.end_hold_requested) { - if (cm.queue_flush_state == FLUSH_OFF) { // either no flush or wait until it's done flushing - cm_end_hold(); - } - } - return (STAT_OK); -} - -/* - * cm_has_hold() - return true if a hold condition exists (or a pending hold request) - * cm_start_hold() - start a feedhhold by signalling the exec - * cm_end_hold() - end a feedhold by returning the system to normal operation - * cm_queue_flush() - Flush planner queue and correct model positions - */ -bool cm_has_hold() -{ - return (cm.hold_state != FEEDHOLD_OFF); -} - -void cm_start_hold() -{ - if (mp_has_runnable_buffer()) { // meaning there's something running - cm_spindle_optional_pause(spindle.pause_on_hold); // pause if this option is selected - cm_coolant_optional_pause(coolant.pause_on_hold); // pause if this option is selected - cm_set_motion_state(MOTION_HOLD); - cm.hold_state = FEEDHOLD_SYNC; // invokes hold from aline execution - } -} -void cm_end_hold() -{ - if (cm.hold_state == FEEDHOLD_HOLD) { - cm.end_hold_requested = false; - mp_exit_hold_state(); - - // State machine cases: - if (cm.machine_state == MACHINE_ALARM) { - cm_spindle_off_immediate(); - cm_coolant_off_immediate(); - - } else if (cm.motion_state == MOTION_STOP) { // && (! MACHINE_ALARM) - cm_spindle_off_immediate(); - cm_coolant_off_immediate(); - cm_cycle_end(); - - } else { // (MOTION_RUN || MOTION_PLANNING) && (! MACHINE_ALARM) - cm_cycle_start(); - cm_spindle_resume(spindle.dwell_seconds); - cm_coolant_resume(); - st_request_exec_move(); - } - } -} - -void cm_queue_flush() -{ - if (mp_runtime_is_idle()) { // can't flush planner during movement - mp_flush_planner(); - - for (uint8_t axis = AXIS_X; axis < AXES; axis++) { // set all positions - cm_set_position(axis, mp_get_runtime_absolute_position(axis)); - } - if(cm.hold_state == FEEDHOLD_HOLD) { // end feedhold if we're in one - cm_end_hold(); - } - cm.queue_flush_state = FLUSH_OFF; - qr_request_queue_report(0); // request a queue report, since we've changed the number of buffers available - } -} - -/****************************** - * Program Functions (4.3.10) * - ******************************/ +/**************************************************************************************** + **** Program Functions (4.3.10) ******************************************************** + ****************************************************************************************/ /* This group implements stop, start, and end functions. * It is extended beyond the NIST spec to handle various situations. * * _exec_program_finalize() - helper - * cm_cycle_start() - * cm_cycle_end() - * cm_program_stop() - M0 - * cm_optional_program_stop() - M1 - * cm_program_end() - M2, M30 + * cm_cycle_start() - sets MACHINE_CYCLE condition wwhich enables planner execution + * cm_cycle_end() - resets MACHINE_CYCLE + * cm_program_stop() - M0 - performs NIST STOP functions + * cm_optional_program_stop() - M1 - conditionally performs NIST STOP functions + * cm_program_end() - M2, M30 - performs NIST END functions (with some differences) */ /* * Program and cycle state functions * - * cm_program_stop and cm_optional_program_stop are synchronous Gcode commands - * that are received through the interpreter. They cause all motion to stop - * at the end of the current command, including spindle motion. + * cm_program_stop and cm_optional_program_stop are synchronous Gcode commands that are + * received through the interpreter. They cause all motion to stop at the end of the + * current command, including spindle motion. * - * Note that the stop occurs at the end of the immediately preceding command - * (i.e. the stop is queued behind the last command). + * Note that the stop occurs at the end of the immediately preceding command + * (i.e. the stop is queued behind the last command). * - * cm_program_end is a stop that also resets the machine to initial state + * cm_program_end is a stop that also resets the machine to initial state * - * cm_program_end() implements M2 and M30 - * The END behaviors are defined by NIST 3.6.1 are: - * 1a. Origin offsets are set to the default (like G54) - * 1b. Axis offsets are set to zero (like G92.2) + * cm_program_end() implements M2 and M30 + * The END behaviors are defined by NIST 3.6.1 are: + * 1a. Coordinate offsets are set to the default (like G54) + * 1b. G92 origin offsets are set to zero (like G92.2) * 2. Selected plane is set to CANON_PLANE_XY (like G17) * 3. Distance mode is set to MODE_ABSOLUTE (like G90) * 4. Feed rate mode is set to UNITS_PER_MINUTE (like G94) @@ -2080,103 +1672,103 @@ void cm_queue_flush() * * cm_program_end() implments things slightly differently (1a, 8): * 1a. Set default coordinate system (uses $gco, not G54) - * 1b. Axis offsets are SUSPENDED (G92.2) + * 1b. G92 origin offsets are SUSPENDED (G92.2) * 2. Selected plane is set to default plane ($gpl) * 3. Distance mode is set to MODE_ABSOLUTE (like G90) * 4. Feed rate mode is set to UNITS_PER_MINUTE (like G94) - * 5. Not implemented - * 6. Not implemented + * 5. Feed and speed overrides are set to ON (like M48) + * 6. (Cutter compensation not implemented) * 7. The spindle is stopped (like M5) * 8. Motion mode is CANCELED like G80 (not set to G1 as per NIST) * 9. Coolant is turned off (like M9) + * 10. Turn off all heaters and fans */ static void _exec_program_finalize(float *value, bool *flag) { - cm_set_motion_state(MOTION_STOP); + cmMachineState machine_state = (cmMachineState)value[0]; + cm_set_motion_state(MOTION_STOP); // also changes active model back to MODEL // Allow update in the alarm state, to accommodate queue flush (RAS) - if ((cm.cycle_state == CYCLE_MACHINING || cm.cycle_state == CYCLE_OFF) && -// (cm.machine_state != MACHINE_ALARM) && // omitted by OMC (RAS) - (cm.machine_state != MACHINE_SHUTDOWN)) { - cm.machine_state = (cmMachineState)value[0]; // don't update macs/cycs if we're in the middle of a canned cycle, - cm.cycle_state = CYCLE_OFF; // or if we're in machine alarm/shutdown mode + if ((cm->cycle_type == CYCLE_MACHINING || cm->cycle_type == CYCLE_NONE) && +// (cm->machine_state != MACHINE_ALARM) && // omitted by OMC (RAS) + (cm->machine_state != MACHINE_SHUTDOWN)) { + cm->machine_state = machine_state; // don't update macs/cycs if we're in the middle of a canned cycle, + cm->cycle_type = CYCLE_NONE; // or if we're in machine alarm/shutdown mode } // reset the rest of the states - cm.cycle_state = CYCLE_OFF; - cm.hold_state = FEEDHOLD_OFF; - mp_zero_segment_velocity(); // for reporting purposes + cm->cycle_type = CYCLE_NONE; + cm->hold_state = FEEDHOLD_OFF; + mp_zero_segment_velocity(); // for reporting purposes // perform the following resets if it's a program END - if (((uint8_t)value[0]) == MACHINE_PROGRAM_END) { - cm_suspend_origin_offsets(); // G92.2 - as per NIST -// cm_reset_origin_offsets(); // G92.1 - alternative to above - cm_set_coord_system(cm.default_coord_system); // reset to default coordinate system - cm_select_plane(cm.default_select_plane); // reset to default arc plane - cm_set_distance_mode(cm.default_distance_mode); - cm_set_arc_distance_mode(INCREMENTAL_DISTANCE_MODE); // always the default - cm_spindle_off_immediate(); // M5 - cm_coolant_off_immediate(); // M9 - cm_set_feed_rate_mode(UNITS_PER_MINUTE_MODE); // G94 + if (machine_state == MACHINE_PROGRAM_END) { + cm_suspend_g92_offsets(); // G92.2 - as per NIST + cm_set_coord_system(cm->default_coord_system); // reset to default coordinate system + cm_select_plane(cm->default_select_plane); // reset to default arc plane + cm_set_distance_mode(cm->default_distance_mode); // reset to default distance mode + cm_set_arc_distance_mode(INCREMENTAL_DISTANCE_MODE);// always the default +// toolhead.control_immediate(TOOLHEAD_OFF); // M5 + spindle_control_immediate(SPINDLE_OFF); // M5 + coolant_control_immediate(COOLANT_OFF,COOLANT_BOTH);// M9 + cm_set_feed_rate_mode(UNITS_PER_MINUTE_MODE); // G94 cm_set_motion_mode(MODEL, MOTION_MODE_CANCEL_MOTION_MODE);// NIST specifies G1 (MOTION_MODE_STRAIGHT_FEED), but we cancel motion mode. Safer. - cm_reset_overrides(); // reset feedrate the spindle overrides - temperature_reset(); // turn off all heaters and fans + cm_reset_overrides(); // enable G48, reset feed rate, traverse and spindle overrides + temperature_reset(); // turn off all heaters and fans } - sr_request_status_report(SR_REQUEST_IMMEDIATE); // request a final and full status report (not filtered) + sr_request_status_report(SR_REQUEST_IMMEDIATE); // request a final and full status report (not filtered) } +// Will start a cycle regardless of whether the planner has moves or not void cm_cycle_start() { - if (cm.cycle_state == CYCLE_OFF) { // don't (re)start homing, probe or other canned cycles - cm.machine_state = MACHINE_CYCLE; - cm.cycle_state = CYCLE_MACHINING; - - // kn->get_position(mp.position); // init planner position + if (cm->cycle_type == CYCLE_NONE) { // don't (re)start homing, probe or other canned cycles + cm->cycle_type = CYCLE_MACHINING; + cm->machine_state = MACHINE_CYCLE; + qr_init_queue_report(); // clear queue reporting buffer counts// } } void cm_cycle_end() { - if(cm.cycle_state == CYCLE_MACHINING) { - float value[] = { (float)MACHINE_PROGRAM_STOP, 0,0,0,0,0 }; - bool flags[] = { 1,0,0,0,0,0 }; - _exec_program_finalize(value, flags); + if (cm->cycle_type == CYCLE_MACHINING) { + float value[] = { (float)MACHINE_PROGRAM_STOP }; + _exec_program_finalize(value, nullptr); } } void cm_canned_cycle_end() { - cm.cycle_state = CYCLE_OFF; - float value[] = { (float)MACHINE_PROGRAM_STOP, 0,0,0,0,0 }; - bool flags[] = { 1,0,0,0,0,0 }; - _exec_program_finalize(value, flags); + cm->cycle_type = CYCLE_NONE; + float value[] = { (float)MACHINE_PROGRAM_STOP }; + _exec_program_finalize(value, nullptr); } void cm_program_stop() { - float value[] = { (float)MACHINE_PROGRAM_STOP, 0,0,0,0,0 }; - bool flags[] = { 1,0,0,0,0,0 }; - mp_queue_command(_exec_program_finalize, value, flags); + float value[] = { (float)MACHINE_PROGRAM_STOP }; + mp_queue_command(_exec_program_finalize, value, nullptr); } void cm_optional_program_stop() { - float value[] = { (float)MACHINE_PROGRAM_STOP, 0,0,0,0,0 }; - bool flags[] = { 1,0,0,0,0,0 }; - mp_queue_command(_exec_program_finalize, value, flags); + float value[] = { (float)MACHINE_PROGRAM_STOP }; + mp_queue_command(_exec_program_finalize, value, nullptr); } void cm_program_end() { - float value[] = { (float)MACHINE_PROGRAM_END, 0,0,0,0,0 }; - bool flags[] = { 1,0,0,0,0,0 }; - mp_queue_command(_exec_program_finalize, value, flags); + float value[] = { (float)MACHINE_PROGRAM_END }; + mp_queue_command(_exec_program_finalize, value, nullptr); } - +/**************************************************************************************** + **** Additional Functions ************************************************************** + ****************************************************************************************/ /* * cm_json_command() - M100 + * cm_json_wait() - M102 */ stat_t cm_json_command(char *json_string) { @@ -2199,22 +1791,163 @@ stat_t cm_json_wait(char *json_string) return mp_json_wait(json_string); } +/**************************************************************************************** + * cm_run_home() - run homing sequence + */ + +stat_t cm_run_home(nvObj_t *nv) +{ + if (nv->value_int) { // if true + float axes[] = INIT_AXES_ONES; + bool flags[] = INIT_AXES_TRUE; + cm_homing_cycle_start(axes, flags); + } + return (STAT_OK); +} + +/**************************************************************************************** + * Jogging Commands + * + * cm_get_jogging_dest() + * cm_run_jog() + */ + +float cm_get_jogging_dest(void) +{ + return cm->jogging_dest; +} + +stat_t cm_run_jog(nvObj_t *nv) +{ + set_float(nv, cm->jogging_dest); + cm_jogging_cycle_start(_axis(nv)); + return (STAT_OK); +} /************************************** * END OF CANONICAL MACHINE FUNCTIONS * **************************************/ -/*********************************************************************************** - * CONFIGURATION AND INTERFACE FUNCTIONS +/**************************************************************************************** + **** CONFIGURATION AND INTERFACE FUNCTIONS ********************************************* + **************************************************************************************** * Functions to get and set variables from the cfgArray table * These functions are not part of the NIST defined functions - ***********************************************************************************/ + ****************************************************************************************/ -// Strings for writing settings as nvObj string values -// Ref: http://www.avrfreaks.net/index.php?name=PNphpBB2&file=printview&t=120881&start=0 +/***** AXIS HELPERS ********************************************************************** + * _coord() - return coordinate system number (53=0...59=6) or -1 if error + * _axis() - return axis # or -1 if not an axis (works for mapped motors as well) + * cm_get_axis_type() - return linear axis (0), rotary axis (1) or error (-1) + * cm_get_axis_char() - return ASCII char for internal axis number provided + */ + +static int8_t _coord(nvObj_t *nv) // extract coordinate system from 3rd character +{ + char *ptr = ((*nv->group == 0) ? &nv->token[1] : &nv->group[1]); // skip past the 'g' to the number + return (max ((atoi(ptr)-53), -1)); // return G54-G59 as 0-5, error as -1 +} + +/* _axis() + * + * Cases handled: + * - sys/... value is a system parameter (global), there is no axis (AXIS_TYPE_SYSTEM) + * - xam, yvm any prefixed axis parameter + * - 1ma, 2tr any motor parameter will return mapped axis for that motor + * - posx, mpox readouts + * - g54x, g92z offsets + * - tofx tool offsets + * - tt1x, tt32x tool table + * - _tex, _tra diagnostic parameters + * - u,v,w handles U, V and W axes + * + * Note that this function will return an erroneous value if called by a non-axis tag, + * such as 'coph' But it should not be called in these cases in any event. + */ + +static int8_t _axis(const nvObj_t *nv) +{ + // test if this is a SYS parameter (global), in which case there will be no axis + if (strcmp("sys", cfgArray[nv->index].group) == 0) { + return (AXIS_TYPE_SYSTEM); + } + + // if the leading character of the token is a number it's a motor + char c = cfgArray[nv->index].token[0]; + + if (isdigit(c)) { + return(st_cfg.mot[c-0x31].motor_map); // return the axis associated with the motor + } + + // otherwise it's an axis. Or undefined, which is usually a global. + char *ptr; + char axes[] = {"xyzuvwabc"}; + + if ((ptr = strchr(axes, c)) == NULL) { // not NULL indicates a prefixed axis + c = *(cfgArray[nv->index].token + strlen(cfgArray[nv->index].token) -1); // get the last character + if ((ptr = strchr(axes, c)) == NULL) { // test for a postfixed axis + return (AXIS_TYPE_UNDEFINED); + } + } + return (ptr - axes); +} + +cmAxisType cm_get_axis_type(const nvObj_t *nv) +{ + int8_t axis = _axis(nv); + if (axis <= AXIS_TYPE_UNDEFINED) { + return ((cmAxisType)axis); + } + if (axis >= AXIS_A) { + return (AXIS_TYPE_ROTARY); + } + return (AXIS_TYPE_LINEAR); +} + +char cm_get_axis_char(const int8_t axis) // Uses internal axis numbering +{ + char axis_char[] = "XYZUVWABC"; + if ((axis < 0) || (axis > AXES)) return (' '); + return (axis_char[axis]); +} + +/**** Functions called directly from cfgArray table - mostly wrappers **** + * _get_msg_helper() - helper to get string values + * + * cm_get_stat() - get combined machine state as value and string + * cm_get_macs() - get raw machine state as value and string + * cm_get_cycs() - get raw cycle state as value and string + * cm_get_mots() - get raw motion state as value and string + * cm_get_hold() - get raw hold state as value and string + * cm_get_home() - get raw homing state as value and string + * + * cm_get_unit() - get units mode as integer and display string + * cm_get_coor() - get goodinate system + * cm_get_momo() - get runtime motion mode + * cm_get_plan() - get model plane select + * cm_get_path() - get model path control mode + * cm_get_dist() - get model distance mode + * cm_get_admo() - get model arc distance mode + * cm_get_frmo() - get model feed rate mode + * cm_get_tool() - get tool + * cm_get_feed() - get feed rate + * cm_get_mline() - get model line number for status reports + * cm_get_line() - get active (model or runtime) line number for status reports + * cm_get_vel() - get runtime velocity + * cm_get_ofs() - get current work offset (runtime) + * cm_get_pos() - get current work position (runtime) + * cm_get_mpos() - get current machine position (runtime) + * + * cm_print_pos() - print work position (with proper units) + * cm_print_mpos()- print machine position (always mm units) + * cm_print_coor()- print coordinate offsets with linear units + * cm_print_corr()- print coordinate offsets with rotary units + */ #ifdef __TEXT_MODE +// Strings for text mode displays: + static const char msg_units0[] = " in"; // used by generic print functions static const char msg_units1[] = " mm"; static const char msg_units2[] = " deg"; @@ -2246,9 +1979,9 @@ static const char msg_stat11[] = "Interlock"; static const char msg_stat12[] = "Shutdown"; static const char msg_stat13[] = "Panic"; static const char *const msg_stat[] = { msg_stat0, msg_stat1, msg_stat2, msg_stat3, - msg_stat4, msg_stat5, msg_stat6, msg_stat7, - msg_stat8, msg_stat9, msg_stat10, msg_stat11, - msg_stat12, msg_stat13 }; + msg_stat4, msg_stat5, msg_stat6, msg_stat7, + msg_stat8, msg_stat9, msg_stat10, msg_stat11, + msg_stat12, msg_stat13 }; static const char msg_macs0[] = "Initializing"; static const char msg_macs1[] = "Ready"; @@ -2260,8 +1993,8 @@ static const char msg_macs6[] = "Interlock"; static const char msg_macs7[] = "SHUTDOWN"; static const char msg_macs8[] = "PANIC"; static const char *const msg_macs[] = { msg_macs0, msg_macs1, msg_macs2, msg_macs3, - msg_macs4, msg_macs5, msg_macs6, msg_macs7, - msg_macs8 }; + msg_macs4, msg_macs5, msg_macs6, msg_macs7, + msg_macs8 }; static const char msg_cycs0[] = "Off"; static const char msg_cycs1[] = "Machining"; @@ -2280,18 +2013,29 @@ static const char msg_hold0[] = "Off"; static const char msg_hold1[] = "Requested"; static const char msg_hold2[] = "Sync"; static const char msg_hold3[] = "Decel Continue"; -static const char msg_hold4[] = "Decel to Zero"; -static const char msg_hold5[] = "Decel Done"; -static const char msg_hold6[] = "Pending"; -static const char msg_hold7[] = "Hold"; -static const char *const msg_hold[] = { msg_hold0, msg_hold1, msg_hold2, msg_hold3, - msg_hold4, msg_hold5, msg_hold6, msg_hold7 }; +static const char msg_hold4[] = "Decel To Zero"; +static const char msg_hold5[] = "Decel Complete"; +static const char msg_hold6[] = "Motion Stopping"; +static const char msg_hold7[] = "Motion Stopped"; +static const char msg_hold8[] = "Hold Actions Pending"; +static const char msg_hold9[] = "Hold Actions Complete"; +static const char msg_hold10[] = "Holding"; +static const char msg_hold11[] = "Hold Exit Actions Pending"; +static const char msg_hold12[] = "Hold Exit Actions Complete"; +static const char *const msg_hold[] = { msg_hold0, msg_hold1, msg_hold2, msg_hold3, msg_hold4, + msg_hold5, msg_hold6, msg_hold7, msg_hold8, msg_hold9, + msg_hold10, msg_hold11, msg_hold12 }; static const char msg_home0[] = "Not Homed"; static const char msg_home1[] = "Homed"; static const char msg_home2[] = "Homing"; static const char *const msg_home[] = { msg_home0, msg_home1, msg_home2 }; +static const char msg_probe0[] = "Probe Failed"; +static const char msg_probe1[] = "Probe Succeeded"; +static const char msg_probe2[] = "Probe Waiting"; +static const char *const msg_probe[] = { msg_probe0, msg_probe1, msg_probe2 }; + static const char msg_g53[] = "G53 - machine coordinate system"; static const char msg_g54[] = "G54 - coordinate system 1"; static const char msg_g55[] = "G55 - coordinate system 2"; @@ -2352,129 +2096,20 @@ static const char *const msg_frmo[] = { msg_g93, msg_g94, msg_g95 }; #endif // __TEXT_MODE - -/***** AXIS HELPERS ***************************************************************** - * _get_axis() - return axis # or -1 if not an axis (works for mapped motors as well) - * _coord() - return coordinate system number or -1 if error - * cm_get_axis_char() - return ASCII char for axis given the axis number - * cm_get_axis_type() - return linear axis (0), rotary axis (1) or error (-1) - */ - -/* _get_axis() - * - * Cases that are handled by _get_axis(): - * - sys/... value is a system parameter (global), there is no axis - * - xam any axis parameter will return the axis number - * - 1ma any motor parameter will return the mapped axis for that motor - * - 1su an example of the above - * - mpox readouts - * - g54x offsets - * - tlx tool length offset - * - tt1x tool table - * - tt32x tool table - * - _tex diagnostic parameters - */ - -static int8_t _get_axis(const index_t index) +//_get_msg_helper() - add the string for the enum to the nv, but leave it as a TYPE_INTEGER +stat_t _get_msg_helper(nvObj_t *nv, const char *const msg_array[], int32_t value) { - // test if this is a SYS parameter (global), in which case there will be no axis - if (strcmp("sys", cfgArray[index].group) == 0) { - return (AXIS_TYPE_SYSTEM); - } - - // if the leading character of the token is a number it's a motor - char c = cfgArray[index].token[0]; - if (isdigit(cfgArray[index].token[0])) { - return(st_cfg.mot[c-0x31].motor_map); - } - - // otherwise it's an axis. Or undefined, which is usually a global. - char *ptr; - char axes[] = {"xyzabc"}; - if ((ptr = strchr(axes, c)) == NULL) { // test the character in the 0 and 3 positions - if ((ptr = strchr(axes, cfgArray[index].token[3])) == NULL) { // to accommodate 'xam' and 'g54x' styles - return (AXIS_TYPE_UNDEFINED); - } - } - return (ptr - axes); -} - -/**** not used yet **** -static int8_t _coord(char *token) // extract coordinate system from 3rd character -{ - char *ptr; - char coord_list[] = {"456789"}; - - if ((ptr = strchr(coord_list, token[2])) == NULL) { // test the 3rd character against the string - return (-1); - } - return (ptr - coord_list); -} -*/ - -char cm_get_axis_char(const int8_t axis) -{ - char axis_char[] = "XYZABC"; - if ((axis < 0) || (axis > AXES)) return (' '); - return (axis_char[axis]); -} - -cmAxisType cm_get_axis_type(const index_t index) -{ - int8_t axis = _get_axis(index); - if (axis == AXIS_TYPE_UNDEFINED) { return (AXIS_TYPE_UNDEFINED); } - if (axis == AXIS_TYPE_SYSTEM) { return (AXIS_TYPE_SYSTEM); } - if (axis >= AXIS_A) { return (AXIS_TYPE_ROTARY); } - return (AXIS_TYPE_LINEAR); -} - -/**** Functions called directly from cfgArray table - mostly wrappers **** - * _get_msg_helper() - helper to get string values - * - * cm_get_stat() - get combined machine state as value and string - * cm_get_macs() - get raw machine state as value and string - * cm_get_cycs() - get raw cycle state as value and string - * cm_get_mots() - get raw motion state as value and string - * cm_get_hold() - get raw hold state as value and string - * cm_get_home() - get raw homing state as value and string - * - * cm_get_unit() - get units mode as integer and display string - * cm_get_coor() - get goodinate system - * cm_get_momo() - get runtime motion mode - * cm_get_plan() - get model plane select - * cm_get_path() - get model path control mode - * cm_get_dist() - get model distance mode - * cm_get_admo() - get model arc distance mode - * cm_get_frmo() - get model feed rate mode - * cm_get_tool() - get tool - * cm_get_feed() - get feed rate - * cm_get_mline()- get model line number for status reports - * cm_get_line() - get active (model or runtime) line number for status reports - * cm_get_vel() - get runtime velocity - * cm_get_ofs() - get current work offset (runtime) - * cm_get_pos() - get current work position (runtime) - * cm_get_mpos() - get current machine position (runtime) - * - * cm_print_pos()- print work position (with proper units) - * cm_print_mpos()- print machine position (always mm units) - * cm_print_coor()- print coordinate offsets with linear units - * cm_print_corr()- print coordinate offsets with rotary units - */ - -// Add the string for the enum to the nv, but leave it as a TYPE_INT -stat_t _get_msg_helper(nvObj_t *nv, const char *const msg_array[], uint8_t value) -{ - nv->value = (float)value; - nv->valuetype = TYPE_INT; + nv->value_int = value; + nv->valuetype = TYPE_INTEGER; return(nv_copy_string(nv, (const char *)GET_TEXT_ITEM(msg_array, value))); } -stat_t cm_get_stat(nvObj_t *nv) { return(_get_msg_helper(nv, msg_stat, cm_get_combined_state()));} +stat_t cm_get_stat(nvObj_t *nv) { return(_get_msg_helper(nv, msg_stat, cm_get_combined_state(&cm1)));} +stat_t cm_get_stat2(nvObj_t *nv){ return(_get_msg_helper(nv, msg_stat, cm_get_combined_state(&cm2)));} stat_t cm_get_macs(nvObj_t *nv) { return(_get_msg_helper(nv, msg_macs, cm_get_machine_state()));} -stat_t cm_get_cycs(nvObj_t *nv) { return(_get_msg_helper(nv, msg_cycs, cm_get_cycle_state()));} +stat_t cm_get_cycs(nvObj_t *nv) { return(_get_msg_helper(nv, msg_cycs, cm_get_cycle_type()));} stat_t cm_get_mots(nvObj_t *nv) { return(_get_msg_helper(nv, msg_mots, cm_get_motion_state()));} stat_t cm_get_hold(nvObj_t *nv) { return(_get_msg_helper(nv, msg_hold, cm_get_hold_state()));} -stat_t cm_get_home(nvObj_t *nv) { return(_get_msg_helper(nv, msg_home, cm_get_homing_state()));} stat_t cm_get_unit(nvObj_t *nv) { return(_get_msg_helper(nv, msg_unit, cm_get_units_mode(ACTIVE_MODEL)));} stat_t cm_get_coor(nvObj_t *nv) { return(_get_msg_helper(nv, msg_coor, cm_get_coord_system(ACTIVE_MODEL)));} @@ -2485,35 +2120,18 @@ stat_t cm_get_dist(nvObj_t *nv) { return(_get_msg_helper(nv, msg_dist, cm_get_di stat_t cm_get_admo(nvObj_t *nv) { return(_get_msg_helper(nv, msg_admo, cm_get_arc_distance_mode(ACTIVE_MODEL)));} stat_t cm_get_frmo(nvObj_t *nv) { return(_get_msg_helper(nv, msg_frmo, cm_get_feed_rate_mode(ACTIVE_MODEL)));} -stat_t cm_get_toolv(nvObj_t *nv) -{ - nv->value = (float)cm_get_tool(ACTIVE_MODEL); - nv->valuetype = TYPE_INT; - return (STAT_OK); -} - -stat_t cm_get_mline(nvObj_t *nv) -{ - nv->value = (float)cm_get_linenum(MODEL); - nv->valuetype = TYPE_INT; - return (STAT_OK); -} - -stat_t cm_get_line(nvObj_t *nv) -{ - nv->value = (float)cm_get_linenum(ACTIVE_MODEL); - nv->valuetype = TYPE_INT; - return (STAT_OK); -} +stat_t cm_get_toolv(nvObj_t *nv) { return(get_integer(nv, cm_get_tool(ACTIVE_MODEL))); } +stat_t cm_get_mline(nvObj_t *nv) { return(get_integer(nv, cm_get_linenum(MODEL))); } +stat_t cm_get_line(nvObj_t *nv) { return(get_integer(nv, cm_get_linenum(ACTIVE_MODEL))); } stat_t cm_get_vel(nvObj_t *nv) { if (cm_get_motion_state() == MOTION_STOP) { - nv->value = 0; + nv->value_flt = 0; } else { - nv->value = mp_get_runtime_velocity(); + nv->value_flt = mp_get_runtime_velocity(); if (cm_get_units_mode(RUNTIME) == INCHES) { - nv->value *= INCHES_PER_MM; + nv->value_flt *= INCHES_PER_MM; } } nv->precision = GET_TABLE_WORD(precision); @@ -2521,117 +2139,108 @@ stat_t cm_get_vel(nvObj_t *nv) return (STAT_OK); } -stat_t cm_get_feed(nvObj_t *nv) +stat_t cm_get_feed(nvObj_t *nv) { return (get_float(nv, cm_get_feed_rate(ACTIVE_MODEL))); } +stat_t cm_get_pos(nvObj_t *nv) { return (get_float(nv, cm_get_display_position(ACTIVE_MODEL, _axis(nv)))); } +stat_t cm_get_mpo(nvObj_t *nv) { return (get_float(nv, cm_get_absolute_position(ACTIVE_MODEL, _axis(nv)))); } +stat_t cm_get_ofs(nvObj_t *nv) { return (get_float(nv, cm_get_display_offset(ACTIVE_MODEL, _axis(nv)))); } + +stat_t cm_get_home(nvObj_t *nv) { return(_get_msg_helper(nv, msg_home, cm_get_homing_state())); } +stat_t cm_set_home(nvObj_t *nv) { return (set_integer(nv, ((uint8_t &)(cm->homing_state)), false, true)); } +stat_t cm_get_hom(nvObj_t *nv) { return (get_integer(nv, cm->homed[_axis(nv)])); } + +stat_t cm_get_prob(nvObj_t *nv) { return(_get_msg_helper(nv, msg_probe, cm_get_probe_state())); } +stat_t cm_get_prb(nvObj_t *nv) { return (get_float(nv, cm->probe_results[0][_axis(nv)])); } + +stat_t cm_get_coord(nvObj_t *nv) { return (get_float(nv, cm->coord_offset[_coord(nv)][_axis(nv)])); } +stat_t cm_set_coord(nvObj_t *nv) { return (set_float(nv, cm->coord_offset[_coord(nv)][_axis(nv)])); } + +stat_t cm_get_g92e(nvObj_t *nv) { return (get_integer(nv, cm->gmx.g92_offset_enable)); } +stat_t cm_get_g92(nvObj_t *nv) { return (get_float(nv, cm->gmx.g92_offset[_axis(nv)])); } +stat_t cm_get_g28(nvObj_t *nv) { return (get_float(nv, cm->gmx.g28_position[_axis(nv)])); } +stat_t cm_get_g30(nvObj_t *nv) { return (get_float(nv, cm->gmx.g30_position[_axis(nv)])); } + +/***************************************************** + **** TOOL TABLE AND OFFSET GET AND SET FUNCTIONS **** + *****************************************************/ + +static uint8_t _tool(nvObj_t *nv) { - nv->value = cm_get_feed_rate(ACTIVE_MODEL); - if (cm_get_units_mode(ACTIVE_MODEL) == INCHES) { - nv->value *= INCHES_PER_MM; + if (nv->group[0] != 0) { + return (atoi(&nv->group[2])); // ttNN is the group, axis is in the token } - nv->precision = GET_TABLE_WORD(precision); - nv->valuetype = TYPE_FLOAT; - return (STAT_OK); + return (atoi(&nv->token[2])); // ttNNx is all in the token } -stat_t cm_get_pos(nvObj_t *nv) +stat_t cm_get_tof(nvObj_t *nv) { return (get_float(nv, cm->tool_offset[_axis(nv)])); } +stat_t cm_set_tof(nvObj_t *nv) { return (set_float(nv, cm->tool_offset[_axis(nv)])); } + +stat_t cm_get_tt(nvObj_t *nv) { - nv->value = cm_get_work_position(ACTIVE_MODEL, _get_axis(nv->index)); - nv->precision = GET_TABLE_WORD(precision); - nv->valuetype = TYPE_FLOAT; - return (STAT_OK); + uint8_t toolnum = _tool(nv); + if (toolnum > TOOLS) { + return (STAT_INPUT_EXCEEDS_MAX_VALUE); + } + return (get_float(nv, tt.tt_offset[toolnum][_axis(nv)])); } -stat_t cm_get_mpo(nvObj_t *nv) +stat_t cm_set_tt(nvObj_t *nv) { - nv->value = cm_get_absolute_position(ACTIVE_MODEL, _get_axis(nv->index)); - nv->precision = GET_TABLE_WORD(precision); - nv->valuetype = TYPE_FLOAT; - return (STAT_OK); -} - -stat_t cm_get_ofs(nvObj_t *nv) -{ - nv->value = cm_get_work_offset(ACTIVE_MODEL, _get_axis(nv->index)); - nv->precision = GET_TABLE_WORD(precision); - nv->valuetype = TYPE_FLOAT; - return (STAT_OK); -} - -stat_t cm_get_tof(nvObj_t *nv) -{ - nv->value = cm.tl_offset[_get_axis(nv->index)]; - nv->precision = GET_TABLE_WORD(precision); - nv->valuetype = TYPE_FLOAT; - return (STAT_OK); + uint8_t toolnum = _tool(nv); + if (toolnum > TOOLS) { + return (STAT_INPUT_EXCEEDS_MAX_VALUE); + } + return(set_float(nv, tt.tt_offset[toolnum][_axis(nv)])); } +/************************************ + **** AXIS GET AND SET FUNCTIONS **** + ************************************/ /* - * AXIS GET AND SET FUNCTIONS - * * cm_get_am() - get axis mode w/enumeration string * cm_set_am() - set axis mode w/exception handling for axis type - * cm_set_hi() - set homing input + * cm_get_tn() - get axis travel min + * cm_set_tn() - set axis travel min + * cm_get_tm() - get axis travel max + * cm_set_tm() - set axis travel max */ stat_t cm_get_am(nvObj_t *nv) { - get_ui8(nv); - return(_get_msg_helper(nv, msg_am, nv->value)); + int8_t axis = _axis(nv); + nv->value_int = cm->a[axis].axis_mode; + return(_get_msg_helper(nv, msg_am, nv->value_int)); } stat_t cm_set_am(nvObj_t *nv) // axis mode { - if (cm_get_axis_type(nv->index) == 0) { // linear - if (nv->value > AXIS_MODE_MAX_LINEAR) { + if (cm_get_axis_type(nv) == AXIS_TYPE_LINEAR) { + if (nv->value_int > AXIS_MODE_LINEAR_MAX) { nv->valuetype = TYPE_NULL; return (STAT_INPUT_EXCEEDS_MAX_VALUE); } } else { - if (nv->value > AXIS_MODE_MAX_ROTARY) { + if (nv->value_int > AXIS_MODE_ROTARY_MAX) { nv->valuetype = TYPE_NULL; return (STAT_INPUT_EXCEEDS_MAX_VALUE); } } - set_ui8(nv); + nv->valuetype = TYPE_INTEGER; + cm->a[_axis(nv)].axis_mode = (cmAxisMode)nv->value_int; return(STAT_OK); } -stat_t cm_set_hi(nvObj_t *nv) -{ - if (nv->value < 0) { - nv->valuetype = TYPE_NULL; - return (STAT_INPUT_LESS_THAN_MIN_VALUE); - } - if (nv->value > D_IN_CHANNELS) { - nv->valuetype = TYPE_NULL; - return (STAT_INPUT_EXCEEDS_MAX_VALUE); - } - set_ui8(nv); - return (STAT_OK); -} +stat_t cm_get_tn(nvObj_t *nv) { return (get_float(nv, cm->a[_axis(nv)].travel_min)); } +stat_t cm_set_tn(nvObj_t *nv) { return (set_float(nv, cm->a[_axis(nv)].travel_min)); } +stat_t cm_get_tm(nvObj_t *nv) { return (get_float(nv, cm->a[_axis(nv)].travel_max)); } +stat_t cm_set_tm(nvObj_t *nv) { return (set_float(nv, cm->a[_axis(nv)].travel_max)); } +stat_t cm_get_ra(nvObj_t *nv) { return (get_float(nv, cm->a[_axis(nv)].radius)); } +stat_t cm_set_ra(nvObj_t *nv) { return (set_float_range(nv, cm->a[_axis(nv)].radius, RADIUS_MIN, 1000000)); } -/**** Velocity and Jerk functions - * cm_get_axis_jerk() - returns jerk for an axis - * cm_set_axis_jerk() - sets the jerk for an axis, including recirpcal and cached values - * - * cm_set_vm() - set velocity max value - called from dispatch table - * cm_set_fr() - set feedrate max value - called from dispatch table - * cm_set_jm() - set jerk max value - called from dispatch table - * cm_set_jh() - set jerk homing value - called from dispatch table - * - * Jerk values can be rather large, often in the billions. This makes for some pretty big - * numbers for people to deal with. Jerk values are stored in the system in truncated format; - * values are divided by 1,000,000 then reconstituted before use. - * - * The set_xjm() nad set_xjh() functions will accept either truncated or untruncated jerk - * numbers as input. If the number is > 1,000,000 it is divided by 1,000,000 before storing. - * Numbers are accepted in either millimeter or inch mode and converted to millimeter mode. - * - * The axis_jerk() functions expect the jerk in divided-by 1,000,000 form +/**** Axis Jerk Primitives + * cm_get_axis_jerk() - returns max jerk for an axis + * cm_set_axis_jerk() - sets the jerk for an axis, including reciprocal and cached values */ -float cm_get_axis_jerk(const uint8_t axis) -{ - return (cm.a[axis].jerk_max); -} +float cm_get_axis_jerk(const uint8_t axis) { return (cm->a[axis].jerk_max); } // Precompute sqrt(3)/10 for the max_junction_accel. // See plan_line.cpp -> _calculate_junction_vmax() notes for details. @@ -2639,152 +2248,182 @@ static const float _junction_accel_multiplier = sqrt(3.0)/10.0; // Important note: Actual jerk is stored jerk * JERK_MULTIPLIER, and // Time Quanta is junction_integration_time / 1000. -// We no longer incorporate jerk into this, since it can be channged per-move. -void _cm_recalc_max_junction_accel(const uint8_t axis) { - float T = cm.junction_integration_time / 1000.0; +void _cm_recalc_junction_accel(const uint8_t axis) { + float T = cm->junction_integration_time / 1000.0; float T2 = T*T; - - cm.a[axis].max_junction_accel = _junction_accel_multiplier * T2 * JERK_MULTIPLIER; + cm->a[axis].max_junction_accel = _junction_accel_multiplier * T2 * (cm->a[axis].jerk_max * JERK_MULTIPLIER); + cm->a[axis].high_junction_accel = _junction_accel_multiplier * T2 * (cm->a[axis].jerk_high * JERK_MULTIPLIER); } -void cm_set_axis_jerk(const uint8_t axis, const float jerk) +void cm_set_axis_max_jerk(const uint8_t axis, const float jerk) { - cm.a[axis].jerk_max = jerk; + cm->a[axis].jerk_max = jerk; + _cm_recalc_junction_accel(axis); // Must recalculate the max_junction_accel now that the jerk has changed. } +void cm_set_axis_high_jerk(const uint8_t axis, const float jerk) +{ + cm->a[axis].jerk_high = jerk; + _cm_recalc_junction_accel(axis); // Must recalculate the max_junction_accel now that the jerk has changed. +} + +/**** Axis Velocity and Jerk Settings + * + * cm_get_vm() - get velocity max value - called from dispatch table + * cm_set_vm() - set velocity max value - called from dispatch table + * cm_get_fr() - get feedrate max value - called from dispatch table + * cm_set_fr() - set feedrate max value - called from dispatch table + * cm_get_jm() - get jerk max value - called from dispatch table + * cm_set_jm() - set jerk max value - called from dispatch table + * cm_get_jh() - get jerk homing value - called from dispatch table + * cm_set_jh() - set jerk homing value - called from dispatch table + * + * Jerk values can be rather large, often in the billions. This makes for some pretty big + * numbers for people to deal with. Jerk values are stored in the system in truncated format; + * values are divided by 1,000,000 then reconstituted before use. + * + * The axis_jerk() functions expect the jerk in divided-by 1,000,000 form. + * The set_xjm() and set_xjh() functions accept values divided by 1,000,000. + * This is corrected to mm/min^3 by the internals of the code. + */ + +stat_t cm_get_vm(nvObj_t *nv) { return (get_float(nv, cm->a[_axis(nv)].velocity_max)); } stat_t cm_set_vm(nvObj_t *nv) { - uint8_t axis = _get_axis(nv->index); - if ((axis == AXIS_A) || (axis == AXIS_B) || (axis == AXIS_C)) { - ritorno(set_fltp(nv)); - } else { - ritorno(set_flup(nv)); - } - cm.a[axis].recip_velocity_max = 1/nv->value; + uint8_t axis = _axis(nv); + ritorno(set_float_range(nv, cm->a[axis].velocity_max, 0, MAX_LONG)); + cm->a[axis].recip_velocity_max = 1/nv->value_flt; return(STAT_OK); } +stat_t cm_get_fr(nvObj_t *nv) { return (get_float(nv, cm->a[_axis(nv)].feedrate_max)); } stat_t cm_set_fr(nvObj_t *nv) { - uint8_t axis = _get_axis(nv->index); - if ((axis == AXIS_A) || (axis == AXIS_B) || (axis == AXIS_C)) { - ritorno(set_fltp(nv)); - } else { - ritorno(set_flup(nv)); - } - cm.a[axis].recip_feedrate_max = 1/nv->value; + uint8_t axis = _axis(nv); + ritorno(set_float_range(nv, cm->a[axis].feedrate_max, 0, MAX_LONG)); + cm->a[axis].recip_feedrate_max = 1/nv->value_flt; return(STAT_OK); } +stat_t cm_get_jm(nvObj_t *nv) { return (get_float(nv, cm->a[_axis(nv)].jerk_max)); } stat_t cm_set_jm(nvObj_t *nv) { - if (nv->value < JERK_INPUT_MIN) { - nv->valuetype = TYPE_NULL; - return (STAT_INPUT_LESS_THAN_MIN_VALUE); - } - if (nv->value > JERK_INPUT_MAX) { - nv->valuetype = TYPE_NULL; - return (STAT_INPUT_EXCEEDS_MAX_VALUE); - } - set_flu(nv); - cm_set_axis_jerk(_get_axis(nv->index), nv->value); + uint8_t axis = _axis(nv); + ritorno(set_float_range(nv, cm->a[axis].jerk_max, JERK_INPUT_MIN, JERK_INPUT_MAX)); + cm_set_axis_max_jerk(axis, nv->value_flt); return(STAT_OK); } +stat_t cm_get_jh(nvObj_t *nv) { return (get_float(nv, cm->a[_axis(nv)].jerk_high)); } stat_t cm_set_jh(nvObj_t *nv) { - if (nv->value < JERK_INPUT_MIN) { - nv->valuetype = TYPE_NULL; - return (STAT_INPUT_LESS_THAN_MIN_VALUE); - } - if (nv->value > JERK_INPUT_MAX) { - nv->valuetype = TYPE_NULL; - return (STAT_INPUT_EXCEEDS_MAX_VALUE); - } - set_flu(nv); + uint8_t axis = _axis(nv); + ritorno(set_float_range(nv, cm->a[axis].jerk_high, JERK_INPUT_MIN, JERK_INPUT_MAX)); + cm_set_axis_high_jerk(axis, nv->value_flt); return(STAT_OK); } -stat_t cm_set_jt(nvObj_t *nv) -{ - stat_t status = STAT_OK; +/**** Axis Homing Settings + * cm_get_hi() - get homing input + * cm_set_hi() - set homing input + * cm_get_hd() - get homing direction + * cm_set_hd() - set homing direction + * cm_get_sv() - get homing search velocity + * cm_set_sv() - set homing search velocity + * cm_get_lv() - get homing latch velocity + * cm_set_lv() - set homing latch velocity + * cm_get_lb() - get homing latch backoff + * cm_set_lb() - set homing latch backoff + * cm_get_zb() - get homing zero backoff + * cm_set_zb() - set homing zero backoff + */ - if (nv->value < JUNCTION_INTEGRATION_MIN) { - nv->valuetype = TYPE_NULL; - return (STAT_INPUT_LESS_THAN_MIN_VALUE); - } - if (nv->value > JUNCTION_INTEGRATION_MAX) { - nv->valuetype = TYPE_NULL; - return (STAT_INPUT_EXCEEDS_MAX_VALUE); - } - set_flt(nv); - - // Must recalculate the max_junction_accel now that the time quanta has changed. - for (uint8_t axis=0; axisa[_axis(nv)].homing_input)); } +stat_t cm_set_hi(nvObj_t *nv) { return (set_integer(nv, cm->a[_axis(nv)].homing_input, 0, D_IN_CHANNELS)); } +stat_t cm_get_hd(nvObj_t *nv) { return (get_integer(nv, cm->a[_axis(nv)].homing_dir)); } +stat_t cm_set_hd(nvObj_t *nv) { return (set_integer(nv, cm->a[_axis(nv)].homing_dir, 0, 1)); } +stat_t cm_get_sv(nvObj_t *nv) { return (get_float(nv, cm->a[_axis(nv)].search_velocity)); } +stat_t cm_set_sv(nvObj_t *nv) { return (set_float_range(nv, cm->a[_axis(nv)].search_velocity, 0, MAX_LONG)); } +stat_t cm_get_lv(nvObj_t *nv) { return (get_float(nv, cm->a[_axis(nv)].latch_velocity)); } +stat_t cm_set_lv(nvObj_t *nv) { return (set_float_range(nv, cm->a[_axis(nv)].latch_velocity, 0, MAX_LONG)); } +stat_t cm_get_lb(nvObj_t *nv) { return (get_float(nv, cm->a[_axis(nv)].latch_backoff)); } +stat_t cm_set_lb(nvObj_t *nv) { return (set_float(nv, cm->a[_axis(nv)].latch_backoff)); } +stat_t cm_get_zb(nvObj_t *nv) { return (get_float(nv, cm->a[_axis(nv)].zero_backoff)); } +stat_t cm_set_zb(nvObj_t *nv) { return (set_float(nv, cm->a[_axis(nv)].zero_backoff)); } +/*** Canonical Machine Global Settings ***/ /* + * cm_get_jt() - get junction integration time + * cm_set_jt() - set junction integration time + * cm_get_ct() - get chordal tolerance + * cm_set_ct() - set chordal tolerance + * cm_get_sl() - get soft limit enable + * cm_set_sl() - set soft limit enable + * cm_get_lim() - get hard limit enable + * cm_set_lim() - set hard limit enable + * cm_get_saf() - get safety interlock enable + * cm_set_saf() - set safety interlock enable * cm_set_mfo() - set manual feedrate override factor * cm_set_mto() - set manual traverse override factor */ -stat_t cm_set_mfo(nvObj_t *nv) +stat_t cm_get_jt(nvObj_t *nv) { return(get_float(nv, cm->junction_integration_time)); } +stat_t cm_set_jt(nvObj_t *nv) { - if (nv->value < FEED_OVERRIDE_MIN) { - nv->valuetype = TYPE_NULL; - return (STAT_INPUT_LESS_THAN_MIN_VALUE); + ritorno(set_float_range(nv, cm->junction_integration_time, JUNCTION_INTEGRATION_MIN, JUNCTION_INTEGRATION_MAX)); + for (uint8_t axis=0; axisvalue > FEED_OVERRIDE_MAX) { - nv->valuetype = TYPE_NULL; - return (STAT_INPUT_EXCEEDS_MAX_VALUE); - } - set_flt(nv); return(STAT_OK); } -stat_t cm_set_mto(nvObj_t *nv) -{ - if (nv->value < TRAVERSE_OVERRIDE_MIN) { - nv->valuetype = TYPE_NULL; - return (STAT_INPUT_LESS_THAN_MIN_VALUE); - } - if (nv->value > TRAVERSE_OVERRIDE_MAX) { - nv->valuetype = TYPE_NULL; - return (STAT_INPUT_EXCEEDS_MAX_VALUE); - } - set_flt(nv); - return(STAT_OK); -} +stat_t cm_get_ct(nvObj_t *nv) { return(get_float(nv, cm->chordal_tolerance)); } +stat_t cm_set_ct(nvObj_t *nv) { return(set_float_range(nv, cm->chordal_tolerance, CHORDAL_TOLERANCE_MIN, 10000000)); } -/* - * Commands - * - * cm_run_qf() - flush planner queue - * cm_run_home() - run homing sequence - */ +stat_t cm_get_zl(nvObj_t *nv) { return(get_float(nv, cm->feedhold_z_lift)); } +stat_t cm_set_zl(nvObj_t *nv) { return(set_float(nv, cm->feedhold_z_lift)); } -stat_t cm_run_qf(nvObj_t *nv) -{ - cm_request_queue_flush(); - return (STAT_OK); -} +stat_t cm_get_sl(nvObj_t *nv) { return(get_integer(nv, cm->soft_limit_enable)); } +stat_t cm_set_sl(nvObj_t *nv) { return(set_integer(nv, (uint8_t &)cm->soft_limit_enable, 0, 1)); } -stat_t cm_run_home(nvObj_t *nv) -{ - if (fp_TRUE(nv->value)) { - float axes[] = { 1,1,1,1,1,1 }; - bool flags[] = { 1,1,1,1,1,1 }; - cm_homing_cycle_start(axes, flags); - } - return (STAT_OK); -} +stat_t cm_get_lim(nvObj_t *nv) { return(get_integer(nv, cm->limit_enable)); } +stat_t cm_set_lim(nvObj_t *nv) { return(set_integer(nv, (uint8_t &)cm->limit_enable, 0, 1)); } -/* +stat_t cm_get_saf(nvObj_t *nv) { return(get_integer(nv, cm->safety_interlock_enable)); } +stat_t cm_set_saf(nvObj_t *nv) { return(set_integer(nv, (uint8_t &)cm->safety_interlock_enable, 0, 1)); } + +stat_t cm_get_m48(nvObj_t *nv) { return(get_integer(nv, cm->gmx.m48_enable)); } +stat_t cm_set_m48(nvObj_t *nv) { return(set_integer(nv, (uint8_t &)cm->gmx.m48_enable, 0, 1)); } + +stat_t cm_get_froe(nvObj_t *nv) { return(get_integer(nv, cm->gmx.mfo_enable)); } +stat_t cm_set_froe(nvObj_t *nv) { return(set_integer(nv, (uint8_t &)cm->gmx.mfo_enable, 0, 1)); } +stat_t cm_get_fro(nvObj_t *nv) { return(get_float(nv, cm->gmx.mfo_factor)); } +stat_t cm_set_fro(nvObj_t *nv) { return(set_float_range(nv, cm->gmx.mfo_factor, FEED_OVERRIDE_MIN, FEED_OVERRIDE_MAX)); } + +stat_t cm_get_troe(nvObj_t *nv) { return(get_integer(nv, cm->gmx.mto_enable)); } +stat_t cm_set_troe(nvObj_t *nv) { return(set_integer(nv, (uint8_t &)cm->gmx.mto_enable, 0, 1)); } +stat_t cm_get_tro(nvObj_t *nv) { return(get_float(nv, cm->gmx.mto_factor)); } +stat_t cm_set_tro(nvObj_t *nv) { return(set_float_range(nv, cm->gmx.mto_factor, TRAVERSE_OVERRIDE_MIN, TRAVERSE_OVERRIDE_MAX)); } + +stat_t cm_get_gpl(nvObj_t *nv) { return(get_integer(nv, cm->default_select_plane)); } +stat_t cm_set_gpl(nvObj_t *nv) { return(set_integer(nv, (uint8_t &)cm->default_select_plane, CANON_PLANE_XY, CANON_PLANE_YZ)); } + +stat_t cm_get_gun(nvObj_t *nv) { return(get_integer(nv, cm->default_units_mode)); } +stat_t cm_set_gun(nvObj_t *nv) { return(set_integer(nv, (uint8_t &)cm->default_units_mode, INCHES, MILLIMETERS)); } + +stat_t cm_get_gco(nvObj_t *nv) { return(get_integer(nv, cm->default_coord_system)); } +stat_t cm_set_gco(nvObj_t *nv) { return(set_integer(nv, (uint8_t &)cm->default_coord_system, G54, G59)); } + +stat_t cm_get_gpa(nvObj_t *nv) { return(get_integer(nv, cm->default_path_control)); } +stat_t cm_set_gpa(nvObj_t *nv) { return(set_integer(nv, (uint8_t &)cm->default_path_control, PATH_EXACT_PATH, PATH_CONTINUOUS)); } + +stat_t cm_get_gdi(nvObj_t *nv) { return(get_integer(nv, cm->default_distance_mode)); } +stat_t cm_set_gdi(nvObj_t *nv) { return(set_integer(nv, (uint8_t &)cm->default_distance_mode, ABSOLUTE_DISTANCE_MODE, INCREMENTAL_DISTANCE_MODE)); } + +/*********************************************************************************** * Debugging Commands - * + ***********************************************************************************/ +/* * cm_dam() - dump active model */ @@ -2812,43 +2451,6 @@ stat_t cm_dam(nvObj_t *nv) return (STAT_OK); } -/*********************************************************************************** - * AXIS JOGGING - ***********************************************************************************/ - -float cm_get_jogging_dest(void) -{ - return cm.jogging_dest; -} - -stat_t cm_run_jogx(nvObj_t *nv) -{ - set_flt(nv); - cm_jogging_cycle_start(AXIS_X); - return (STAT_OK); -} - -stat_t cm_run_jogy(nvObj_t *nv) -{ - set_flt(nv); - cm_jogging_cycle_start(AXIS_Y); - return (STAT_OK); -} - -stat_t cm_run_jogz(nvObj_t *nv) -{ - set_flt(nv); - cm_jogging_cycle_start(AXIS_Z); - return (STAT_OK); -} - -stat_t cm_run_joga(nvObj_t *nv) -{ - set_flt(nv); - cm_jogging_cycle_start(AXIS_A); - return (STAT_OK); -} - /*********************************************************************************** * TEXT MODE SUPPORT * Functions to print variables from the cfgArray table @@ -2912,31 +2514,33 @@ void cm_print_gdi(nvObj_t *nv) { text_print(nv, fmt_gdi);} // TYPE_INT /* system parameter print functions */ -static const char fmt_jt[] = "[jt] junction integrgation time%6.2f\n"; +static const char fmt_jt[] = "[jt] junction integration time%7.2f\n"; static const char fmt_ct[] = "[ct] chordal tolerance%17.4f%s\n"; +static const char fmt_zl[] = "[zl] Z lift on feedhold%16.3f%s\n"; static const char fmt_sl[] = "[sl] soft limit enable%12d [0=disable,1=enable]\n"; static const char fmt_lim[] ="[lim] limit switch enable%10d [0=disable,1=enable]\n"; static const char fmt_saf[] ="[saf] safety interlock enable%6d [0=disable,1=enable]\n"; void cm_print_jt(nvObj_t *nv) { text_print(nv, fmt_jt);} // TYPE FLOAT void cm_print_ct(nvObj_t *nv) { text_print_flt_units(nv, fmt_ct, GET_UNITS(ACTIVE_MODEL));} +void cm_print_zl(nvObj_t *nv) { text_print_flt_units(nv, fmt_zl, GET_UNITS(ACTIVE_MODEL));} void cm_print_sl(nvObj_t *nv) { text_print(nv, fmt_sl);} // TYPE_INT void cm_print_lim(nvObj_t *nv){ text_print(nv, fmt_lim);} // TYPE_INT void cm_print_saf(nvObj_t *nv){ text_print(nv, fmt_saf);} // TYPE_INT -static const char fmt_m48e[] = "[m48e] overrides enabled%11d [0=disable,1=enable]\n"; -static const char fmt_mfoe[] = "[mfoe] manual feed override enab%3d [0=disable,1=enable]\n"; -static const char fmt_mfo[] = "[mfo] manual feedrate override%8.3f [0.05 < mfo < 2.00]\n"; -static const char fmt_mtoe[] = "[mtoe] manual traverse over enab%3d [0=disable,1=enable]\n"; -static const char fmt_mto[] = "[mto] manual traverse override%8.3f [0.05 < mto < 1.00]\n"; +static const char fmt_m48[] = "[m48] overrides enabled%12d [0=disable,1=enable]\n"; +static const char fmt_froe[] = "[froe] feed override enable%8d [0=disable,1=enable]\n"; +static const char fmt_fro[] = "[fro] feedrate override%15.3f [0.05 < mfo < 2.00]\n"; +static const char fmt_troe[] = "[troe] traverse over enable%8d [0=disable,1=enable]\n"; +static const char fmt_tro[] = "[tro] traverse override%15.3f [0.05 < mto < 1.00]\n"; static const char fmt_tram[] = "[tram] is coordinate space rotated to be tram %s\n"; -static const char fmt_nxln[] = "[nxln] the next line number expected is %10d\n"; +static const char fmt_nxln[] = "[nxln] next line number %lu\n"; -void cm_print_m48e(nvObj_t *nv) { text_print(nv, fmt_m48e);} // TYPE_INT -void cm_print_mfoe(nvObj_t *nv) { text_print(nv, fmt_mfoe);} // TYPE INT -void cm_print_mfo(nvObj_t *nv) { text_print(nv, fmt_mfo);} // TYPE FLOAT -void cm_print_mtoe(nvObj_t *nv) { text_print(nv, fmt_mtoe);} // TYPE INT -void cm_print_mto(nvObj_t *nv) { text_print(nv, fmt_mto);} // TYPE FLOAT +void cm_print_m48(nvObj_t *nv) { text_print(nv, fmt_m48);} // TYPE_INT +void cm_print_froe(nvObj_t *nv) { text_print(nv, fmt_froe);} // TYPE INT +void cm_print_fro(nvObj_t *nv) { text_print(nv, fmt_fro);} // TYPE FLOAT +void cm_print_troe(nvObj_t *nv) { text_print(nv, fmt_troe);} // TYPE INT +void cm_print_tro(nvObj_t *nv) { text_print(nv, fmt_tro);} // TYPE FLOAT void cm_print_tram(nvObj_t *nv) { text_print(nv, fmt_tram);}; // TYPE BOOL void cm_print_nxln(nvObj_t *nv) { text_print(nv, fmt_nxln);}; // TYPE INT @@ -2962,8 +2566,8 @@ void cm_print_nxln(nvObj_t *nv) { text_print(nv, fmt_nxln);}; // TYPE INT * cm_print_zb() * * cm_print_pos() - print position with unit displays for MM or Inches - * cm_print_mpo() - print position with fixed unit display - always in Degrees or MM - * cm_print_tram() - print if the coordinate system is rotated + * cm_print_mpo() - print position with fixed unit display - always in Degrees or MM + * cm_print_tram() - print if the coordinate system is rotated */ static const char fmt_Xam[] = "[%s%s] %s axis mode%18d %s\n"; @@ -2991,55 +2595,55 @@ static const char fmt_hom[] = "%c axis homing state:%2.0f\n"; static void _print_axis_ui8(nvObj_t *nv, const char *format) { - sprintf(cs.out_buf, format, nv->group, nv->token, nv->group, (uint8_t)nv->value); + sprintf(cs.out_buf, format, nv->group, nv->token, nv->group, nv->value_int); xio_writeline(cs.out_buf); } static void _print_axis_flt(nvObj_t *nv, const char *format) { char *units; - if (cm_get_axis_type(nv->index) == 0) { // linear + if (cm_get_axis_type(nv) == AXIS_TYPE_LINEAR) { units = (char *)GET_UNITS(MODEL); } else { units = (char *)GET_TEXT_ITEM(msg_units, DEGREE_INDEX); } - sprintf(cs.out_buf, format, nv->group, nv->token, nv->group, nv->value, units); + sprintf(cs.out_buf, format, nv->group, nv->token, nv->group, nv->value_flt, units); xio_writeline(cs.out_buf); } static void _print_axis_coord_flt(nvObj_t *nv, const char *format) { char *units; - if (cm_get_axis_type(nv->index) == 0) { // linear + if (cm_get_axis_type(nv) == AXIS_TYPE_LINEAR) { units = (char *)GET_UNITS(MODEL); } else { units = (char *)GET_TEXT_ITEM(msg_units, DEGREE_INDEX); } - sprintf(cs.out_buf, format, nv->group, nv->token, nv->group, nv->token, nv->value, units); + sprintf(cs.out_buf, format, nv->group, nv->token, nv->group, nv->token, nv->value_flt, units); xio_writeline(cs.out_buf); } static void _print_pos(nvObj_t *nv, const char *format, uint8_t units) { char axes[] = {"XYZABC"}; - uint8_t axis = _get_axis(nv->index); + uint8_t axis = _axis(nv); if (axis >= AXIS_A) { units = DEGREES;} - sprintf(cs.out_buf, format, axes[axis], nv->value, GET_TEXT_ITEM(msg_units, units)); + sprintf(cs.out_buf, format, axes[axis], nv->value_flt, GET_TEXT_ITEM(msg_units, units)); xio_writeline(cs.out_buf); } static void _print_hom(nvObj_t *nv, const char *format) { char axes[] = {"XYZABC"}; - uint8_t axis = _get_axis(nv->index); - sprintf(cs.out_buf, format, axes[axis], nv->value); + uint8_t axis = _axis(nv); + sprintf(cs.out_buf, format, axes[axis], nv->value_int); xio_writeline(cs.out_buf); } void cm_print_am(nvObj_t *nv) // print axis mode with enumeration string { - sprintf(cs.out_buf, fmt_Xam, nv->group, nv->token, nv->group, (uint8_t)nv->value, - GET_TEXT_ITEM(msg_am, (uint8_t)nv->value)); + sprintf(cs.out_buf, fmt_Xam, nv->group, nv->token, nv->group, (int)nv->value_int, + GET_TEXT_ITEM(msg_am, nv->value_int)); xio_writeline(cs.out_buf); } diff --git a/g2core/canonical_machine.h b/g2core/canonical_machine.h index 55e217e2..94c6d450 100644 --- a/g2core/canonical_machine.h +++ b/g2core/canonical_machine.h @@ -2,8 +2,8 @@ * canonical_machine.h - rs274/ngc canonical machining functions * This file is part of the g2core project * - * Copyright (c) 2010 - 2017 Alden S. Hart Jr. - * Copyright (c) 2016 - 2017 Rob Giseburt + * Copyright (c) 2010 - 2019 Alden S. Hart Jr. + * Copyright (c) 2016 - 2019 Rob Giseburt * * This code is a loose implementation of Kramer, Proctor and Messina's * canonical machining functions as described in the NIST RS274/NGC v3 @@ -35,6 +35,7 @@ #include "config.h" #include "hardware.h" // Note: hardware.h is specific to the hardware target selected #include "settings.h" +#include "gcode.h" #if MARLIN_COMPAT_ENABLED == true #include "marlin_compatibility.h" // import Marlin definitions and enums @@ -42,342 +43,178 @@ /* Defines, Macros, and Assorted Parameters */ -#define MODEL (GCodeState_t *)&cm.gm // absolute pointer from canonical machine gm model -#define PLANNER (GCodeState_t *)&bf->gm // relative to buffer *bf is currently pointing to -#define RUNTIME (GCodeState_t *)&mr.gm // absolute pointer from runtime mm struct -#define ACTIVE_MODEL cm.am // active model pointer is maintained by state management +#define MODEL (GCodeState_t *)&cm->gm // absolute pointer from canonical machine gm model +#define RUNTIME (GCodeState_t *)&mr->gm // absolute pointer from runtime mm struct +#define ACTIVE_MODEL cm->am // active model pointer is maintained by cm_set_motion_state() -#define _to_millimeters(a) ((cm.gm.units_mode == INCHES) ? (a * MM_PER_INCH) : a) - -#define JOGGING_START_VELOCITY ((float)10.0) -#define DISABLE_SOFT_LIMIT (999999) -#define JERK_INPUT_MIN (0.01) // minimum allowable jerk setting in millions mm/min^3 -#define JERK_INPUT_MAX (1000000) // maximum allowable jerk setting in millions mm/min^3 -#define PROBES_STORED 3 // we store three probes for coordinate rotation computation +#define _to_millimeters(a) ((cm->gm.units_mode == INCHES) ? ((float)a * (float)MM_PER_INCH) : (float)a) +#define _to_inches(a) ((cm->gm.units_mode == INCHES) ? ((float)a * (float)(1/MM_PER_INCH)) : (float)a) +#define DISABLE_SOFT_LIMIT (999999) +#define JERK_INPUT_MIN (0.01) // minimum allowable jerk setting in millions mm/min^3 +#define JERK_INPUT_MAX (1000000) // maximum allowable jerk setting in millions mm/min^3 +#define PROBES_STORED 3 // we store three probes for coordinate rotation computation +#define MAX_LINENUM 2000000000 // set 2 billion as max line number /***************************************************************************** * MACHINE STATE MODEL * * The following main variables track canonical machine state and state transitions. - * - cm.machine_state - overall state of machine and program execution - * - cm.cycle_state - what cycle the machine is executing (or none) - * - cm.motion_state - state of movement + * - cm->machine_state - overall state of machine and program execution + * - cm->motion_state - state of movement + * - cm->cycle_type - what type of cycle the machine is executing (or none) */ // *** Note: check config printout strings align with all the state variables // ### LAYER 8 CRITICAL REGION ### // ### DO NOT CHANGE THESE ENUMERATIONS WITHOUT COMMUNITY INPUT ### -typedef enum { // check alignment with messages in config.c / msg_stat strings - COMBINED_INITIALIZING = 0, // [0] machine is initializing //iff macs == MACHINE_INITIALIZING - COMBINED_READY, // [1] machine is ready for use //iff macs == MACHINE_READY - COMBINED_ALARM, // [2] machine in alarm state //iff macs == MACHINE_ALARM - COMBINED_PROGRAM_STOP, // [3] program stop/no more blocks //iff macs == MACHINE_PROGRAM_STOP - COMBINED_PROGRAM_END, // [4] program end //iff macs == MACHINE_PROGRAM_END - COMBINED_RUN, // [5] machine is running //iff macs == MACHINE_CYCLE, cycs == CYCLE_OFF, mots != MOTION_HOLD - COMBINED_HOLD, // [6] machine is holding //iff macs == MACHINE_CYCLE, cycs == CYCLE_OFF, mots == MOTION_HOLD - COMBINED_PROBE, // [7] probe cycle active //iff macs == MACHINE_CYCLE, cycs == CYCLE_PROBE - COMBINED_CYCLE, // [8] reserved for canned cycles < not used > - COMBINED_HOMING, // [9] homing cycle active //iff macs == MACHINE_CYCLE, cycs = CYCLE_HOMING - COMBINED_JOG, // [10] jogging cycle active //iff macs == MACHINE_CYCLE, cycs = CYCLE_JOG - COMBINED_INTERLOCK, // [11] machine in safety interlock hold//iff macs == MACHINE_INTERLOCK - COMBINED_SHUTDOWN, // [12] machine in shutdown state //iff macs == MACHINE_SHUTDOWN - COMBINED_PANIC // [13] machine in panic state //iff macs == MACHINE_PANIC +typedef enum { // check alignment with messages in config.c / msg_stat strings + COMBINED_INITIALIZING = 0, // [0] machine is initializing + COMBINED_READY, // [1] machine is ready for use + COMBINED_ALARM, // [2] machine in alarm state + COMBINED_PROGRAM_STOP, // [3] program stop/no more blocks + COMBINED_PROGRAM_END, // [4] program end + COMBINED_RUN, // [5] machine is running + COMBINED_HOLD, // [6] machine is holding + COMBINED_PROBE, // [7] probe cycle activ + COMBINED_CYCLE, // [8] reserved for canned cycles + COMBINED_HOMING, // [9] homing cycle active + COMBINED_JOG, // [10] jogging cycle active + COMBINED_INTERLOCK, // [11] machine in safety interlock hold + COMBINED_SHUTDOWN, // [12] machine in shutdown state + COMBINED_PANIC // [13] machine in panic state } cmCombinedState; //### END CRITICAL REGION ### -typedef enum { - MACHINE_INITIALIZING = 0, // [0] machine is initializing - MACHINE_READY, // [1] machine is ready for use - MACHINE_ALARM, // [2] machine in alarm state - MACHINE_PROGRAM_STOP, // [3] no blocks to run; like PROGRAM_END but without the M2 to reset gcode state - MACHINE_PROGRAM_END, // [4] program end (same as MACHINE_READY, really...) - MACHINE_CYCLE, // [5] machine is running; blocks still to run, or steppers are busy - MACHINE_INTERLOCK, // [6] machine in interlock state - MACHINE_SHUTDOWN, // [7] machine in shutdown state - MACHINE_PANIC // [8] machine in panic state +typedef enum { // Note: MachineState signals if the machine is in cycle (5) or some other non-cycle state + MACHINE_INITIALIZING = 0, // machine is initializing + MACHINE_READY, // machine is ready for use but idle + MACHINE_ALARM, // machine is in alarm state + MACHINE_PROGRAM_STOP, // no blocks to run; like PROGRAM_END but without the M2 to reset gcode state + MACHINE_PROGRAM_END, // program end (same as MACHINE_READY, really...) + MACHINE_CYCLE, // machine is in cycle, running; blocks still to run, or steppers are busy + MACHINE_INTERLOCK, // machine is in interlock state + MACHINE_SHUTDOWN, // machine is in shutdown state + MACHINE_PANIC // machine is in panic state } cmMachineState; typedef enum { - CYCLE_OFF = 0, // machine is idle - CYCLE_MACHINING, // in normal machining cycle - CYCLE_HOMING, // in homing cycle - CYCLE_PROBE, // in probe cycle - CYCLE_JOG // in jogging cycle -} cmCycleState; - -typedef enum { - MOTION_STOP = 0, // motion has stopped: set when the steppers reach the end of the planner queue - MOTION_PLANNING, // machine has planned an ALINE segment, but not yet started to execute them - MOTION_RUN, // machine is in motion: set when the steppers execute an ALINE segment - MOTION_HOLD // feedhold in progress: set whenever we leave FEEDHOLD_OFF, unset whenever we enter FEEDHOLD_OFF + MOTION_STOP = 0, // motion has stopped: set when the steppers reach the end of the planner queue + MOTION_RUN // machine is in motion: set when the steppers execute an ALINE segment } cmMotionState; -typedef enum { // feedhold state machine - FEEDHOLD_OFF = 0, // no feedhold in effect - FEEDHOLD_REQUESTED, // feedhold has been requested but not started yet - FEEDHOLD_SYNC, // start hold - sync to latest aline segment - FEEDHOLD_DECEL_CONTINUE, // in deceleration that will not end at zero - FEEDHOLD_DECEL_TO_ZERO, // in deceleration that will go to zero - FEEDHOLD_DECEL_END, // end the deceleration - FEEDHOLD_PENDING, // waiting to finalize the deceleration once motion stops - FEEDHOLD_HOLD // holding +typedef enum { + CYCLE_NONE = 0, // not in a cycle + CYCLE_MACHINING, // in normal machining cycle + CYCLE_HOMING, // in homing cycle + CYCLE_PROBE, // in probe cycle + CYCLE_JOG // in jogging cycle +// CYCLE_G81 // illustration of canned cycles +// ... +} cmCycleType; + +typedef enum { // feedhold type parameter + FEEDHOLD_TYPE_HOLD, // simple feedhold at max jerk with no actions + FEEDHOLD_TYPE_ACTIONS, // feedhold at max jerk with hold entry actions + FEEDHOLD_TYPE_SKIP, // feedhold at max jerk with queue flush and sync command + FEEDHOLD_TYPE_SCRAM // feedhold at high jerk and stop all active devices +} cmFeedholdType; + +typedef enum { // feedhold final operation + FEEDHOLD_EXIT_CYCLE = 0, // exit feedhold with cycle restart + FEEDHOLD_EXIT_FLUSH, // exit feedhold with flush + FEEDHOLD_EXIT_STOP, // perform program stop + FEEDHOLD_EXIT_END, // perform program end + FEEDHOLD_EXIT_ALARM, // perform alarm + FEEDHOLD_EXIT_SHUTDOWN, // perform shutdown + FEEDHOLD_EXIT_INTERLOCK, // report as interlock + FEEDHOLD_EXIT_RESET_POSITION // reset machine positions to hold point +} cmFeedholdExit; + +typedef enum { // feedhold state machine + FEEDHOLD_OFF = 0, // no feedhold in effect + FEEDHOLD_REQUESTED, // feedhold has been requested but not started yet + FEEDHOLD_SYNC, // start hold - sync to latest aline segment + FEEDHOLD_DECEL_CONTINUE, // in deceleration that will not end at zero + FEEDHOLD_DECEL_TO_ZERO, // in deceleration that will go to zero + FEEDHOLD_DECEL_COMPLETE, // feedhold deceleration has completed, but motors may not have stopped yet + FEEDHOLD_MOTION_STOPPING, // waiting for motors to have stopped at hold point (motion stop) + FEEDHOLD_MOTION_STOPPED, // motion has stopped at hold point + FEEDHOLD_HOLD_ACTIONS_PENDING, // wait for feedhold actions to complete + FEEDHOLD_HOLD_ACTIONS_COMPLETE, // + FEEDHOLD_HOLD, // HOLDING (steady state) + FEEDHOLD_EXIT_ACTIONS_PENDING, // performing exit actions + FEEDHOLD_EXIT_ACTIONS_COMPLETE // completed exit actions } cmFeedholdState; -typedef enum { // feed override state machine +typedef enum { // Motion profiles + PROFILE_NORMAL = 0, // Normal jerk in effect + PROFILE_FAST // High speed jerk in effect +} cmMotionProfile; + +typedef enum { // state machine for cycle start + CYCLE_START_OFF = 0, // not requested + CYCLE_START_REQUESTED, + CYCLE_START_COMPLETE +} cmCycleState; + +typedef enum { // queue flush state machine + QUEUE_FLUSH_OFF = 0, // no queue flush in effect + QUEUE_FLUSH_REQUESTED // flush has been requested but not started yet +} cmFlushState; + +typedef enum { // applies to cm->homing_state + HOMING_NOT_HOMED = 0, // machine is not homed (0=false) + HOMING_HOMED = 1, // machine is homed (1=true) + HOMING_WAITING // machine waiting to be homed +} cmHomingState; + +typedef enum { // applies to cm->probe_state + PROBE_FAILED = 0, // probe reached endpoint without triggering + PROBE_SUCCEEDED = 1, // probe was triggered, cm->probe_results has position + PROBE_WAITING = 2 // probe is waiting to be started or is running +} cmProbeState; + +typedef enum { + SAFETY_INTERLOCK_ENGAGED = 0, // meaning the interlock input is CLOSED (low) + SAFETY_INTERLOCK_DISENGAGED +} cmSafetyState; + +typedef enum { // feed override state machine MFO_OFF = 0, MFO_REQUESTED, MFO_SYNC } cmOverrideState; -typedef enum { // queue flush state machine - FLUSH_OFF = 0, // no queue flush in effect - FLUSH_REQUESTED, // flush has been requested but not started yet -} cmQueueFlushState; - -typedef enum { // applies to cm.homing_state - HOMING_NOT_HOMED = 0, // machine is not homed (0=false) - HOMING_HOMED = 1, // machine is homed (1=true) - HOMING_WAITING // machine waiting to be homed -} cmHomingState; - -typedef enum { // applies to cm.probe_state - PROBE_FAILED = 0, // probe reached endpoint without triggering - PROBE_SUCCEEDED = 1, // probe was triggered, cm.probe_results has position - PROBE_WAITING = 2 // probe is waiting to be started or is running -} cmProbeState; - -typedef enum { - SAFETY_INTERLOCK_ENGAGED = 0, // meaning the interlock input is CLOSED (low) - SAFETY_INTERLOCK_DISENGAGED -} cmSafetyState; - -typedef enum { // G Modal Group 1 - MOTION_MODE_STRAIGHT_TRAVERSE=0, // G0 - straight traverse - MOTION_MODE_STRAIGHT_FEED, // G1 - straight feed - MOTION_MODE_CW_ARC, // G2 - clockwise arc feed - MOTION_MODE_CCW_ARC, // G3 - counter-clockwise arc feed - MOTION_MODE_CANCEL_MOTION_MODE, // G80 - MOTION_MODE_STRAIGHT_PROBE, // G38.2 - MOTION_MODE_CANNED_CYCLE_81, // G81 - drilling - MOTION_MODE_CANNED_CYCLE_82, // G82 - drilling with dwell - MOTION_MODE_CANNED_CYCLE_83, // G83 - peck drilling - MOTION_MODE_CANNED_CYCLE_84, // G84 - right hand tapping - MOTION_MODE_CANNED_CYCLE_85, // G85 - boring, no dwell, feed out - MOTION_MODE_CANNED_CYCLE_86, // G86 - boring, spindle stop, rapid out - MOTION_MODE_CANNED_CYCLE_87, // G87 - back boring - MOTION_MODE_CANNED_CYCLE_88, // G88 - boring, spindle stop, manual out - MOTION_MODE_CANNED_CYCLE_89 // G89 - boring, dwell, feed out -} cmMotionMode; - -typedef enum { // canonical plane - translates to: - // axis_0 axis_1 axis_2 - CANON_PLANE_XY = 0, // G17 X Y Z - CANON_PLANE_XZ, // G18 X Z Y - CANON_PLANE_YZ // G19 Y Z X -} cmCanonicalPlane; - -typedef enum { - INCHES = 0, // G20 - MILLIMETERS, // G21 - DEGREES // ABC axes (this value used for displays only) -} cmUnitsMode; - -typedef enum { - ABSOLUTE_COORDS = 0, // machine coordinate system - G54, // G54 coordinate system - G55, // G55 coordinate system - G56, // G56 coordinate system - G57, // G57 coordinate system - G58, // G58 coordinate system - G59 // G59 coordinate system -} cmCoordSystem; -#define COORD_SYSTEM_MAX G59 // set this manually to the last one - -typedef enum { - ABSOLUTE_OVERRIDE_OFF = 0,// G53 enabled - ABSOLUTE_OVERRIDE_ON -} cmAbsoluteOverride; - -typedef enum { // G Modal Group 13 - PATH_EXACT_PATH = 0, // G61 - hits corners but does not stop if it does not need to. - PATH_EXACT_STOP, // G61.1 - stops at all corners - PATH_CONTINUOUS // G64 and typically the default mode -} cmPathControl; - -typedef enum { - ABSOLUTE_DISTANCE_MODE = 0, // G90 / G90.1 - INCREMENTAL_DISTANCE_MODE // G91 / G91.1 -} cmDistanceMode; - -typedef enum { - INVERSE_TIME_MODE = 0, // G93 - UNITS_PER_MINUTE_MODE, // G94 - UNITS_PER_REVOLUTION_MODE// G95 (unimplemented) -} cmFeedRateMode; - -typedef enum { - ORIGIN_OFFSET_SET=0, // G92 - set origin offsets - ORIGIN_OFFSET_CANCEL, // G92.1 - zero out origin offsets - ORIGIN_OFFSET_SUSPEND, // G92.2 - do not apply offsets, but preserve the values - ORIGIN_OFFSET_RESUME // G92.3 - resume application of the suspended offsets -} cmOriginOffset; - -typedef enum { - PROGRAM_STOP = 0, - PROGRAM_END -} cmProgramFlow; - -typedef enum { // used for spindle and arc dir - DIRECTION_CW = 0, - DIRECTION_CCW -} cmDirection; - -typedef enum { // axis types - AXIS_TYPE_UNDEFINED=-2, // invalid type - AXIS_TYPE_SYSTEM=-1, // token is global system token, not axis - AXIS_TYPE_LINEAR, // linear axis - AXIS_TYPE_ROTARY // rotary axis -} cmAxisType; - -typedef enum { // axis modes (ordered: see _cm_get_feed_time()) - AXIS_DISABLED = 0, // kill axis - AXIS_STANDARD, // axis in coordinated motion w/standard behaviors - AXIS_INHIBITED, // axis is computed but not activated - AXIS_RADIUS // rotary axis calibrated to circumference -} cmAxisMode; -#define AXIS_MODE_MAX_LINEAR AXIS_INHIBITED -#define AXIS_MODE_MAX_ROTARY AXIS_RADIUS - -/***************************************************************************** - * GCODE MODEL - The following GCodeModel/GCodeInput structs are used: - * - * - gm is the core Gcode model state. It keeps the internal gcode state model in - * normalized, canonical form. All values are unit converted (to mm) and in the - * machine coordinate system (absolute coordinate system). Gm is owned by the - * canonical machine layer and should be accessed only through cm_ routines. - * - * The gm core struct is copied and passed as context to the runtime where it is - * used for planning, move execution, feedholds, and reporting. - * - * - gmx is the extended gcode model variables that are only used by the canonical - * machine and do not need to be passed further down. It keeps "global" gcode - * state that does not change when you go down through the planner to the runtime. - * Other Gcode model state is kept in the singletons for various sub-systems, such - * as arcs, spindle, coolant, and others (i.e. not ALL gcode global state is in gmx) - * - * - gn is used by the gcode interpreter and is re-initialized for each - * gcode block.It accepts data in the new gcode block in the formats - * present in the block (pre-normalized forms). During initialization - * some state elements are necessarily restored from gm. - * - * - gf is used by the gcode parser interpreter to hold flags for any data - * that has changed in gn during the parse. cm.gf.target[] values are also used - * by the canonical machine during set_target(). - * - * - cfg (config struct in config.h) is also used heavily and contains some - * values that might be considered to be Gcode model values. The distinction - * is that all values in the config are persisted and restored, whereas the - * gm structs are transient. So cfg has the G54 - G59 offsets, but gm has the - * G92 offsets. cfg has the power-on / reset gcode default values, but gm has - * the operating state for the values (which may have changed). - */ -typedef struct GCodeState { // Gcode model state - used by model, planning and runtime - uint32_t linenum; // Gcode block line number - cmMotionMode motion_mode; // Group1: G0, G1, G2, G3, G38.2, G80, G81, - // G82, G83 G84, G85, G86, G87, G88, G89 - - float target[AXES]; // XYZABC where the move should go - float target_comp[AXES]; // summation compensation (Kahan) overflow value - float work_offset[AXES]; // offset from the work coordinate system (for reporting only) - - float feed_rate; // F - normalized to millimeters/minute or in inverse time mode - float parameter; // P - parameter used for dwell time in seconds, G10 coord select... - - cmFeedRateMode feed_rate_mode; // See cmFeedRateMode for settings - cmCanonicalPlane select_plane; // G17,G18,G19 - values to set plane to - cmUnitsMode units_mode; // G20,G21 - 0=inches (G20), 1 = mm (G21) - cmPathControl path_control; // G61... EXACT_PATH, EXACT_STOP, CONTINUOUS - cmDistanceMode distance_mode; // G90=use absolute coords, G91=incremental movement - cmDistanceMode arc_distance_mode; // G90.1=use absolute IJK offsets, G91.1=incremental IJK offsets - cmAbsoluteOverride absolute_override;// G53 TRUE = move using machine coordinates - this block only - cmCoordSystem coord_system; // G54-G59 - select coordinate system 1-9 - uint8_t tool; // G // M6 tool change - moves "tool_select" to "tool" - uint8_t tool_select; // G // T value - T sets this value - - void reset() { - linenum = 0; - motion_mode = MOTION_MODE_STRAIGHT_TRAVERSE; - - for (uint8_t i = 0; i< AXES; i++) { - target[i] = 0.0; - work_offset[i] = 0.0; - } - - feed_rate = 0.0; - parameter = 0.0; - - feed_rate_mode = INVERSE_TIME_MODE; - select_plane = CANON_PLANE_XY; - units_mode = INCHES; - path_control = PATH_EXACT_PATH; - distance_mode = ABSOLUTE_DISTANCE_MODE; - arc_distance_mode = ABSOLUTE_DISTANCE_MODE; - absolute_override = ABSOLUTE_OVERRIDE_OFF; - coord_system = ABSOLUTE_COORDS; - tool = 1; - tool_select = 1; - }; -} GCodeState_t; - -typedef struct GCodeStateExtended { // Gcode dynamic state extensions - used by model and arcs - uint16_t magic_start; // magic number to test memory integrity - uint8_t next_action; // handles G modal group 1 moves & non-modals - uint8_t program_flow; // used only by the gcode_parser - - float position[AXES]; // XYZABC model position (Note: not used in gn or gf) - float origin_offset[AXES]; // XYZABC G92 offsets (Note: not used in gn or gf) - float g28_position[AXES]; // XYZABC stored machine position for G28 - float g30_position[AXES]; // XYZABC stored machine position for G30 - - bool m48_enable; // master feedrate / spindle speed override enable - bool mfo_enable; // feedrate override enable - float mfo_factor; // 1.0000 x F feed rate. Go up or down from there - bool mto_enable; // traverse override enable - float mto_factor; // valid from 0.05 to 1.00 - - bool origin_offset_enable; // G92 offsets enabled/disabled. 0=disabled, 1=enabled - bool block_delete_switch; // set true to enable block deletes (true is default) - -// unimplemented gcode parameters -// float cutter_radius; // D - cutter radius compensation (0 is off) -// float cutter_length; // H - cutter length compensation (0 is off) - - uint32_t last_line_number; // keep track of the last issued line number - - uint16_t magic_end; - -} GCodeStateX_t; +typedef enum { // job kill state machine + JOB_KILL_OFF = 0, + JOB_KILL_REQUESTED, + JOB_KILL_RUNNING +} cmJobKillState; /***************************************************************************** * CANONICAL MACHINE STRUCTURES */ typedef struct cmAxis { + + // axis settings cmAxisMode axis_mode; // see cmAxisMode above float velocity_max; // max velocity in mm/min or deg/min - float recip_velocity_max; float feedrate_max; // max velocity in mm/min or deg/min - float recip_feedrate_max; - float travel_max; // max work envelope for soft limits - float travel_min; // min work envelope for soft limits float jerk_max; // max jerk (Jm) in mm/min^3 divided by 1 million float jerk_high; // high speed deceleration jerk (Jh) in mm/min^3 divided by 1 million - //float recip_jerk; // stored reciprocal of current jerk value - has the million in it - float max_junction_accel; // cornering time quanta times JERK_MULTIPLIER, must be multiplied by active jerk first! - float junction_dev; // aka cornering delta -- DEPRICATED! + float travel_min; // min work envelope for soft limits + float travel_max; // max work envelope for soft limits float radius; // radius in mm for rotary axis modes + // internal derived variables - computed during data entry and cached for computational efficiency + float recip_velocity_max; + float recip_feedrate_max; + float max_junction_accel; + float high_junction_accel; + + // homing settings uint8_t homing_input; // set 1-N for homing input. 0 will disable homing uint8_t homing_dir; // 0=search to negative, 1=search to positive float search_velocity; // homing search velocity @@ -386,48 +223,94 @@ typedef struct cmAxis { float zero_backoff; // backoff from switches for machine zero } cfgAxis_t; -typedef struct cmSingleton { // struct to manage cm globals and cycles +typedef struct cmArc { // planner and runtime variables for arc generation + magic_t magic_start; + uint8_t run_state; // runtime state machine sequence + + float position[AXES]; // accumulating runtime position + float ijk_offset[3]; // arc IJK offsets + + float length; // length of line or helix in mm + float radius; // Raw R value, or computed via offsets + float theta; // starting angle of arc + float angular_travel; // travel along the arc in radians + float planar_travel; // travel in arc plane in mm + float linear_travel; // travel along linear axis of arc in mm + bool full_circle; // True if full circle arcs specified + float rotations; // number of full rotations to add (P value + sign) + + cmAxes plane_axis_0; // arc plane axis 0 - e.g. X for G17 + cmAxes plane_axis_1; // arc plane axis 1 - e.g. Y for G17 + cmAxes linear_axis; // linear axis (normal to plane) + + float segments; // number of segments in arc or blend + int32_t segment_count; // count of running segments + float segment_theta; // angular motion per segment + float segment_linear_travel; // linear motion per segment + float center_0; // center of circle at plane axis 0 (e.g. X for G17) + float center_1; // center of circle at plane axis 1 (e.g. Y for G17) + + GCodeState_t gm; // Gcode state struct is passed for each arc segment. + magic_t magic_end; +} cmArc_t; + +typedef struct cmMachine { // struct to manage canonical machine globals and state magic_t magic_start; // magic number to test memory integrity /**** Config variables (PUBLIC) ****/ - // system group settings + // System group settings float junction_integration_time; // how aggressively will the machine corner? 1.6 or so is about the upper limit float chordal_tolerance; // arc chordal accuracy setting in mm + float feedhold_z_lift; // mm to move Z axis on feedhold, or 0 to disable bool soft_limit_enable; // true to enable soft limit testing on Gcode inputs bool limit_enable; // true to enable limit switches (disabled is same as override) - bool safety_interlock_enable; // true to enable safety interlock system + + // Coordinate systems and offsets + float coord_offset[COORDS+1][AXES]; // persistent coordinate offsets: absolute (G53) + G54,G55,G56,G57,G58,G59 + float tool_offset[AXES]; // current tool offset + + // Axis settings + cfgAxis_t a[AXES]; // gcode power-on default settings - defaults are not the same as the gm state - cmCoordSystem default_coord_system; // G10 active coordinate system default + cmCoordSystem default_coord_system; // G10 active coordinate system default cmCanonicalPlane default_select_plane; // G17,G18,G19 reset default - cmUnitsMode default_units_mode; // G20,G21 reset default - cmPathControl default_path_control; // G61,G61.1,G64 reset default - cmDistanceMode default_distance_mode; // G90,G91 reset default - - // coordinate systems and offsets - float offset[COORDS+1][AXES]; // persistent coordinate offsets: absolute (G53) + G54,G55,G56,G57,G58,G59 - float tl_offset[AXES]; // current tool length offset - float tt_offset[TOOLS+1][AXES]; // persistent tool table offsets - - // settings for axes X,Y,Z,A B,C - cfgAxis_t a[AXES]; + cmUnitsMode default_units_mode; // G20,G21 reset default + cmPathControl default_path_control; // G61,G61.1,G64 reset default + cmDistanceMode default_distance_mode; // G90,G91 reset default /**** Runtime variables (PRIVATE) ****/ - // global state variables and requestors + // Global state variables and flags - cmMachineState machine_state; // macs: machine/cycle/motion is the actual machine state - cmCycleState cycle_state; // cycs - cmMotionState motion_state; // momo + cmMachineState machine_state; // macs: machine/cycle/motion is the actual machine state + cmCycleType cycle_type; // cycs + cmMotionState motion_state; // mots + + cmFeedholdType hold_type; // hold: type of feedhold requested + cmFeedholdExit hold_exit; // hold: final state of hold on exit + cmMotionProfile hold_profile; // hold: motion profile to use for deceleration cmFeedholdState hold_state; // hold: feedhold state machine - cmOverrideState mfo_state; // feed override state machine - cmQueueFlushState queue_flush_state; // master queue flush state machine + cmFlushState queue_flush_state; // queue flush state machine + cmCycleState cycle_start_state; // used to manage cycle starts and restarts + cmJobKillState job_kill_state; // used to manage job kill transitions + cmOverrideState mfo_state; // feed override state machine + + bool return_flags[AXES]; // flags for recording which axes moved - used in feedhold exit move + + uint8_t limit_requested; // set non-zero to request limit switch processing (value is input number) + uint8_t shutdown_requested; // set non-zero to request shutdown in support of external estop (value is input number) + bool deferred_write_flag; // G10 data has changed (e.g. offsets) - flag to persist them + + bool safety_interlock_enable; // true to enable safety interlock system + bool request_interlock; // enter interlock + bool request_interlock_exit; // exit interlock uint8_t safety_interlock_disengaged; // set non-zero to start interlock processing (value is input number) uint8_t safety_interlock_reengaged; // set non-zero to end interlock processing (value is input number) cmSafetyState safety_interlock_state; // safety interlock state - uint32_t esc_boot_timer; // timer for Electronic Speed Control (Spindle electronics) to boot + // Motate::Timeout esc_boot_timer; // timer for Electronic Speed Control (Spindle electronics) to boot cmHomingState homing_state; // home: homing cycle sub-state machine uint8_t homed[AXES]; // individual axis homing flags @@ -437,31 +320,31 @@ typedef struct cmSingleton { // struct to manage cm globals and c uint8_t probe_input; // probing digital input float probe_results[PROBES_STORED][AXES]; // probing results - float rotation_matrix[3][3]; // three-by-three rotation matrix. We ignore rotary axes. - float rotation_z_offset; // we separately handle a z-offset, so that the new plane - // maintains a consistent distance from the old one. - // We only need z, since we are rotating to the z axis. + float rotation_matrix[3][3]; // three-by-three rotation matrix. We ignore UVW and ABC axes + float rotation_z_offset; // separately handle a z-offset to maintain consistent distance to bed - float jogging_dest; // jogging direction as a relative move from current position + float jogging_dest; // jogging destination as a relative move from current position - bool g28_flag; // true = complete a G28 move - bool g30_flag; // true = complete a G30 move - bool deferred_write_flag; // G10 data has changed (e.g. offsets) - flag to persist them - bool end_hold_requested; // request restart after feedhold - uint8_t limit_requested; // set non-zero to request limit switch processing (value is input number) - uint8_t shutdown_requested; // set non-zero to request shutdown in support of external estop (value is input number) - - /**** Model states ****/ + /**** Model state structures ****/ + void *mp; // linked mpPlanner_t - use a void pointer to avoid circular header files + cmArc_t arc; // arc parameters GCodeState_t *am; // active Gcode model is maintained by state management GCodeState_t gm; // core gcode model state GCodeStateX_t gmx; // extended gcode model state magic_t magic_end; -} cmSingleton_t; +} cmMachine_t; -/**** Externs - See canonical_machine.c for allocation ****/ +typedef struct cmToolTable { // struct to keep a global tool table + float tt_offset[TOOLS+1][AXES]; // persistent tool table offsets +} cmToolTable_t; -extern cmSingleton_t cm; // canonical machine controller singleton +/**** Externs - See canonical_machine.cpp for allocation ****/ + +extern cmMachine_t *cm; // pointer to active canonical machine +extern cmMachine_t cm1; // canonical machine primary machine +extern cmMachine_t cm2; // canonical machine secondary machine +extern cmToolTable_t tt; /***************************************************************************** * FUNCTION PROTOTYPES @@ -471,17 +354,16 @@ extern cmSingleton_t cm; // canonical machine controller singleton /*--- Internal functions and helpers ---*/ // Model state getters and setters -cmCombinedState cm_get_combined_state(void); +cmCombinedState cm_get_combined_state(cmMachine_t *_cm); cmMachineState cm_get_machine_state(void); -cmCycleState cm_get_cycle_state(void); +cmCycleType cm_get_cycle_type(void); cmMotionState cm_get_motion_state(void); cmFeedholdState cm_get_hold_state(void); cmHomingState cm_get_homing_state(void); +cmProbeState cm_get_probe_state(void); uint8_t cm_get_jogging_state(void); void cm_set_motion_state(const cmMotionState motion_state); -float cm_get_axis_jerk(const uint8_t axis); -void cm_set_axis_jerk(const uint8_t axis, const float jerk); uint32_t cm_get_linenum(const GCodeState_t *gcode_state); cmMotionMode cm_get_motion_mode(const GCodeState_t *gcode_state); @@ -500,19 +382,18 @@ float cm_get_feed_rate(const GCodeState_t *gcode_state); void cm_set_motion_mode(GCodeState_t *gcode_state, const uint8_t motion_mode); void cm_set_tool_number(GCodeState_t *gcode_state, const uint8_t tool); void cm_set_absolute_override(GCodeState_t *gcode_state, const uint8_t absolute_override); -void cm_set_model_linenum(const uint32_t linenum); +void cm_set_model_linenum(int32_t linenum); stat_t cm_check_linenum(); // Coordinate systems and offsets -float cm_get_active_coord_offset(const uint8_t axis); -float cm_get_work_offset(const GCodeState_t *gcode_state, const uint8_t axis); -void cm_set_work_offsets(GCodeState_t *gcode_state); +float cm_get_combined_offset(const uint8_t axis); +float cm_get_display_offset(const GCodeState_t *gcode_state, const uint8_t axis); +void cm_set_display_offsets(GCodeState_t *gcode_state); +float cm_get_display_position(const GCodeState_t *gcode_state, const uint8_t axis); float cm_get_absolute_position(const GCodeState_t *gcode_state, const uint8_t axis); -float cm_get_work_position(const GCodeState_t *gcode_state, const uint8_t axis); // Critical helpers -void cm_update_model_position_from_runtime(void); -void cm_finalize_move(void); +void cm_update_model_position(void); stat_t cm_deferred_write_callback(void); void cm_set_model_target(const float target[], const bool flag[]); bool cm_get_soft_limits(void); @@ -523,25 +404,12 @@ stat_t cm_test_soft_limits(const float target[]); /*--- Canonical machining functions (loosely) defined by NIST [organized by NIST Gcode doc] ---*/ // Initialization and termination (4.3.2) -void canonical_machine_init(void); -void canonical_machine_reset_rotation(void); // NOT in NIST -void canonical_machine_reset(void); -void canonical_machine_init_assertions(void); -stat_t canonical_machine_test_assertions(void); - -// Alarms and state management -stat_t cm_alrm(nvObj_t *nv); // trigger alarm from command input -stat_t cm_shutd(nvObj_t *nv); // trigger shutdown from command input -stat_t cm_pnic(nvObj_t *nv); // trigger panic from command input -stat_t cm_clr(nvObj_t *nv); // clear alarm and shutdown from command input -void cm_clear(void); // raw clear command -void cm_parse_clear(const char *s); // parse gcode for M30 or M2 clear condition -stat_t cm_is_alarmed(void); // return non-zero status if alarm, shutdown or panic -void cm_halt_all(void); // halt motion, spindle and coolant -void cm_halt_motion(void); // halt motion (immediate stop) but not spindle & other IO -stat_t cm_alarm(const stat_t status, const char *msg); // enter alarm state - preserve Gcode state -stat_t cm_shutdown(const stat_t status, const char *msg); // enter shutdown state - dump all state -stat_t cm_panic(const stat_t status, const char *msg); // enter panic state - needs RESET +void canonical_machine_inits(void); +void canonical_machine_init(cmMachine_t *_cm, void *_mp); +void canonical_machine_reset_rotation(cmMachine_t *_cm); // NOT in NIST +void canonical_machine_reset(cmMachine_t *_cm); +void canonical_machine_init_assertions(cmMachine_t *_cm); +stat_t canonical_machine_test_assertions(cmMachine_t *_cm); // Representation (4.3.3) stat_t cm_select_plane(const uint8_t plane); // G17, G18, G19 @@ -555,18 +423,19 @@ stat_t cm_set_g10_data(const uint8_t P_word, const bool P_flag, // G const uint8_t L_word, const bool L_flag, const float offset[], const bool flag[]); -void cm_set_position(const uint8_t axis, const float position); // set absolute position - single axis +void cm_set_position_by_axis(const uint8_t axis, const float position); // set position to abs pos - single axis +void cm_reset_position_to_absolute_position(cmMachine_t *_cm); // set position to abs pos - all axes stat_t cm_set_absolute_origin(const float origin[], bool flag[]); // G28.3 void cm_set_axis_origin(uint8_t axis, const float position); // G28.3 planner callback stat_t cm_set_coord_system(const uint8_t coord_system); // G54 - G59 -stat_t cm_set_origin_offsets(const float offset[], const bool flag[]); // G92 -stat_t cm_reset_origin_offsets(void); // G92.1 -stat_t cm_suspend_origin_offsets(void); // G92.2 -stat_t cm_resume_origin_offsets(void); // G92.3 +stat_t cm_set_g92_offsets(const float offset[], const bool flag[]); // G92 +stat_t cm_reset_g92_offsets(void); // G92.1 +stat_t cm_suspend_g92_offsets(void); // G92.2 +stat_t cm_resume_g92_offsets(void); // G92.3 // Free Space Motion (4.3.4) -stat_t cm_straight_traverse(const float target[], const bool flags[]); // G0 +stat_t cm_straight_traverse(const float *target, const bool *flags, const uint8_t motion_profile); // G0 stat_t cm_set_g28_position(void); // G28.1 stat_t cm_goto_g28_position(const float target[], const bool flags[]); // G28 stat_t cm_set_g30_position(void); // G30.1 @@ -578,7 +447,7 @@ stat_t cm_set_feed_rate_mode(const uint8_t mode); // G stat_t cm_set_path_control(GCodeState_t *gcode_state, const uint8_t mode); // G61, G61.1, G64 // Machining Functions (4.3.6) -stat_t cm_straight_feed(const float target[], const bool flags[]); // G1 +stat_t cm_straight_feed(const float *target, const bool *flags, const uint8_t motion_profile); //G1 stat_t cm_dwell(const float seconds); // G4, P parameter stat_t cm_arc_feed(const float target[], const bool target_f[], // G2/G3 - target endpoint @@ -602,25 +471,11 @@ void cm_message(const char *message); // msg to consol void cm_reset_overrides(void); stat_t cm_m48_enable(uint8_t enable); // M48, M49 -stat_t cm_mfo_enable(uint8_t enable); // M50 -stat_t cm_mfo_control(const float P_word, const bool P_flag); // M50 -stat_t cm_mto_control(const float P_word, const bool P_flag); // M50.1 -// See spindle.cpp for cm_sso_control() // M51 +stat_t cm_fro_control(const float P_word, const bool P_flag); // M50 +stat_t cm_tro_control(const float P_word, const bool P_flag); // M50.1 +// See spindle.cpp for cm_spo_control() // M51 // Program Functions (4.3.10) -void cm_request_feedhold(void); -void cm_request_end_hold(void); -void cm_request_queue_flush(void); -void cm_request_end_queue_flush(void); -stat_t cm_feedhold_sequencing_callback(void); // process feedhold, cycle start and queue flush requests - -bool cm_has_hold(void); -void cm_start_hold(void); -void cm_end_hold(void); - -void cm_queue_flush(void); // flush serial and planner queues with coordinate resets -void cm_end_queue_flush(void); - void cm_cycle_start(void); // (no Gcode) void cm_cycle_end(void); // (no Gcode) void cm_canned_cycle_end(void); // end of canned cycle @@ -632,9 +487,23 @@ stat_t cm_json_command(char *json_string); // M100 stat_t cm_json_command_immediate(char *json_string); // M100.1 stat_t cm_json_wait(char *json_string); // M102 -/*--- Cycles ---*/ +/**** Cycles and External FIles ****/ -// Homing cycles +// Feedhold and related functions (cycle_feedhold.cpp) +void cm_operation_init(void); +stat_t cm_operation_runner_callback(void); // operation action runner + +void cm_request_alarm(void); +void cm_request_fasthold(void); +void cm_request_cycle_start(void); +void cm_request_feedhold(cmFeedholdType type, cmFeedholdExit exit); +void cm_request_queue_flush(void); +stat_t cm_feedhold_sequencing_callback(void); // process feedhold, cycle start and queue flush requests +stat_t cm_feedhold_command_blocker(void); + +bool cm_has_hold(void); // has hold in primary planner + +// Homing cycles (cycle_homing.cpp) stat_t cm_homing_cycle_start(const float axes[], const bool flags[]); // G28.2 stat_t cm_homing_cycle_start_no_set(const float axes[], const bool flags[]); // G28.4 stat_t cm_homing_cycle_callback(void); // G28.2/.4 main loop callback @@ -646,64 +515,140 @@ stat_t cm_probing_cycle_callback(void); // G38.x main lo stat_t cm_get_prbr(nvObj_t *nv); // enable/disable probe report stat_t cm_set_prbr(nvObj_t *nv); -// Jogging cycle +// Jogging cycle (cycle_jogging.cpp) stat_t cm_jogging_cycle_callback(void); // jogging cycle main loop stat_t cm_jogging_cycle_start(uint8_t axis); // {"jogx":-100.3} -float cm_get_jogging_dest(void); +float cm_get_jogging_dest(void); // get jogging destination -/*--- cfgArray interface functions ---*/ +// Alarm management (alarm.cpp) +stat_t cm_alrm(nvObj_t *nv); // trigger alarm from command input +stat_t cm_shutd(nvObj_t *nv); // trigger shutdown from command input +stat_t cm_pnic(nvObj_t *nv); // trigger panic from command input +stat_t cm_clr(nvObj_t *nv); // clear alarm and shutdown from command input +void cm_clear(void); // raw clear command +void cm_parse_clear(const char *s); // parse gcode for M30 or M2 clear condition +stat_t cm_is_alarmed(void); // return non-zero status if alarm, shutdown or panic +void cm_halt(void); // halt motion, spindle, coolant, heaters +void cm_halt_motion(void); // halt motion (immediate stop) but not spindle & other IO +stat_t cm_alarm(const stat_t status, const char *msg); // enter alarm state - preserve Gcode state +stat_t cm_shutdown(const stat_t status, const char *msg); // enter shutdown state - dump all state +stat_t cm_panic(const stat_t status, const char *msg); // enter panic state - needs RESET +void cm_request_job_kill(void); // control-D handler + +/**** cfgArray interface functions ****/ char cm_get_axis_char(const int8_t axis); -cmAxisType cm_get_axis_type(const index_t index); +cmAxisType cm_get_axis_type(const nvObj_t *nv); stat_t cm_get_mline(nvObj_t *nv); // get model line number stat_t cm_get_line(nvObj_t *nv); // get active (model or runtime) line number stat_t cm_get_stat(nvObj_t *nv); // get combined machine state as value and string +stat_t cm_get_stat2(nvObj_t *nv); // get combined machine state as value and string stat_t cm_get_macs(nvObj_t *nv); // get raw machine state as value and string -stat_t cm_get_cycs(nvObj_t *nv); // get raw cycle state (etc etc)... -stat_t cm_get_mots(nvObj_t *nv); // get raw motion state... -stat_t cm_get_hold(nvObj_t *nv); // get raw hold state... -stat_t cm_get_home(nvObj_t *nv); // get raw homing state... -stat_t cm_get_unit(nvObj_t *nv); // get unit mode... -stat_t cm_get_coor(nvObj_t *nv); // get coordinate system in effect... -stat_t cm_get_momo(nvObj_t *nv); // get motion mode... -stat_t cm_get_plan(nvObj_t *nv); // get active plane... -stat_t cm_get_path(nvObj_t *nv); // get patch control mode... -stat_t cm_get_dist(nvObj_t *nv); // get distance mode... -stat_t cm_get_admo(nvObj_t *nv); // get arc offset mode... -stat_t cm_get_frmo(nvObj_t *nv); // get feedrate mode... +stat_t cm_get_cycs(nvObj_t *nv); // get raw cycle state +stat_t cm_get_mots(nvObj_t *nv); // get raw motion state +stat_t cm_get_hold(nvObj_t *nv); // get raw hold state + +stat_t cm_get_home(nvObj_t *nv); // get machine homing state +stat_t cm_set_home(nvObj_t *nv); // set machine homing state +stat_t cm_get_hom(nvObj_t *nv); // get homing state for axis +stat_t cm_get_prob(nvObj_t *nv); // get probe state +stat_t cm_get_prb (nvObj_t *nv); // get probe result for axis +stat_t cm_run_jog(nvObj_t *nv); // start jogging cycle + +stat_t cm_get_unit(nvObj_t *nv); // get unit mode +stat_t cm_get_coor(nvObj_t *nv); // get coordinate system in effect +stat_t cm_get_momo(nvObj_t *nv); // get motion mode +stat_t cm_get_plan(nvObj_t *nv); // get active plane +stat_t cm_get_path(nvObj_t *nv); // get patch control mode +stat_t cm_get_dist(nvObj_t *nv); // get distance mode +stat_t cm_get_admo(nvObj_t *nv); // get arc offset mode +stat_t cm_get_frmo(nvObj_t *nv); // get feedrate mode stat_t cm_get_toolv(nvObj_t *nv); // get tool (value) stat_t cm_get_pwr(nvObj_t *nv); // get motor power enable state -stat_t cm_get_vel(nvObj_t *nv); // get runtime velocity... +stat_t cm_get_vel(nvObj_t *nv); // get runtime velocity stat_t cm_get_feed(nvObj_t *nv); // get feed rate, converted to units -stat_t cm_get_pos(nvObj_t *nv); // get runtime work position... -stat_t cm_get_mpo(nvObj_t *nv); // get runtime machine position... -stat_t cm_get_ofs(nvObj_t *nv); // get runtime work offset... -stat_t cm_get_tof(nvObj_t *nv); // get runtime tool length offset... +stat_t cm_get_pos(nvObj_t *nv); // get runtime work position +stat_t cm_get_mpo(nvObj_t *nv); // get runtime machine position +stat_t cm_get_ofs(nvObj_t *nv); // get runtime work offset +stat_t cm_get_coord(nvObj_t *nv); // get coordinate offset +stat_t cm_set_coord(nvObj_t *nv); // set coordinate offset -stat_t cm_run_qf(nvObj_t *nv); // run queue flush +stat_t cm_get_g92e(nvObj_t *nv); // get g92 enable state +stat_t cm_get_g92(nvObj_t *nv); // get g92 offset +stat_t cm_get_g28(nvObj_t *nv); // get g28 offset +stat_t cm_get_g30(nvObj_t *nv); // get g30 offset + +//stat_t cm_run_qf(nvObj_t *nv); // run queue flush stat_t cm_run_home(nvObj_t *nv); // start homing cycle -stat_t cm_dam(nvObj_t *nv); // dump active model (debugging command) +//stat_t cm_dam(nvObj_t *nv); // dump active model (debugging command) -stat_t cm_run_jogx(nvObj_t *nv); // start jogging cycle for x -stat_t cm_run_jogy(nvObj_t *nv); // start jogging cycle for y -stat_t cm_run_jogz(nvObj_t *nv); // start jogging cycle for z -stat_t cm_run_joga(nvObj_t *nv); // start jogging cycle for a +stat_t cm_get_tof(nvObj_t *nv); // get tool offset +stat_t cm_set_tof(nvObj_t *nv); // set tool offset +stat_t cm_get_tt(nvObj_t *nv); // get tool table value +stat_t cm_set_tt(nvObj_t *nv); // set tool table value stat_t cm_get_am(nvObj_t *nv); // get axis mode stat_t cm_set_am(nvObj_t *nv); // set axis mode -stat_t cm_set_hi(nvObj_t *nv); // set homing input +stat_t cm_get_tn(nvObj_t *nv); // get travel minimum +stat_t cm_set_tn(nvObj_t *nv); // set travel minimum +stat_t cm_get_tm(nvObj_t *nv); // get travel maximum +stat_t cm_set_tm(nvObj_t *nv); // set travel maximum +stat_t cm_get_ra(nvObj_t *nv); // get radius +stat_t cm_set_ra(nvObj_t *nv); // set radius -stat_t cm_set_jt(nvObj_t *nv); // set junction integration time constant +float cm_get_axis_jerk(const uint8_t axis); +void cm_set_axis_max_jerk(const uint8_t axis, const float jerk); +void cm_set_axis_high_jerk(const uint8_t axis, const float jerk); + +stat_t cm_get_vm(nvObj_t *nv); // get velocity max stat_t cm_set_vm(nvObj_t *nv); // set velocity max and reciprocal +stat_t cm_get_fr(nvObj_t *nv); // get feedrate max stat_t cm_set_fr(nvObj_t *nv); // set feedrate max and reciprocal +stat_t cm_get_jm(nvObj_t *nv); // get jerk max with 1,000,000 correction stat_t cm_set_jm(nvObj_t *nv); // set jerk max with 1,000,000 correction +stat_t cm_get_jh(nvObj_t *nv); // get jerk high with 1,000,000 correction stat_t cm_set_jh(nvObj_t *nv); // set jerk high with 1,000,000 correction -stat_t cm_set_mfo(nvObj_t *nv); // set manual feedrate override factor -stat_t cm_set_mto(nvObj_t *nv); // set manual traverse override factor +stat_t cm_get_hi(nvObj_t *nv); // get homing input +stat_t cm_set_hi(nvObj_t *nv); // set homing input +stat_t cm_get_hd(nvObj_t *nv); // get homing direction +stat_t cm_set_hd(nvObj_t *nv); // set homing direction +stat_t cm_get_sv(nvObj_t *nv); // get homing search velocity +stat_t cm_set_sv(nvObj_t *nv); // set homing search velocity +stat_t cm_get_lv(nvObj_t *nv); // get homing latch velocity +stat_t cm_set_lv(nvObj_t *nv); // set homing latch velocity +stat_t cm_get_lb(nvObj_t *nv); // get homing latch backoff +stat_t cm_set_lb(nvObj_t *nv); // set homing latch backoff +stat_t cm_get_zb(nvObj_t *nv); // get homing zero backoff +stat_t cm_set_zb(nvObj_t *nv); // set homing zero backoff + +stat_t cm_get_jt(nvObj_t *nv); // get junction integration time constant +stat_t cm_set_jt(nvObj_t *nv); // set junction integration time constant +stat_t cm_get_ct(nvObj_t *nv); // get chordal tolerance +stat_t cm_set_ct(nvObj_t *nv); // set chordal tolerance +stat_t cm_get_zl(nvObj_t *nv); // get feedhold Z lift +stat_t cm_set_zl(nvObj_t *nv); // set feedhold Z lift +stat_t cm_get_sl(nvObj_t *nv); // get soft limit enable +stat_t cm_set_sl(nvObj_t *nv); // set soft limit enable +stat_t cm_get_lim(nvObj_t *nv); // get hard limit enable +stat_t cm_set_lim(nvObj_t *nv); // set hard limit enable +stat_t cm_get_saf(nvObj_t *nv); // get safety interlock enable +stat_t cm_set_saf(nvObj_t *nv); // set safety interlock enable + +stat_t cm_get_m48(nvObj_t *nv); // get M48 value (enable/disable overrides) +stat_t cm_set_m48(nvObj_t *nv); // set M48 value (enable/disable overrides) +stat_t cm_get_froe(nvObj_t *nv); // get feedrate override enable +stat_t cm_set_froe(nvObj_t *nv); // set feedrate override enable +stat_t cm_get_fro(nvObj_t *nv); // get feedrate override factor +stat_t cm_set_fro(nvObj_t *nv); // set feedrate override factor + +stat_t cm_get_troe(nvObj_t *nv); // get traverse override enable +stat_t cm_set_troe(nvObj_t *nv); // set traverse override enable +stat_t cm_get_tro(nvObj_t *nv); // get traverse override factor +stat_t cm_set_tro(nvObj_t *nv); // set traverse override factor stat_t cm_set_probe(nvObj_t *nv); // store current position as the latest probe @@ -714,6 +659,17 @@ stat_t cm_get_tram(nvObj_t *nv); // return if the rotation matrix is non- stat_t cm_set_nxln(nvObj_t *nv); // set what value we expect the next line number to have stat_t cm_get_nxln(nvObj_t *nv); // return what value we expect the next line number to have +stat_t cm_get_gpl(nvObj_t *nv); // get gcode default plane +stat_t cm_set_gpl(nvObj_t *nv); // set gcode default plane +stat_t cm_get_gun(nvObj_t *nv); // get gcode default units mode +stat_t cm_set_gun(nvObj_t *nv); // set gcode default units mode +stat_t cm_get_gco(nvObj_t *nv); // get gcode default coordinate system +stat_t cm_set_gco(nvObj_t *nv); // set gcode default coordinate system +stat_t cm_get_gpa(nvObj_t *nv); // get gcode default path control mode +stat_t cm_set_gpa(nvObj_t *nv); // set gcode default path control mode +stat_t cm_get_gdi(nvObj_t *nv); // get gcode default distance mode +stat_t cm_set_gdi(nvObj_t *nv); // set gcode default distance mode + /*--- text_mode support functions ---*/ #ifdef __TEXT_MODE @@ -753,15 +709,16 @@ stat_t cm_get_nxln(nvObj_t *nv); // return what value we expect the next line void cm_print_jt(nvObj_t *nv); // global CM settings void cm_print_ct(nvObj_t *nv); + void cm_print_zl(nvObj_t *nv); void cm_print_sl(nvObj_t *nv); void cm_print_lim(nvObj_t *nv); void cm_print_saf(nvObj_t *nv); - void cm_print_m48e(nvObj_t *nv); - void cm_print_mfoe(nvObj_t *nv); - void cm_print_mfo(nvObj_t *nv); - void cm_print_mtoe(nvObj_t *nv); - void cm_print_mto(nvObj_t *nv); + void cm_print_m48(nvObj_t *nv); + void cm_print_froe(nvObj_t *nv); + void cm_print_fro(nvObj_t *nv); + void cm_print_troe(nvObj_t *nv); + void cm_print_tro(nvObj_t *nv); void cm_print_tram(nvObj_t *nv); // print if the axis has been rotated void cm_print_nxln(nvObj_t *nv); // print the value of the next line number expected @@ -820,15 +777,17 @@ stat_t cm_get_nxln(nvObj_t *nv); // return what value we expect the next line #define cm_print_jt tx_print_stub // global CM settings #define cm_print_ct tx_print_stub + #define cm_print_zl tx_print_stub #define cm_print_sl tx_print_stub #define cm_print_lim tx_print_stub #define cm_print_saf tx_print_stub - #define cm_print_m48e tx_print_stub - #define cm_print_mfoe tx_print_stub - #define cm_print_mfo tx_print_stub - #define cm_print_mtoe tx_print_stub - #define cm_print_mto tx_print_stub + #define cm_print_m48 tx_print_stub + #define cm_print_froe tx_print_stub + #define cm_print_fro tx_print_stub + #define cm_print_troe tx_print_stub + #define cm_print_tro tx_print_stub + #define cm_print_tram tx_print_stub #define cm_print_tram tx_print_stub #define cm_print_nxln tx_print_stub diff --git a/g2core/config.cpp b/g2core/config.cpp index 26335686..724170b5 100644 --- a/g2core/config.cpp +++ b/g2core/config.cpp @@ -2,7 +2,7 @@ * config.cpp - application independent configuration handling * This file is part of the g2core project * - * Copyright (c) 2010 - 2017 Alden S. Hart, Jr. + * Copyright (c) 2010 - 2019 Alden S. Hart, Jr. * * This file ("the software") is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License, version 2 as published by the @@ -67,7 +67,7 @@ stat_t nv_set(nvObj_t *nv) if (nv->index >= nv_index_max()) { return(STAT_INTERNAL_RANGE_ERROR); } - return (((fptrCmd)GET_TABLE_WORD(set))(nv)); + return (((fptrCmd)cfgArray[nv->index].set)(nv)); } stat_t nv_get(nvObj_t *nv) @@ -75,7 +75,7 @@ stat_t nv_get(nvObj_t *nv) if (nv->index >= nv_index_max()) { return(STAT_INTERNAL_RANGE_ERROR); } - return (((fptrCmd)GET_TABLE_WORD(get))(nv)); + return (((fptrCmd)cfgArray[nv->index].get)(nv)); } void nv_print(nvObj_t *nv) @@ -83,7 +83,7 @@ void nv_print(nvObj_t *nv) if (nv->index >= nv_index_max()) { return; } - ((fptrPrint)GET_TABLE_WORD(print))(nv); + ((fptrCmd)cfgArray[nv->index].print)(nv); } stat_t nv_persist(nvObj_t *nv) @@ -125,13 +125,20 @@ void config_init() static void _set_defa(nvObj_t *nv, bool print) { - cm_set_units_mode(MILLIMETERS); // must do inits in MM mode + cm_set_units_mode(MILLIMETERS); // must do inits in MM mode for (nv->index=0; nv_index_is_single(nv->index); nv->index++) { - if (GET_TABLE_BYTE(flags) & F_INITIALIZE) { - nv->value = GET_TABLE_FLOAT(def_value); + if (cfgArray[nv->index].flags & F_INITIALIZE) { + if ((cfgArray[nv->index].flags & TYPE_INTEGER) || + (cfgArray[nv->index].flags & TYPE_BOOLEAN)) { // Fix for Issue #357 + nv->value_int = cfgArray[nv->index].def_value; + } else { + nv->value_flt = cfgArray[nv->index].def_value; + } strncpy(nv->token, cfgArray[nv->index].token, TOKEN_LEN); - nv_set(nv); - nv_persist(nv); + cfgArray[nv->index].set(nv); // run the set method, nv_set(nv); + if (cfgArray[nv->index].flags & F_PERSIST) { + nv_persist(nv); + } } } sr_init_status_report(); // reset status reports @@ -142,17 +149,19 @@ static void _set_defa(nvObj_t *nv, bool print) stat_t set_defaults(nvObj_t *nv) { - // failsafe. nv->value must be true or no action occurs - if (fp_FALSE(nv->value)) return(help_defa(nv)); + // failsafe. nv->value_int must be true or no action occurs + if (!nv->value_int) { + return(help_defa(nv)); + } _set_defa(nv, true); // The nvlist was used for the initialize message so the values are all garbage // Mark the nv as $defa so it displays nicely in the response nv_reset_nv_list(); strncpy(nv->token, "defa", TOKEN_LEN); -// nv->index = nv_get_index("", nv->token); // correct, but not required - nv->valuetype = TYPE_INT; - nv->value = 1; +// nv->index = nv_get_index("", nv->token); // correct, but not required + nv->valuetype = TYPE_INTEGER; // ++++ probably should be TYPE_BOOLEAN + nv->value_int = true; return (STAT_OK); } @@ -188,9 +197,7 @@ stat_t config_test_assertions() /* Generic gets() * get_nul() - get nothing (returns STAT_NOOP) - * get_ui8() - get value as 8 bit uint8_t (use uint8 for booleans) - * get_int8() - get value as 8 bit int8_t - * get_int() - get value as 32 bit integer + * get_int32() - get value as 32 bit integer * get_data() - get value as 32 bit integer blind cast * get_flt() - get value as float */ @@ -200,55 +207,36 @@ stat_t get_nul(nvObj_t *nv) return (STAT_NOOP); } -stat_t get_ui8(nvObj_t *nv) +stat_t get_int32(nvObj_t *nv) { - nv->value = (float)*((uint8_t *)GET_TABLE_WORD(target)); - nv->valuetype = TYPE_INT; - return (STAT_OK); -} - -stat_t get_int8(nvObj_t *nv) -{ - nv->value = (float)*((int8_t *)GET_TABLE_WORD(target)); - nv->valuetype = TYPE_INT; - return (STAT_OK); -} - -stat_t get_int(nvObj_t *nv) -{ - nv->value = *((uint32_t *)GET_TABLE_WORD(target)); - nv->valuetype = TYPE_INT; - return (STAT_OK); -} - -stat_t get_data(nvObj_t *nv) -{ - uint32_t *v = (uint32_t*)&nv->value; - *v = *((uint32_t *)GET_TABLE_WORD(target)); - nv->valuetype = TYPE_DATA; + nv->value_int = *((int32_t *)GET_TABLE_WORD(target)); + nv->valuetype = TYPE_INTEGER; return (STAT_OK); } stat_t get_flt(nvObj_t *nv) { - nv->value = *((float *)GET_TABLE_WORD(target)); + nv->value_flt= *((float *)GET_TABLE_WORD(target)); nv->precision = (int8_t)GET_TABLE_WORD(precision); nv->valuetype = TYPE_FLOAT; return (STAT_OK); } +stat_t get_data(nvObj_t *nv) +{ + uint32_t *v = (uint32_t*)&nv->value_flt; + *v = *((uint32_t *)GET_TABLE_WORD(target)); + nv->valuetype = TYPE_DATA; + return (STAT_OK); +} + /* Generic sets() - * set_noop() - set nothing and return OK - * set_nul() - set nothing, return OK - * set_ro() - set nothing, return read-only error - * set_ui8() - set value as 8 bit uint8_t value - * set_int8() - set value as an 8 bit int8_t value - * set_01() - set a 0 or 1 uint8_t value with validation - * set_012() - set a 0, 1 or 2 uint8_t value with validation - * set_0123() - set a 0, 1, 2 or 3 uint8_t value with validation - * set_uint() - set value as 32 bit unsigned integer - * set_data() - set value as 32 bit integer blind cast - * set_flt() - set value as float + * set_noop() - set nothing and return OK + * set_nul() - set nothing and return READ_ONLY error + * set_ro() - set nothing, return read-only error + * set_int32() - set value as 32 bit unsigned integer + * set_flt() - set value as float + * set_data() - set value as 32 bit integer blind cast */ stat_t set_noop(nvObj_t *nv) { @@ -257,96 +245,41 @@ stat_t set_noop(nvObj_t *nv) { } stat_t set_nul(nvObj_t *nv) { -// nv->valuetype = TYPE_NULL; -// return (STAT_PARAMETER_IS_READ_ONLY); // this is what it should be - return (STAT_OK); // hack until JSON is refactored + nv->valuetype = TYPE_NULL; + return (STAT_PARAMETER_IS_READ_ONLY); // this is what it should be } stat_t set_ro(nvObj_t *nv) { - // hack. If setting an SR it doesn't fail - if (strcmp(nv_body->token, "sr") == 0) { - return (STAT_OK); + if (strcmp(nv_body->token, "sr") == 0) { // hack. If setting an SR it doesn't fail + return (STAT_OK); } nv->valuetype = TYPE_NULL; - return (STAT_PARAMETER_IS_READ_ONLY); + return (STAT_PARAMETER_IS_READ_ONLY); } -stat_t set_ui8(nvObj_t *nv) +stat_t set_int32(nvObj_t *nv) { - *((uint8_t *)GET_TABLE_WORD(target)) = nv->value; - nv->valuetype = TYPE_INT; - return(STAT_OK); -} - -stat_t set_int8(nvObj_t *nv) -{ - *((int8_t *)GET_TABLE_WORD(target)) = (int8_t)nv->value; - nv->valuetype = TYPE_INT; - return(STAT_OK); -} - -stat_t set_01(nvObj_t *nv) -{ - if (nv->value < 0) { - nv->valuetype = TYPE_NULL; - return (STAT_INPUT_LESS_THAN_MIN_VALUE); - } - if (nv->value > 1) { - nv->valuetype = TYPE_NULL; - return (STAT_INPUT_EXCEEDS_MAX_VALUE); - } - return (set_ui8(nv)); -} - -stat_t set_012(nvObj_t *nv) -{ - if (nv->value < 0) { - nv->valuetype = TYPE_NULL; - return (STAT_INPUT_LESS_THAN_MIN_VALUE); - } - if (nv->value > 2) { - nv->valuetype = TYPE_NULL; - return (STAT_INPUT_EXCEEDS_MAX_VALUE); - } - return (set_ui8(nv)); -} - -stat_t set_0123(nvObj_t *nv) -{ - if (nv->value < 0) { - nv->valuetype = TYPE_NULL; - return (STAT_INPUT_LESS_THAN_MIN_VALUE); - } - if (nv->value > 3) { - nv->valuetype = TYPE_NULL; - return (STAT_INPUT_EXCEEDS_MAX_VALUE); - } - return (set_ui8(nv)); -} - -stat_t set_int(nvObj_t *nv) -{ - *((uint32_t *)GET_TABLE_WORD(target)) = (uint32_t)nv->value; - nv->valuetype = TYPE_INT; - return(STAT_OK); -} - -stat_t set_data(nvObj_t *nv) -{ - uint32_t *v = (uint32_t*)&nv->value; - *((uint32_t *)GET_TABLE_WORD(target)) = *v; - nv->valuetype = TYPE_DATA; + *((int32_t *)GET_TABLE_WORD(target)) = nv->value_int; + nv->valuetype = TYPE_INTEGER; return(STAT_OK); } stat_t set_flt(nvObj_t *nv) { - *((float *)GET_TABLE_WORD(target)) = nv->value; + *((float *)GET_TABLE_WORD(target)) = nv->value_flt; nv->precision = GET_TABLE_WORD(precision); nv->valuetype = TYPE_FLOAT; return(STAT_OK); } +stat_t set_data(nvObj_t *nv) +{ + uint32_t *v = (uint32_t*)&nv->value_flt; + *((uint32_t *)GET_TABLE_WORD(target)) = *v; + nv->valuetype = TYPE_DATA; + return(STAT_OK); +} + /************************************************************************************ * Group operations * @@ -496,6 +429,29 @@ uint8_t nv_get_type(nvObj_t *nv) return (NV_TYPE_CONFIG); } +/* + * nv_coerce_types() - change types based on type field in configApp table + */ + +void nv_coerce_types(nvObj_t *nv) +{ + if (nv->valuetype == TYPE_NULL) { // don't change type if it's a GET query + return; + } + valueType type = (valueType)(cfgArray[nv->index].flags & F_TYPE_MASK); + if (type == TYPE_INTEGER) { + nv->valuetype = TYPE_INTEGER; // will pay attention to the int value, not the float + } else if (type == TYPE_BOOLEAN) { // it may have been marked as a boolean, but if it's not... + nv->valuetype = TYPE_BOOLEAN; + if (nv->valuetype == TYPE_INTEGER) { + nv->value_int = nv->value_int ? true : false; + } else + if (nv->valuetype == TYPE_FLOAT) { + nv->value_int = (fp_ZERO(nv->value_flt)) ? true : false; + } + } +} + /****************************************************************************** * nvObj low-level object and list operations * nv_get_nvObj() - setup a nv object by providing the index @@ -534,20 +490,21 @@ void nv_get_nvObj(nvObj_t *nv) // special processing for system groups and stripping tokens for groups if (nv->group[0] != NUL) { - if (GET_TABLE_BYTE(flags) & F_NOSTRIP) { + if (cfgArray[nv->index].flags & F_NOSTRIP) { nv->group[0] = NUL; } else { strcpy(nv->token, &nv->token[strlen(nv->group)]); // strip group from the token } } - ((fptrCmd)GET_TABLE_WORD(get))(nv); // populate the value + ((fptrCmd)cfgArray[nv->index].get)(nv); // populate the value } nvObj_t *nv_reset_nv(nvObj_t *nv) // clear a single nvObj structure { nv->valuetype = TYPE_EMPTY; // selective clear is much faster than calling memset nv->index = 0; - nv->value = 0; + nv->value_int = 0; + nv->value_flt = 0; nv->precision = 0; nv->token[0] = NUL; nv->group[0] = NUL; @@ -636,7 +593,7 @@ nvObj_t *nv_add_object(const char *token) // add an object to the body usi return (NULL); } -nvObj_t *nv_add_integer(const char *token, const uint32_t value)// add an integer object to the body +nvObj_t *nv_add_integer(const char *token, const int32_t value) // add an integer object to the body { nvObj_t *nv = nv_body; for (uint8_t i=0; itoken, token, TOKEN_LEN); - nv->value = (float) value; - nv->valuetype = TYPE_INT; + nv->value_int = value; + nv->valuetype = TYPE_INTEGER; return (nv); } return (NULL); @@ -666,7 +623,7 @@ nvObj_t *nv_add_data(const char *token, const uint32_t value)// add an integer o } strcpy(nv->token, token); float *v = (float*)&value; - nv->value = *v; + nv->value_flt = *v; nv->valuetype = TYPE_DATA; return (nv); } @@ -684,7 +641,7 @@ nvObj_t *nv_add_float(const char *token, const float value) // add a float ob continue; } strncpy(nv->token, token, TOKEN_LEN); - nv->value = value; + nv->value_flt = value; nv->valuetype = TYPE_FLOAT; return (nv); } @@ -713,12 +670,12 @@ nvObj_t *nv_add_string(const char *token, const char *string) // add a string ob } /* - * cm_conditional_message() - queue a RAM string as a message in the response (conditionally) + * nv_add__conditional_message() - queue a RAM string as a message in the response (conditionally) */ nvObj_t *nv_add_conditional_message(const char *string) // conditionally add a message object to the body { - if ((js.json_mode == JSON_MODE) && (js.echo_json_messages != true)) { return (NULL);} + if ((js.json_mode == JSON_MODE) && (js.echo_json_messages != true)) { return (NULL); } return(nv_add_string((const char *)"msg", string)); } @@ -758,7 +715,7 @@ void nv_dump_nv(nvObj_t *nv) nv->depth, nv->valuetype, nv->precision, - (double)nv->value, + (double)nv->value_flt, // would need to add in value_int to be complete nv->group, nv->token, (char *)nv->stringp); diff --git a/g2core/config.h b/g2core/config.h index 9fc56645..e8c0ca04 100644 --- a/g2core/config.h +++ b/g2core/config.h @@ -2,7 +2,7 @@ * config.h - configuration sub-system generic part (see config_app for application part) * This file is part of the g2core project * - * Copyright (c) 2010 - 2017 Alden S. Hart, Jr. + * Copyright (c) 2010 - 2019 Alden S. Hart, Jr. * * This file ("the software") is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License, version 2 as published by the @@ -69,20 +69,22 @@ * - axis tokens start with the axis letter and are typically 3 characters including the axis letter * - motor tokens start with the motor digit and are typically 3 characters including the motor digit * - non-axis or non-motor tokens are 2-6 characters and by convention generally should not start - * with: xyzabcuvw0123456789 (but there can be exceptions) + * with: xyzuvwabc0123456789 (but there can be exceptions) * * "Groups" are collections of values that mimic REST resources. Groups include: - * - axis groups prefixed by "xyzabc" ("uvw" are reserved) + * - axis groups prefixed by "xyzuvwabc" * - motor groups prefixed by "123456" ("789" are reserved) * - PWM groups prefixed by p1, p2 (p3 - p9 are reserved) * - coordinate system groups prefixed by g54, g55, g56, g57, g59, g28, g30, g92 * - a system group is identified by "sys" and contains a collection of otherwise unrelated values * * "Uber-groups" are groups of groups that are only used for text-mode printing - e.g. - * - group of all axes groups - * - group of all motor groups - * - group of all offset groups - * - group of all groups + * - group of all motor groups (m) + * - group of all axes groups (q) + * - group of all offset groups (o) + * - group of all digital input groups (di) + * - group of all digital output groups (do) + * - group of all groups ($) */ /* --- Making changes and adding new values * @@ -169,9 +171,7 @@ ***********************************************************************************/ // Sizing and footprints // chose one based on # of elements in cfgArray -//typedef uint8_t index_t; // use this if there are < 256 indexed objects -//typedef uint16_t index_t; // use this if there are > 255 indexed objects -typedef uint32_t index_t; // use this because set/get_int is expecting an int32_t +typedef uint32_t index_t; // set/get_int is expecting an int32_t // defines allocated from stack (not-pre-allocated) #define NV_FORMAT_LEN 128 // print formatting string max length @@ -206,37 +206,80 @@ typedef enum { FLOW_CONTROL_RTS // flow control uses RTS/CTS } flowControl; -typedef enum { // value typing for config and JSON - TYPE_EMPTY = -1, // value struct is empty (which is not the same as "NULL") - TYPE_NULL = 0, // value is 'null' (meaning the JSON null value) - TYPE_PARENT, // object is a parent to a sub-object - TYPE_FLOAT, // value is a floating point number - TYPE_INT, // value is a signed or unsigned integer or any size - TYPE_STRING, // value is in string field - TYPE_BOOL, // value is "true" (1) or "false"(0) - TYPE_DATA, // value is blind cast to uint32_t - TYPE_ARRAY // value is array element count, values are CSV ASCII in string field +typedef enum { // value typing for config and JSON + // exception types + TYPE_SKIP = -2, // do not serialize this object (used for filtering) + TYPE_EMPTY = -1, // value struct is empty (which is not the same as "NULL") + + // fundamental JSON types // must be 0-3. Do not change. Used by F_'s and JSON decoding + TYPE_NULL = 0, // value is 'null' (meaning the JSON null value) + TYPE_BOOLEAN = 1, // value is "true" (1) or "false"(0) + TYPE_INTEGER = 2, // value is an 8 or 32 bit signed integer + TYPE_STRING = 3, // value is in string field + + // derived data types // must be 4-7. Do not change. + TYPE_FLOAT = 4, // value is a floating point number + TYPE_UINT32 = 5, // unsigned 32 bit integer (unused) + TYPE_ARRAY = 6, // value is array element count, values are CSV ASCII in string field + TYPE_DATA = 7, // value is blind cast to uint32_t (willbe removed) + + // transient types and types used during JSON processing + TYPE_PARENT // object is a parent to a sub-object +// TYPE_NULL_STRING, // empty string - treated as null +// TYPE_TYPE_ERROR // none of the above } valueType; /**** operations flags and shorthand ****/ -#define F_INITIALIZE 0x01 // initialize this item (run set during initialization) -#define F_PERSIST 0x02 // persist this item when set is run -#define F_NOSTRIP 0x04 // do not strip the group prefix from the token -#define F_CONVERT 0x08 // set if unit conversion is required -#define F_ICONVERT 0x10 // set if unit conversion is required AND value is an inverse quantity +#define F_TYPE_MASK 0x00000007 // 3 LSB's used for data type -#define _f0 0x00 -#define _fi (F_INITIALIZE) -#define _fp (F_PERSIST) -#define _fn (F_NOSTRIP) -#define _fc (F_CONVERT) -#define _fic (F_INITIALIZE | F_CONVERT) -#define _fip (F_INITIALIZE | F_PERSIST) -#define _fipc (F_INITIALIZE | F_PERSIST | F_CONVERT) -#define _fipn (F_INITIALIZE | F_PERSIST | F_NOSTRIP) -#define _fipi (F_INITIALIZE | F_PERSIST | F_ICONVERT) -#define _fipnc (F_INITIALIZE | F_PERSIST | F_NOSTRIP | F_CONVERT) +#define F_INITIALIZE 0x08 // initialize this item (run set during initialization) +#define F_PERSIST 0x10 // persist this item when set is run +#define F_NOSTRIP 0x20 // do not strip the group prefix from the token +#define F_CONVERT 0x40 // set if unit conversion is required +#define F_ICONVERT 0x80 // set if unit conversion is required AND value is an inverse quantity + +// Shorthand +// _n(ull) (used by commands or other case where there is no target data) +// _b(oolean) +// _s(tring) +// _i(nteger) +// _f(loat) +// _t(ext container) +// _d(ata) + +#define _n0 (TYPE_NULL) +#define _nn (TYPE_NULL | F_NOSTRIP) + +#define _s0 (TYPE_STRING) +#define _sn (TYPE_STRING | F_NOSTRIP) + +#define _d0 (TYPE_DATA) +#define _dip (TYPE_DATA | F_INITIALIZE | F_PERSIST) + +#define _b0 (TYPE_BOOLEAN) // boolean data types (only listing the ones we use) +#define _bip (TYPE_BOOLEAN | F_INITIALIZE | F_PERSIST) +#define _bin (TYPE_BOOLEAN | F_INITIALIZE | F_NOSTRIP) +#define _bipn (TYPE_BOOLEAN | F_INITIALIZE | F_PERSIST | F_NOSTRIP) + +#define _i0 (TYPE_INTEGER) // integer data types (only listing the ones we use) +#define _ii (TYPE_INTEGER | F_INITIALIZE) +#define _ip (TYPE_INTEGER | F_PERSIST) +#define _in (TYPE_INTEGER | F_NOSTRIP) +#define _iip (TYPE_INTEGER | F_INITIALIZE | F_PERSIST) +#define _iipn (TYPE_INTEGER | F_INITIALIZE | F_PERSIST | F_NOSTRIP) + +#define _f0 (TYPE_FLOAT) // floating point data types (only listing the ones we use) +#define _fi (TYPE_FLOAT | F_INITIALIZE) +#define _fp (TYPE_FLOAT | F_PERSIST) +#define _fn (TYPE_FLOAT | F_NOSTRIP) +#define _fip (TYPE_FLOAT | F_INITIALIZE | F_PERSIST) +#define _fic (TYPE_FLOAT | F_INITIALIZE | F_CONVERT) +#define _fin (TYPE_FLOAT | F_INITIALIZE | F_NOSTRIP) +#define _fipc (TYPE_FLOAT | F_INITIALIZE | F_PERSIST | F_CONVERT) +#define _fipn (TYPE_FLOAT | F_INITIALIZE | F_PERSIST | F_NOSTRIP) +#define _fipi (TYPE_FLOAT | F_INITIALIZE | F_PERSIST | F_ICONVERT) +#define _fipnc (TYPE_FLOAT | F_INITIALIZE | F_PERSIST | F_NOSTRIP | F_CONVERT) /**** Structures ****/ @@ -254,14 +297,15 @@ typedef struct nvString { // shared string object typedef struct nvObject { // depending on use, not all elements may be populated struct nvObject *pv; // pointer to previous object or NULL if first object struct nvObject *nx; // pointer to next object or NULL if last object - index_t index; // index of tokenized name, or -1 if no token (optional) int8_t depth; // depth of object in the tree. 0 is root (-1 is invalid) - valueType valuetype; // see valueType enum - int8_t precision; // decimal precision for reporting (JSON) - float value; // numeric value char group[GROUP_LEN+1]; // group prefix or NUL if not in a group char token[TOKEN_LEN+1]; // full mnemonic token for lookup - char (*stringp)[]; // pointer to array of characters from shared character array + index_t index; // index of tokenized name, or -1 if no token (optional) + valueType valuetype; // type of value that follows: see valueType enum + int8_t precision; // decimal precision for reporting floating point numbers (JSON only) + float value_flt; // floating point values + int32_t value_int; // signed integer values and booleans + char (*stringp)[]; // string value: pointer to array of characters from shared character array } nvObj_t; // OK, so it's not REALLY an object typedef uint8_t (*fptrCmd)(nvObj_t *nv);// required for cfg table access @@ -281,7 +325,7 @@ typedef struct cfgItem { fptrPrint print; // print binding: aka void (*print)(nvObj_t *nv); fptrCmd get; // GET binding aka uint8_t (*get)(nvObj_t *nv) fptrCmd set; // SET binding aka uint8_t (*set)(nvObj_t *nv) - float *target; // target for writing config value + void *target; // target for writing config value float def_value; // default value for config item } cfgItem_t; @@ -311,6 +355,7 @@ stat_t nv_persist(nvObj_t *nv); // main entry point for persistence // helpers uint8_t nv_get_type(nvObj_t *nv); +void nv_coerce_types(nvObj_t *nv); index_t nv_get_index(const char *group, const char *token); index_t nv_index_max(void); // (see config_app.c) bool nv_index_is_single(index_t index); // (see config_app.c) @@ -319,25 +364,17 @@ bool nv_index_lt_groups(index_t index); // (see config_app.c) bool nv_group_is_prefixed(char *group); // generic internal functions and accessors -stat_t set_noop(nvObj_t *nv); // set nothing and return OK -stat_t set_nul(nvObj_t *nv); // set nothing, return OK -stat_t set_ro(nvObj_t *nv); // set nothing, return read-only error -stat_t set_ui8(nvObj_t *nv); // set uint8_t value -stat_t set_int8(nvObj_t *nv); // set signed 8 bit integer -stat_t set_01(nvObj_t *nv); // set a 0 or 1 value with validation -stat_t set_012(nvObj_t *nv); // set a 0, 1 or 2 value with validation -stat_t set_0123(nvObj_t *nv); // set a 0, 1, 2 or 3 value with validation -stat_t set_int(nvObj_t *nv); // set uint32_t integer value -stat_t set_data(nvObj_t *nv); // set uint32_t integer value blind cast -stat_t set_flt(nvObj_t *nv); // set floating point value - stat_t get_nul(nvObj_t *nv); // get null value type -stat_t get_bool(nvObj_t *nv); // get boolean value -stat_t get_ui8(nvObj_t *nv); // get uint8_t value -stat_t get_int8(nvObj_t *nv); // get signed 8 bit integer -stat_t get_int(nvObj_t *nv); // get uint32_t integer value -stat_t get_data(nvObj_t *nv); // get uint32_t integer value blind cast +stat_t get_int32(nvObj_t *nv); // get int32_t integer value stat_t get_flt(nvObj_t *nv); // get floating point value +stat_t get_data(nvObj_t *nv); // get uint32_t integer value blind cast + +stat_t set_noop(nvObj_t *nv); // set nothing and return OK +stat_t set_nul(nvObj_t *nv); // set nothing and return READ_ONLY error +stat_t set_ro(nvObj_t *nv); // set nothing, return read-only error +stat_t set_int32(nvObj_t *nv); // set int32_t integer value +stat_t set_flt(nvObj_t *nv); // set floating point value +stat_t set_data(nvObj_t *nv); // set uint32_t integer value blind cast stat_t set_grp(nvObj_t *nv); // set data for a group stat_t get_grp(nvObj_t *nv); // get data for a group @@ -356,10 +393,20 @@ nvObj_t *nv_add_conditional_message(const char *string); void nv_print_list(stat_t status, uint8_t text_flags, uint8_t json_flags); // application specific helpers and functions (config_app.c) -stat_t set_flu(nvObj_t *nv); // set floating point number with G20/G21 units conversion -stat_t set_flup(nvObj_t *nv); // set positive floating point number with G20/G21 units conversion -stat_t set_fltp(nvObj_t *nv); // set positive floating point number with no units conversion -void preprocess_float(nvObj_t *nv); // pre-process float values for units and illegal values + +void convert_incoming_float(nvObj_t *nv); // pre-process outgoing float values for units and illegal values +void convert_outgoing_float(nvObj_t *nv); // pre-process incoming float values for canonical units + +stat_t get_float(nvObj_t *nv, const float value); // boilerplate for retrieving raw floating point value +stat_t set_float(nvObj_t *nv, float &value); // boilerplate for setting a floating point value w/conversion +stat_t set_float_range(nvObj_t *nv, float &value, float low, float high); + +stat_t get_integer(nvObj_t *nv, const int32_t value); // boilerplate for retrieving 8 bit integer value +stat_t set_integer(nvObj_t *nv, uint8_t &value, uint8_t low, uint8_t high); +stat_t set_int32(nvObj_t *nv, int32_t &value, int32_t low, int32_t high); +stat_t set_uint32(nvObj_t *nv, uint32_t &value, int32_t low, int32_t high); + +stat_t get_string(nvObj_t *nv, const char *str); // diagnostics void nv_dump_nv(nvObj_t *nv); diff --git a/g2core/config_app.cpp b/g2core/config_app.cpp index 6205698c..881da747 100644 --- a/g2core/config_app.cpp +++ b/g2core/config_app.cpp @@ -2,8 +2,8 @@ * config_app.cpp - application-specific part of configuration data * This file is part of the g2core project * - * Copyright (c) 2013 - 2017 Alden S. Hart, Jr. - * Copyright (c) 2016 - 2017 Robert Giseburt + * Copyright (c) 2013 - 2019 Alden S. Hart, Jr. + * Copyright (c) 2016 - 2019 Robert Giseburt * * This file ("the software") is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License, version 2 as published by the @@ -30,7 +30,7 @@ #include "config.h" // #2 #include "controller.h" #include "canonical_machine.h" -#include "gcode_parser.h" +#include "gcode.h" #include "json_parser.h" #include "text_parser.h" #include "settings.h" @@ -96,1422 +96,1644 @@ static stat_t get_tick(nvObj_t *nv); // get system tick count * cannot exceed TOKEN_LEN. String functions working on the table assume these * rules are followed and do not check lengths or perform other validation. * - * - If the count of lines in cfgArray exceeds 255 (which it does) you need to - * ensure index_t is uint16_t in the config.h file (and not uint8_t). - * * - The precision value 'p' only affects JSON responses. You need to also set * the %f in the corresponding format string to set text mode display precision + * + * - Unit conversions are now conditional, and handled by convert_incoming_float() + * and convert_outgoing_float(). Apply conversion flags to all axes, not just linear, + * as rotary axes may be treated as linear if in radius mode, so the flag is needed. */ const cfgItem_t cfgArray[] = { - // group token flags p, print_func, get_func, set_func, target for get/set, default value - { "sys", "fb", _fipn,2, hw_print_fb, get_flt, set_ro, (float *)&cs.fw_build, G2CORE_FIRMWARE_BUILD }, // MUST BE FIRST! - { "sys", "fbs",_fn, 2, hw_print_fbs,hw_get_fbs,set_ro, (float *)&cs.null, 0 }, - { "sys", "fbc",_fn, 2, hw_print_fbc,hw_get_fbc,set_ro, (float *)&cs.null, 0 }, - { "sys", "fv", _fipn,2, hw_print_fv, get_flt, set_ro, (float *)&cs.fw_version, G2CORE_FIRMWARE_VERSION }, - { "sys", "hp", _fipn,0, hw_print_hp, get_flt, set_ro, (float *)&cs.hw_platform, G2CORE_HARDWARE_PLATFORM }, - { "sys", "hv", _fipn,0, hw_print_hv, get_flt, set_ro, (float *)&cs.hw_version, G2CORE_HARDWARE_VERSION }, - { "sys", "id", _fn, 0, hw_print_id, hw_get_id, set_ro, (float *)&cs.null, 0 }, // device ID (ASCII signature) + + // group token flags p, print_func, get_func, set_func, get/set target, default value + { "sys", "fb", _fn, 2, hw_print_fb, hw_get_fb, set_ro, nullptr, 0 }, // MUST BE FIRST for persistence checking! + { "sys", "fv", _fn, 2, hw_print_fv, hw_get_fv, set_ro, nullptr, 0 }, + { "sys", "fbs",_sn, 0, hw_print_fbs, hw_get_fbs, set_ro, nullptr, 0 }, + { "sys", "fbc",_sn, 0, hw_print_fbc, hw_get_fbc, set_ro, nullptr, 0 }, + { "sys", "hp", _sn, 0, hw_print_hp, hw_get_hp, set_ro, nullptr, 0 }, + { "sys", "hv", _sn, 0, hw_print_hv, hw_get_hv, set_ro, nullptr, 0 }, + { "sys", "id", _sn, 0, hw_print_id, hw_get_id, set_ro, nullptr, 0 }, // device ID (ASCII signature) // dynamic model attributes for reporting purposes (up front for speed) - { "", "stat",_f0, 0, cm_print_stat, cm_get_stat, set_ro, (float *)&cs.null, 0 }, // combined machine state - { "", "n", _fi, 0, cm_print_line, cm_get_mline,set_noop,(float *)&cs.null, 0 }, // Model line number - { "", "line",_fi, 0, cm_print_line, cm_get_line, set_ro, (float *)&cs.null, 0 }, // Active line number - model or runtime line number - { "", "vel", _f0, 2, cm_print_vel, cm_get_vel, set_ro, (float *)&cs.null, 0 }, // current velocity - { "", "feed",_f0, 2, cm_print_feed, cm_get_feed, set_ro, (float *)&cs.null, 0 }, // feed rate - { "", "macs",_f0, 0, cm_print_macs, cm_get_macs, set_ro, (float *)&cs.null, 0 }, // raw machine state - { "", "cycs",_f0, 0, cm_print_cycs, cm_get_cycs, set_ro, (float *)&cs.null, 0 }, // cycle state - { "", "mots",_f0, 0, cm_print_mots, cm_get_mots, set_ro, (float *)&cs.null, 0 }, // motion state - { "", "hold",_f0, 0, cm_print_hold, cm_get_hold, set_ro, (float *)&cs.null, 0 }, // feedhold state - { "", "unit",_f0, 0, cm_print_unit, cm_get_unit, set_ro, (float *)&cs.null, 0 }, // units mode - { "", "coor",_f0, 0, cm_print_coor, cm_get_coor, set_ro, (float *)&cs.null, 0 }, // coordinate system - { "", "momo",_f0, 0, cm_print_momo, cm_get_momo, set_ro, (float *)&cs.null, 0 }, // motion mode - { "", "plan",_f0, 0, cm_print_plan, cm_get_plan, set_ro, (float *)&cs.null, 0 }, // plane select - { "", "path",_f0, 0, cm_print_path, cm_get_path, set_ro, (float *)&cs.null, 0 }, // path control mode - { "", "dist",_f0, 0, cm_print_dist, cm_get_dist, set_ro, (float *)&cs.null, 0 }, // distance mode - { "", "admo",_f0, 0, cm_print_admo, cm_get_admo, set_ro, (float *)&cs.null, 0 }, // arc distance mode - { "", "frmo",_f0, 0, cm_print_frmo, cm_get_frmo, set_ro, (float *)&cs.null, 0 }, // feed rate mode - { "", "tool",_f0, 0, cm_print_tool, cm_get_toolv,set_ro, (float *)&cs.null, 0 }, // active tool - { "", "g92e",_f0, 0, cm_print_g92e, get_ui8, set_ro, (float *)&cm.gmx.origin_offset_enable, 0 }, // G92 enabled - + { "", "stat",_i0, 0, cm_print_stat, cm_get_stat, set_ro, nullptr, 0 }, // combined machine state + { "","stat2",_i0, 0, cm_print_stat, cm_get_stat2,set_ro, nullptr, 0 }, // combined machine state + { "", "n", _ii, 0, cm_print_line, cm_get_mline,set_noop,nullptr,0 }, // Model line number + { "", "line",_ii, 0, cm_print_line, cm_get_line, set_ro, nullptr, 0 }, // Active line number - model or runtime line number + { "", "vel", _f0, 2, cm_print_vel, cm_get_vel, set_ro, nullptr, 0 }, // current velocity + { "", "feed",_f0, 2, cm_print_feed, cm_get_feed, set_ro, nullptr, 0 }, // feed rate + { "", "macs",_i0, 0, cm_print_macs, cm_get_macs, set_ro, nullptr, 0 }, // raw machine state + { "", "cycs",_i0, 0, cm_print_cycs, cm_get_cycs, set_ro, nullptr, 0 }, // cycle state + { "", "mots",_i0, 0, cm_print_mots, cm_get_mots, set_ro, nullptr, 0 }, // motion state + { "", "hold",_i0, 0, cm_print_hold, cm_get_hold, set_ro, nullptr, 0 }, // feedhold state + { "", "unit",_i0, 0, cm_print_unit, cm_get_unit, set_ro, nullptr, 0 }, // units mode + { "", "coor",_i0, 0, cm_print_coor, cm_get_coor, set_ro, nullptr, 0 }, // coordinate system + { "", "momo",_i0, 0, cm_print_momo, cm_get_momo, set_ro, nullptr, 0 }, // motion mode + { "", "plan",_i0, 0, cm_print_plan, cm_get_plan, set_ro, nullptr, 0 }, // plane select + { "", "path",_i0, 0, cm_print_path, cm_get_path, set_ro, nullptr, 0 }, // path control mode + { "", "dist",_i0, 0, cm_print_dist, cm_get_dist, set_ro, nullptr, 0 }, // distance mode + { "", "admo",_i0, 0, cm_print_admo, cm_get_admo, set_ro, nullptr, 0 }, // arc distance mode + { "", "frmo",_i0, 0, cm_print_frmo, cm_get_frmo, set_ro, nullptr, 0 }, // feed rate mode + { "", "tool",_i0, 0, cm_print_tool, cm_get_toolv,set_ro, nullptr, 0 }, // active tool + { "", "g92e",_i0, 0, cm_print_g92e, cm_get_g92e, set_ro, nullptr, 0 }, // G92 enable state #ifdef TEMPORARY_HAS_LEDS - { "", "_leds",_f0, 0, tx_print_nul, _get_leds,_set_leds,(float *)&cs.null, 0 }, // TEMPORARY - change LEDs + { "", "_leds",_i0, 0, tx_print_nul, _get_leds,_set_leds, nullptr, 0 }, // TEMPORARY - change LEDs #endif - { "mpo","mpox",_f0, 3, cm_print_mpo, cm_get_mpo, set_ro, (float *)&cs.null, 0 }, // X machine position - { "mpo","mpoy",_f0, 3, cm_print_mpo, cm_get_mpo, set_ro, (float *)&cs.null, 0 }, // Y machine position - { "mpo","mpoz",_f0, 3, cm_print_mpo, cm_get_mpo, set_ro, (float *)&cs.null, 0 }, // Z machine position - { "mpo","mpoa",_f0, 3, cm_print_mpo, cm_get_mpo, set_ro, (float *)&cs.null, 0 }, // A machine position - { "mpo","mpob",_f0, 3, cm_print_mpo, cm_get_mpo, set_ro, (float *)&cs.null, 0 }, // B machine position - { "mpo","mpoc",_f0, 3, cm_print_mpo, cm_get_mpo, set_ro, (float *)&cs.null, 0 }, // C machine position + { "mpo","mpox",_f0, 5, cm_print_mpo, cm_get_mpo, set_ro, nullptr, 0 }, // X machine position + { "mpo","mpoy",_f0, 5, cm_print_mpo, cm_get_mpo, set_ro, nullptr, 0 }, // Y machine position + { "mpo","mpoz",_f0, 5, cm_print_mpo, cm_get_mpo, set_ro, nullptr, 0 }, // Z machine position + { "mpo","mpou",_f0, 5, cm_print_mpo, cm_get_mpo, set_ro, nullptr, 0 }, // U machine position + { "mpo","mpov",_f0, 5, cm_print_mpo, cm_get_mpo, set_ro, nullptr, 0 }, // V machine position + { "mpo","mpow",_f0, 5, cm_print_mpo, cm_get_mpo, set_ro, nullptr, 0 }, // W machine position + { "mpo","mpoa",_f0, 5, cm_print_mpo, cm_get_mpo, set_ro, nullptr, 0 }, // A machine position + { "mpo","mpob",_f0, 5, cm_print_mpo, cm_get_mpo, set_ro, nullptr, 0 }, // B machine position + { "mpo","mpoc",_f0, 5, cm_print_mpo, cm_get_mpo, set_ro, nullptr, 0 }, // C machine position - { "pos","posx",_f0, 3, cm_print_pos, cm_get_pos, set_ro, (float *)&cs.null, 0 }, // X work position - { "pos","posy",_f0, 3, cm_print_pos, cm_get_pos, set_ro, (float *)&cs.null, 0 }, // Y work position - { "pos","posz",_f0, 3, cm_print_pos, cm_get_pos, set_ro, (float *)&cs.null, 0 }, // Z work position - { "pos","posa",_f0, 3, cm_print_pos, cm_get_pos, set_ro, (float *)&cs.null, 0 }, // A work position - { "pos","posb",_f0, 3, cm_print_pos, cm_get_pos, set_ro, (float *)&cs.null, 0 }, // B work position - { "pos","posc",_f0, 3, cm_print_pos, cm_get_pos, set_ro, (float *)&cs.null, 0 }, // C work position + { "pos","posx",_f0, 5, cm_print_pos, cm_get_pos, set_ro, nullptr, 0 }, // X work position + { "pos","posy",_f0, 5, cm_print_pos, cm_get_pos, set_ro, nullptr, 0 }, // Y work position + { "pos","posz",_f0, 5, cm_print_pos, cm_get_pos, set_ro, nullptr, 0 }, // Z work position + { "pos","posu",_f0, 5, cm_print_pos, cm_get_pos, set_ro, nullptr, 0 }, // U work position + { "pos","posv",_f0, 5, cm_print_pos, cm_get_pos, set_ro, nullptr, 0 }, // V work position + { "pos","posw",_f0, 5, cm_print_pos, cm_get_pos, set_ro, nullptr, 0 }, // W work position + { "pos","posa",_f0, 5, cm_print_pos, cm_get_pos, set_ro, nullptr, 0 }, // A work position + { "pos","posb",_f0, 5, cm_print_pos, cm_get_pos, set_ro, nullptr, 0 }, // B work position + { "pos","posc",_f0, 5, cm_print_pos, cm_get_pos, set_ro, nullptr, 0 }, // C work position - { "ofs","ofsx",_f0, 3, cm_print_ofs, cm_get_ofs, set_ro, (float *)&cs.null, 0 }, // X work offset - { "ofs","ofsy",_f0, 3, cm_print_ofs, cm_get_ofs, set_ro, (float *)&cs.null, 0 }, // Y work offset - { "ofs","ofsz",_f0, 3, cm_print_ofs, cm_get_ofs, set_ro, (float *)&cs.null, 0 }, // Z work offset - { "ofs","ofsa",_f0, 3, cm_print_ofs, cm_get_ofs, set_ro, (float *)&cs.null, 0 }, // A work offset - { "ofs","ofsb",_f0, 3, cm_print_ofs, cm_get_ofs, set_ro, (float *)&cs.null, 0 }, // B work offset - { "ofs","ofsc",_f0, 3, cm_print_ofs, cm_get_ofs, set_ro, (float *)&cs.null, 0 }, // C work offset + { "ofs","ofsx",_f0, 5, cm_print_ofs, cm_get_ofs, set_ro, nullptr, 0 }, // X work offset + { "ofs","ofsy",_f0, 5, cm_print_ofs, cm_get_ofs, set_ro, nullptr, 0 }, // Y work offset + { "ofs","ofsz",_f0, 5, cm_print_ofs, cm_get_ofs, set_ro, nullptr, 0 }, // Z work offset + { "ofs","ofsu",_f0, 5, cm_print_ofs, cm_get_ofs, set_ro, nullptr, 0 }, // U work offset + { "ofs","ofsv",_f0, 5, cm_print_ofs, cm_get_ofs, set_ro, nullptr, 0 }, // V work offset + { "ofs","ofsw",_f0, 5, cm_print_ofs, cm_get_ofs, set_ro, nullptr, 0 }, // W work offset + { "ofs","ofsa",_f0, 5, cm_print_ofs, cm_get_ofs, set_ro, nullptr, 0 }, // A work offset + { "ofs","ofsb",_f0, 5, cm_print_ofs, cm_get_ofs, set_ro, nullptr, 0 }, // B work offset + { "ofs","ofsc",_f0, 5, cm_print_ofs, cm_get_ofs, set_ro, nullptr, 0 }, // C work offset - { "hom","home",_f0, 0, cm_print_home,cm_get_home,set_01,(float *)&cm.homing_state, 0 }, // homing state, invoke homing cycle - { "hom","homx",_f0, 0, cm_print_hom, get_ui8, set_01, (float *)&cm.homed[AXIS_X], false }, // X homed - Homing status group - { "hom","homy",_f0, 0, cm_print_hom, get_ui8, set_01, (float *)&cm.homed[AXIS_Y], false }, // Y homed - { "hom","homz",_f0, 0, cm_print_hom, get_ui8, set_01, (float *)&cm.homed[AXIS_Z], false }, // Z homed - { "hom","homa",_f0, 0, cm_print_hom, get_ui8, set_01, (float *)&cm.homed[AXIS_A], false }, // A homed - { "hom","homb",_f0, 0, cm_print_hom, get_ui8, set_01, (float *)&cm.homed[AXIS_B], false }, // B homed - { "hom","homc",_f0, 0, cm_print_hom, get_ui8, set_01, (float *)&cm.homed[AXIS_C], false }, // C homed + { "hom","home",_i0, 0, cm_print_home,cm_get_home,cm_set_home,nullptr,0 }, // homing state, invoke homing cycle + { "hom","homx",_i0, 0, cm_print_hom, cm_get_hom, set_ro, nullptr, 0 }, // X homed - Homing status group + { "hom","homy",_i0, 0, cm_print_hom, cm_get_hom, set_ro, nullptr, 0 }, // Y homed + { "hom","homz",_i0, 0, cm_print_hom, cm_get_hom, set_ro, nullptr, 0 }, // Z homed + { "hom","homu",_i0, 0, cm_print_hom, cm_get_hom, set_ro, nullptr, 0 }, // U homed + { "hom","homv",_i0, 0, cm_print_hom, cm_get_hom, set_ro, nullptr, 0 }, // V homed + { "hom","homw",_i0, 0, cm_print_hom, cm_get_hom, set_ro, nullptr, 0 }, // W homed + { "hom","homa",_i0, 0, cm_print_hom, cm_get_hom, set_ro, nullptr, 0 }, // A homed + { "hom","homb",_i0, 0, cm_print_hom, cm_get_hom, set_ro, nullptr, 0 }, // B homed + { "hom","homc",_i0, 0, cm_print_hom, cm_get_hom, set_ro, nullptr, 0 }, // C homed - { "prb","prbe",_f0, 0, tx_print_nul, get_ui8, set_ro, (float *)&cm.probe_state[0], 0 }, // probing state - { "prb","prbx",_f0, 3, tx_print_nul, get_flt, set_ro, (float *)&cm.probe_results[0][AXIS_X], 0 }, - { "prb","prby",_f0, 3, tx_print_nul, get_flt, set_ro, (float *)&cm.probe_results[0][AXIS_Y], 0 }, - { "prb","prbz",_f0, 3, tx_print_nul, get_flt, set_ro, (float *)&cm.probe_results[0][AXIS_Z], 0 }, - { "prb","prba",_f0, 3, tx_print_nul, get_flt, set_ro, (float *)&cm.probe_results[0][AXIS_A], 0 }, - { "prb","prbb",_f0, 3, tx_print_nul, get_flt, set_ro, (float *)&cm.probe_results[0][AXIS_B], 0 }, - { "prb","prbc",_f0, 3, tx_print_nul, get_flt, set_ro, (float *)&cm.probe_results[0][AXIS_C], 0 }, - { "prb","prbs",_f0, 0, tx_print_nul, get_nul, cm_set_probe, (float *)&cs.null, 0 }, // store probe - { "prb","prbr",_f0, 0, tx_print_nul, cm_get_prbr, cm_set_prbr, nullptr, 0 }, // enable probe report. Init in cm_init - { "prb","prbin",_f0, 0, tx_print_nul, get_ui8, cm_set_hi, (float *)&cm.probe_input, PROBING_INPUT }, // probing state + { "prb","prbe",_i0, 0, tx_print_nul, cm_get_prob,set_ro, nullptr, 0 }, // probing state + { "prb","prbx",_f0, 5, tx_print_nul, cm_get_prb, set_ro, nullptr, 0 }, // X probe results + { "prb","prby",_f0, 5, tx_print_nul, cm_get_prb, set_ro, nullptr, 0 }, // Y probe results + { "prb","prbz",_f0, 5, tx_print_nul, cm_get_prb, set_ro, nullptr, 0 }, // Z probe results + { "prb","prbu",_f0, 5, tx_print_nul, cm_get_prb, set_ro, nullptr, 0 }, // U probe results + { "prb","prbv",_f0, 5, tx_print_nul, cm_get_prb, set_ro, nullptr, 0 }, // V probe results + { "prb","prbw",_f0, 5, tx_print_nul, cm_get_prb, set_ro, nullptr, 0 }, // W probe results + { "prb","prba",_f0, 5, tx_print_nul, cm_get_prb, set_ro, nullptr, 0 }, // A probe results + { "prb","prbb",_f0, 5, tx_print_nul, cm_get_prb, set_ro, nullptr, 0 }, // B probe results + { "prb","prbc",_f0, 5, tx_print_nul, cm_get_prb, set_ro, nullptr, 0 }, // C probe results - { "jog","jogx",_f0, 0, tx_print_nul, get_nul, cm_run_jogx, (float *)&cm.jogging_dest, 0}, - { "jog","jogy",_f0, 0, tx_print_nul, get_nul, cm_run_jogy, (float *)&cm.jogging_dest, 0}, - { "jog","jogz",_f0, 0, tx_print_nul, get_nul, cm_run_jogz, (float *)&cm.jogging_dest, 0}, - { "jog","joga",_f0, 0, tx_print_nul, get_nul, cm_run_joga, (float *)&cm.jogging_dest, 0}, -// { "jog","jogb",_f0, 0, tx_print_nul, get_nul, cm_run_jogb, (float *)&cm.jogging_dest, 0}, -// { "jog","jogc",_f0, 0, tx_print_nul, get_nul, cm_run_jogc, (float *)&cm.jogging_dest, 0}, + { "jog","jogx",_f0, 0, tx_print_nul, get_nul, cm_run_jog, nullptr, 0}, // jog in X axis + { "jog","jogy",_f0, 0, tx_print_nul, get_nul, cm_run_jog, nullptr, 0}, // jog in Y axis + { "jog","jogz",_f0, 0, tx_print_nul, get_nul, cm_run_jog, nullptr, 0}, // jog in Z axis + { "jog","jogu",_f0, 0, tx_print_nul, get_nul, cm_run_jog, nullptr, 0}, // jog in U axis + { "jog","jogv",_f0, 0, tx_print_nul, get_nul, cm_run_jog, nullptr, 0}, // jog in V axis + { "jog","jogw",_f0, 0, tx_print_nul, get_nul, cm_run_jog, nullptr, 0}, // jog in W axis + { "jog","joga",_f0, 0, tx_print_nul, get_nul, cm_run_jog, nullptr, 0}, // jog in A axis + { "jog","jogb",_f0, 0, tx_print_nul, get_nul, cm_run_jog, nullptr, 0}, // jog in B axis + { "jog","jogc",_f0, 0, tx_print_nul, get_nul, cm_run_jog, nullptr, 0}, // jog in C axis - { "pwr","pwr1",_f0, 3, st_print_pwr, st_get_pwr, set_ro, (float *)&cs.null, 0}, // motor power readouts - { "pwr","pwr2",_f0, 3, st_print_pwr, st_get_pwr, set_ro, (float *)&cs.null, 0}, + { "pwr","pwr1",_f0, 3, st_print_pwr, st_get_pwr, set_ro, nullptr, 0}, // motor power readouts + { "pwr","pwr2",_f0, 3, st_print_pwr, st_get_pwr, set_ro, nullptr, 0}, #if (MOTORS > 2) - { "pwr","pwr3",_f0, 3, st_print_pwr, st_get_pwr, set_ro, (float *)&cs.null, 0}, + { "pwr","pwr3",_f0, 3, st_print_pwr, st_get_pwr, set_ro, nullptr, 0}, #endif #if (MOTORS > 3) - { "pwr","pwr4",_f0, 3, st_print_pwr, st_get_pwr, set_ro, (float *)&cs.null, 0}, + { "pwr","pwr4",_f0, 3, st_print_pwr, st_get_pwr, set_ro, nullptr, 0}, #endif #if (MOTORS > 4) - { "pwr","pwr5",_f0, 3, st_print_pwr, st_get_pwr, set_ro, (float *)&cs.null, 0}, + { "pwr","pwr5",_f0, 3, st_print_pwr, st_get_pwr, set_ro, nullptr, 0}, #endif #if (MOTORS > 5) - { "pwr","pwr6",_f0, 3, st_print_pwr, st_get_pwr, set_ro, (float *)&cs.null, 0}, + { "pwr","pwr6",_f0, 3, st_print_pwr, st_get_pwr, set_ro, nullptr, 0}, #endif // Motor parameters // generated with ${PROJECT_ROOT}/Resources/generate_motors_cfgArray.js + #if (MOTORS >= 1) - { "1","1ma", _fip, 0, st_print_ma, get_ui8, st_set_ma, (float *)&st_cfg.mot[MOTOR_1].motor_map, M1_MOTOR_MAP }, - { "1","1sa", _fip, 3, st_print_sa, get_flt, st_set_sa, (float *)&st_cfg.mot[MOTOR_1].step_angle, M1_STEP_ANGLE }, - { "1","1tr", _fipc, 4, st_print_tr, get_flt, st_set_tr, (float *)&st_cfg.mot[MOTOR_1].travel_rev, M1_TRAVEL_PER_REV }, - { "1","1mi", _fip, 0, st_print_mi, get_int, st_set_mi, (float *)&st_cfg.mot[MOTOR_1].microsteps, M1_MICROSTEPS }, - { "1","1su", _fipi, 5, st_print_su, st_get_su,st_set_su, (float *)&st_cfg.mot[MOTOR_1].steps_per_unit, M1_STEPS_PER_UNIT }, - { "1","1po", _fip, 0, st_print_po, get_ui8, set_01, (float *)&st_cfg.mot[MOTOR_1].polarity, M1_POLARITY }, - { "1","1pm", _fip, 0, st_print_pm, st_get_pm,st_set_pm, (float *)&cs.null, M1_POWER_MODE }, - { "1","1pl", _fip, 3, st_print_pl, get_flt, st_set_pl, (float *)&st_cfg.mot[MOTOR_1].power_level, M1_POWER_LEVEL }, - // { "1","1pi", _fip, 3, st_print_pi, get_flt, st_set_pi, (float *)&st_cfg.mot[MOTOR_1].power_idle, M1_POWER_IDLE }, - // { "1","1mt", _fip, 2, st_print_mt, get_flt, st_set_mt, (float *)&st_cfg.mot[MOTOR_1].motor_timeout, M1_MOTOR_TIMEOUT }, + { "1","1ma", _fip, 0, st_print_ma, st_get_ma, st_set_ma, &st_cfg.mot[MOTOR_1].motor_map, M1_MOTOR_MAP }, + { "1","1sa", _fip, 3, st_print_sa, st_get_sa, st_set_sa, &st_cfg.mot[MOTOR_1].step_angle, M1_STEP_ANGLE }, + { "1","1tr", _fipc, 4, st_print_tr, st_get_tr, st_set_tr, &st_cfg.mot[MOTOR_1].travel_rev, M1_TRAVEL_PER_REV }, + { "1","1su", _fipi, 5, st_print_su, st_get_su, st_set_su, &st_cfg.mot[MOTOR_1].steps_per_unit, M1_STEPS_PER_UNIT }, + { "1","1mi", _fip, 0, st_print_mi, st_get_mi, st_set_mi, &st_cfg.mot[MOTOR_1].microsteps, M1_MICROSTEPS }, + { "1","1po", _fip, 0, st_print_po, st_get_po, st_set_po, &st_cfg.mot[MOTOR_1].polarity, M1_POLARITY }, + { "1","1pm", _fip, 0, st_print_pm, st_get_pm, st_set_pm, &cs.null, M1_POWER_MODE }, + { "1","1pl", _fip, 3, st_print_pl, st_get_pl, st_set_pl, &st_cfg.mot[MOTOR_1].power_level, M1_POWER_LEVEL }, + { "1","1ep", _iip, 0, st_print_ep, st_get_ep, st_set_ep, nullptr, M1_ENABLE_POLARITY }, + { "1","1sp", _iip, 0, st_print_sp, st_get_sp, st_set_sp, nullptr, M1_STEP_POLARITY }, +// { "1","1pi", _fip, 3, st_print_pi, st_get_pi, st_set_pi, &st_cfg.mot[MOTOR_1].power_idle, M1_POWER_IDLE }, +// { "1","1mt", _fip, 2, st_print_mt, st_get_mt, st_set_mt, &st_cfg.mot[MOTOR_1].motor_timeout, M1_MOTOR_TIMEOUT }, #ifdef MOTOR_1_IS_TRINAMIC - { "1","1ts", _f0, 0, tx_print_nul, motor_1.get_ts_fn, set_ro, (float *)&motor_1, 0 }, - { "1","1pth", _fip, 0, tx_print_nul, motor_1.get_pth_fn, motor_1.set_pth_fn, (float *)&motor_1, M1_TMC2130_TPWMTHRS }, - { "1","1cth", _fip, 0, tx_print_nul, motor_1.get_cth_fn, motor_1.set_cth_fn, (float *)&motor_1, M1_TMC2130_TCOOLTHRS }, - { "1","1hth", _fip, 0, tx_print_nul, motor_1.get_hth_fn, motor_1.set_hth_fn, (float *)&motor_1, M1_TMC2130_THIGH }, - { "1","1sgt", _fip, 0, tx_print_nul, motor_1.get_sgt_fn, motor_1.set_sgt_fn, (float *)&motor_1, M1_TMC2130_SGT }, - { "1","1sgr", _f0, 0, tx_print_nul, motor_1.get_sgr_fn, set_ro, (float *)&motor_1, 0 }, - { "1","1csa", _f0, 0, tx_print_nul, motor_1.get_csa_fn, set_ro, (float *)&motor_1, 0 }, - { "1","1sgs", _f0, 0, tx_print_nul, motor_1.get_sgs_fn, set_ro, (float *)&motor_1, 0 }, - { "1","1tbl", _fip, 0, tx_print_nul, motor_1.get_tbl_fn, motor_1.set_tbl_fn, (float *)&motor_1, M1_TMC2130_TBL }, - { "1","1pgrd",_fip, 0, tx_print_nul, motor_1.get_pgrd_fn,motor_1.set_pgrd_fn, (float *)&motor_1, M1_TMC2130_PWM_GRAD }, - { "1","1pamp",_fip, 0, tx_print_nul, motor_1.get_pamp_fn,motor_1.set_pamp_fn, (float *)&motor_1, M1_TMC2130_PWM_AMPL }, - { "1","1hend",_fip, 0, tx_print_nul, motor_1.get_hend_fn,motor_1.set_hend_fn, (float *)&motor_1, M1_TMC2130_HEND }, - { "1","1hsrt",_fip, 0, tx_print_nul, motor_1.get_hsrt_fn,motor_1.set_hsrt_fn, (float *)&motor_1, M1_TMC2130_HSTRT }, - { "1","1smin",_fip, 0, tx_print_nul, motor_1.get_smin_fn,motor_1.set_smin_fn, (float *)&motor_1, M1_TMC2130_SMIN }, - { "1","1smax",_fip, 0, tx_print_nul, motor_1.get_smax_fn,motor_1.set_smax_fn, (float *)&motor_1, M1_TMC2130_SMAX }, - { "1","1sup", _fip, 0, tx_print_nul, motor_1.get_sup_fn, motor_1.set_sup_fn, (float *)&motor_1, M1_TMC2130_SUP }, - { "1","1sdn", _fip, 0, tx_print_nul, motor_1.get_sdn_fn, motor_1.set_sdn_fn, (float *)&motor_1, M1_TMC2130_SDN }, + { "1","1ts", _f0, 0, tx_print_nul, motor_1.get_ts_fn, set_ro, &motor_1, 0 }, + { "1","1pth", _fip, 0, tx_print_nul, motor_1.get_pth_fn, motor_1.set_pth_fn, &motor_1, M1_TMC2130_TPWMTHRS }, + { "1","1cth", _fip, 0, tx_print_nul, motor_1.get_cth_fn, motor_1.set_cth_fn, &motor_1, M1_TMC2130_TCOOLTHRS }, + { "1","1hth", _fip, 0, tx_print_nul, motor_1.get_hth_fn, motor_1.set_hth_fn, &motor_1, M1_TMC2130_THIGH }, + { "1","1sgt", _fip, 0, tx_print_nul, motor_1.get_sgt_fn, motor_1.set_sgt_fn, &motor_1, M1_TMC2130_SGT }, + { "1","1sgr", _f0, 0, tx_print_nul, motor_1.get_sgr_fn, set_ro, &motor_1, 0 }, + { "1","1csa", _f0, 0, tx_print_nul, motor_1.get_csa_fn, set_ro, &motor_1, 0 }, + { "1","1sgs", _f0, 0, tx_print_nul, motor_1.get_sgs_fn, set_ro, &motor_1, 0 }, + { "1","1tbl", _fip, 0, tx_print_nul, motor_1.get_tbl_fn, motor_1.set_tbl_fn, &motor_1, M1_TMC2130_TBL }, + { "1","1pgrd",_fip, 0, tx_print_nul, motor_1.get_pgrd_fn,motor_1.set_pgrd_fn, &motor_1, M1_TMC2130_PWM_GRAD }, + { "1","1pamp",_fip, 0, tx_print_nul, motor_1.get_pamp_fn,motor_1.set_pamp_fn, &motor_1, M1_TMC2130_PWM_AMPL }, + { "1","1hend",_fip, 0, tx_print_nul, motor_1.get_hend_fn,motor_1.set_hend_fn, &motor_1, M1_TMC2130_HEND }, + { "1","1hsrt",_fip, 0, tx_print_nul, motor_1.get_hsrt_fn,motor_1.set_hsrt_fn, &motor_1, M1_TMC2130_HSTRT }, + { "1","1smin",_fip, 0, tx_print_nul, motor_1.get_smin_fn,motor_1.set_smin_fn, &motor_1, M1_TMC2130_SMIN }, + { "1","1smax",_fip, 0, tx_print_nul, motor_1.get_smax_fn,motor_1.set_smax_fn, &motor_1, M1_TMC2130_SMAX }, + { "1","1sup", _fip, 0, tx_print_nul, motor_1.get_sup_fn, motor_1.set_sup_fn, &motor_1, M1_TMC2130_SUP }, + { "1","1sdn", _fip, 0, tx_print_nul, motor_1.get_sdn_fn, motor_1.set_sdn_fn, &motor_1, M1_TMC2130_SDN }, #endif #endif #if (MOTORS >= 2) - { "2","2ma", _fip, 0, st_print_ma, get_ui8, st_set_ma, (float *)&st_cfg.mot[MOTOR_2].motor_map, M2_MOTOR_MAP }, - { "2","2sa", _fip, 3, st_print_sa, get_flt, st_set_sa, (float *)&st_cfg.mot[MOTOR_2].step_angle, M2_STEP_ANGLE }, - { "2","2tr", _fipc, 4, st_print_tr, get_flt, st_set_tr, (float *)&st_cfg.mot[MOTOR_2].travel_rev, M2_TRAVEL_PER_REV }, - { "2","2mi", _fip, 0, st_print_mi, get_int, st_set_mi, (float *)&st_cfg.mot[MOTOR_2].microsteps, M2_MICROSTEPS }, - { "2","2su", _fipi, 5, st_print_su, st_get_su,st_set_su, (float *)&st_cfg.mot[MOTOR_2].steps_per_unit, M2_STEPS_PER_UNIT }, - { "2","2po", _fip, 0, st_print_po, get_ui8, set_01, (float *)&st_cfg.mot[MOTOR_2].polarity, M2_POLARITY }, - { "2","2pm", _fip, 0, st_print_pm, st_get_pm,st_set_pm, (float *)&cs.null, M2_POWER_MODE }, - { "2","2pl", _fip, 3, st_print_pl, get_flt, st_set_pl, (float *)&st_cfg.mot[MOTOR_2].power_level, M2_POWER_LEVEL }, - // { "2","2pi", _fip, 3, st_print_pi, get_flt, st_set_pi, (float *)&st_cfg.mot[MOTOR_2].power_idle, M2_POWER_IDLE }, - // { "2","2mt", _fip, 2, st_print_mt, get_flt, st_set_mt, (float *)&st_cfg.mot[MOTOR_2].motor_timeout, M2_MOTOR_TIMEOUT }, + { "2","2ma", _fip, 0, st_print_ma, st_get_ma, st_set_ma, &st_cfg.mot[MOTOR_2].motor_map, M2_MOTOR_MAP }, + { "2","2sa", _fip, 3, st_print_sa, st_get_sa, st_set_sa, &st_cfg.mot[MOTOR_2].step_angle, M2_STEP_ANGLE }, + { "2","2tr", _fipc, 4, st_print_tr, st_get_tr, st_set_tr, &st_cfg.mot[MOTOR_2].travel_rev, M2_TRAVEL_PER_REV }, + { "2","2su", _fipi, 5, st_print_su, st_get_su, st_set_su, &st_cfg.mot[MOTOR_2].steps_per_unit, M2_STEPS_PER_UNIT }, + { "2","2mi", _fip, 0, st_print_mi, st_get_mi, st_set_mi, &st_cfg.mot[MOTOR_2].microsteps, M2_MICROSTEPS }, + { "2","2po", _fip, 0, st_print_po, st_get_po, st_set_po, &st_cfg.mot[MOTOR_2].polarity, M2_POLARITY }, + { "2","2pm", _fip, 0, st_print_pm, st_get_pm, st_set_pm, &cs.null, M2_POWER_MODE }, + { "2","2pl", _fip, 3, st_print_pl, st_get_pl, st_set_pl, &st_cfg.mot[MOTOR_2].power_level, M2_POWER_LEVEL }, + { "2","2ep", _iip, 0, st_print_ep, st_get_ep, st_set_ep, nullptr, M2_ENABLE_POLARITY }, + { "2","2sp", _iip, 0, st_print_sp, st_get_sp, st_set_sp, nullptr, M2_STEP_POLARITY }, +// { "2","2pi", _fip, 3, st_print_pi, st_get_pi, st_set_pi, &st_cfg.mot[MOTOR_2].power_idle, M2_POWER_IDLE }, +// { "2","2mt", _fip, 2, st_print_mt, st_get_mt, st_set_mt, &st_cfg.mot[MOTOR_2].motor_timeout, M2_MOTOR_TIMEOUT }, #ifdef MOTOR_2_IS_TRINAMIC - { "2","2ts", _f0, 0, tx_print_nul, motor_2.get_ts_fn, set_ro, (float *)&motor_2, 0 }, - { "2","2pth", _fip, 0, tx_print_nul, motor_2.get_pth_fn, motor_2.set_pth_fn, (float *)&motor_2, M2_TMC2130_TPWMTHRS }, - { "2","2cth", _fip, 0, tx_print_nul, motor_2.get_cth_fn, motor_2.set_cth_fn, (float *)&motor_2, M2_TMC2130_TCOOLTHRS }, - { "2","2hth", _fip, 0, tx_print_nul, motor_2.get_hth_fn, motor_2.set_hth_fn, (float *)&motor_2, M2_TMC2130_THIGH }, - { "2","2sgt", _fip, 0, tx_print_nul, motor_2.get_sgt_fn, motor_2.set_sgt_fn, (float *)&motor_2, M2_TMC2130_SGT }, - { "2","2sgr", _f0, 0, tx_print_nul, motor_2.get_sgr_fn, set_ro, (float *)&motor_2, 0 }, - { "2","2csa", _f0, 0, tx_print_nul, motor_2.get_csa_fn, set_ro, (float *)&motor_2, 0 }, - { "2","2sgs", _f0, 0, tx_print_nul, motor_2.get_sgs_fn, set_ro, (float *)&motor_2, 0 }, - { "2","2tbl", _fip, 0, tx_print_nul, motor_2.get_tbl_fn, motor_2.set_tbl_fn, (float *)&motor_2, M2_TMC2130_TBL }, - { "2","2pgrd",_fip, 0, tx_print_nul, motor_2.get_pgrd_fn,motor_2.set_pgrd_fn, (float *)&motor_2, M2_TMC2130_PWM_GRAD }, - { "2","2pamp",_fip, 0, tx_print_nul, motor_2.get_pamp_fn,motor_2.set_pamp_fn, (float *)&motor_2, M2_TMC2130_PWM_AMPL }, - { "2","2hend",_fip, 0, tx_print_nul, motor_2.get_hend_fn,motor_2.set_hend_fn, (float *)&motor_2, M2_TMC2130_HEND }, - { "2","2hsrt",_fip, 0, tx_print_nul, motor_2.get_hsrt_fn,motor_2.set_hsrt_fn, (float *)&motor_2, M2_TMC2130_HSTRT }, - { "2","2smin",_fip, 0, tx_print_nul, motor_2.get_smin_fn,motor_2.set_smin_fn, (float *)&motor_2, M2_TMC2130_SMIN }, - { "2","2smax",_fip, 0, tx_print_nul, motor_2.get_smax_fn,motor_2.set_smax_fn, (float *)&motor_2, M2_TMC2130_SMAX }, - { "2","2sup", _fip, 0, tx_print_nul, motor_2.get_sup_fn, motor_2.set_sup_fn, (float *)&motor_2, M2_TMC2130_SUP }, - { "2","2sdn", _fip, 0, tx_print_nul, motor_2.get_sdn_fn, motor_2.set_sdn_fn, (float *)&motor_2, M2_TMC2130_SDN }, + { "2","2ts", _f0, 0, tx_print_nul, motor_2.get_ts_fn, set_ro, &motor_2, 0 }, + { "2","2pth", _fip, 0, tx_print_nul, motor_2.get_pth_fn, motor_2.set_pth_fn, &motor_2, M2_TMC2130_TPWMTHRS }, + { "2","2cth", _fip, 0, tx_print_nul, motor_2.get_cth_fn, motor_2.set_cth_fn, &motor_2, M2_TMC2130_TCOOLTHRS }, + { "2","2hth", _fip, 0, tx_print_nul, motor_2.get_hth_fn, motor_2.set_hth_fn, &motor_2, M2_TMC2130_THIGH }, + { "2","2sgt", _fip, 0, tx_print_nul, motor_2.get_sgt_fn, motor_2.set_sgt_fn, &motor_2, M2_TMC2130_SGT }, + { "2","2sgr", _f0, 0, tx_print_nul, motor_2.get_sgr_fn, set_ro, &motor_2, 0 }, + { "2","2csa", _f0, 0, tx_print_nul, motor_2.get_csa_fn, set_ro, &motor_2, 0 }, + { "2","2sgs", _f0, 0, tx_print_nul, motor_2.get_sgs_fn, set_ro, &motor_2, 0 }, + { "2","2tbl", _fip, 0, tx_print_nul, motor_2.get_tbl_fn, motor_2.set_tbl_fn, &motor_2, M2_TMC2130_TBL }, + { "2","2pgrd",_fip, 0, tx_print_nul, motor_2.get_pgrd_fn,motor_2.set_pgrd_fn, &motor_2, M2_TMC2130_PWM_GRAD }, + { "2","2pamp",_fip, 0, tx_print_nul, motor_2.get_pamp_fn,motor_2.set_pamp_fn, &motor_2, M2_TMC2130_PWM_AMPL }, + { "2","2hend",_fip, 0, tx_print_nul, motor_2.get_hend_fn,motor_2.set_hend_fn, &motor_2, M2_TMC2130_HEND }, + { "2","2hsrt",_fip, 0, tx_print_nul, motor_2.get_hsrt_fn,motor_2.set_hsrt_fn, &motor_2, M2_TMC2130_HSTRT }, + { "2","2smin",_fip, 0, tx_print_nul, motor_2.get_smin_fn,motor_2.set_smin_fn, &motor_2, M2_TMC2130_SMIN }, + { "2","2smax",_fip, 0, tx_print_nul, motor_2.get_smax_fn,motor_2.set_smax_fn, &motor_2, M2_TMC2130_SMAX }, + { "2","2sup", _fip, 0, tx_print_nul, motor_2.get_sup_fn, motor_2.set_sup_fn, &motor_2, M2_TMC2130_SUP }, + { "2","2sdn", _fip, 0, tx_print_nul, motor_2.get_sdn_fn, motor_2.set_sdn_fn, &motor_2, M2_TMC2130_SDN }, #endif #endif #if (MOTORS >= 3) - { "3","3ma", _fip, 0, st_print_ma, get_ui8, st_set_ma, (float *)&st_cfg.mot[MOTOR_3].motor_map, M3_MOTOR_MAP }, - { "3","3sa", _fip, 3, st_print_sa, get_flt, st_set_sa, (float *)&st_cfg.mot[MOTOR_3].step_angle, M3_STEP_ANGLE }, - { "3","3tr", _fipc, 4, st_print_tr, get_flt, st_set_tr, (float *)&st_cfg.mot[MOTOR_3].travel_rev, M3_TRAVEL_PER_REV }, - { "3","3mi", _fip, 0, st_print_mi, get_int, st_set_mi, (float *)&st_cfg.mot[MOTOR_3].microsteps, M3_MICROSTEPS }, - { "3","3su", _fipi, 5, st_print_su, st_get_su,st_set_su, (float *)&st_cfg.mot[MOTOR_3].steps_per_unit, M3_STEPS_PER_UNIT }, - { "3","3po", _fip, 0, st_print_po, get_ui8, set_01, (float *)&st_cfg.mot[MOTOR_3].polarity, M3_POLARITY }, - { "3","3pm", _fip, 0, st_print_pm, st_get_pm,st_set_pm, (float *)&cs.null, M3_POWER_MODE }, - { "3","3pl", _fip, 3, st_print_pl, get_flt, st_set_pl, (float *)&st_cfg.mot[MOTOR_3].power_level, M3_POWER_LEVEL }, - // { "3","3pi", _fip, 3, st_print_pi, get_flt, st_set_pi, (float *)&st_cfg.mot[MOTOR_3].power_idle, M3_POWER_IDLE }, - // { "3","3mt", _fip, 2, st_print_mt, get_flt, st_set_mt, (float *)&st_cfg.mot[MOTOR_3].motor_timeout, M3_MOTOR_TIMEOUT }, + { "3","3ma", _fip, 0, st_print_ma, st_get_ma, st_set_ma, &st_cfg.mot[MOTOR_3].motor_map, M3_MOTOR_MAP }, + { "3","3sa", _fip, 3, st_print_sa, st_get_sa, st_set_sa, &st_cfg.mot[MOTOR_3].step_angle, M3_STEP_ANGLE }, + { "3","3tr", _fipc, 4, st_print_tr, st_get_tr, st_set_tr, &st_cfg.mot[MOTOR_3].travel_rev, M3_TRAVEL_PER_REV }, + { "3","3su", _fipi, 5, st_print_su, st_get_su, st_set_su, &st_cfg.mot[MOTOR_3].steps_per_unit, M3_STEPS_PER_UNIT }, + { "3","3mi", _fip, 0, st_print_mi, st_get_mi, st_set_mi, &st_cfg.mot[MOTOR_3].microsteps, M3_MICROSTEPS }, + { "3","3po", _fip, 0, st_print_po, st_get_po, st_set_po, &st_cfg.mot[MOTOR_3].polarity, M3_POLARITY }, + { "3","3pm", _fip, 0, st_print_pm, st_get_pm, st_set_pm, &cs.null, M3_POWER_MODE }, + { "3","3pl", _fip, 3, st_print_pl, st_get_pl, st_set_pl, &st_cfg.mot[MOTOR_3].power_level, M3_POWER_LEVEL }, + { "3","3ep", _iip, 0, st_print_ep, st_get_ep, st_set_ep, nullptr, M3_ENABLE_POLARITY }, + { "3","3sp", _iip, 0, st_print_sp, st_get_sp, st_set_sp, nullptr, M3_STEP_POLARITY }, +// { "3","3pi", _fip, 3, st_print_pi, st_get_pi, st_set_pi, &st_cfg.mot[MOTOR_3].power_idle, M3_POWER_IDLE }, +// { "3","3mt", _fip, 2, st_print_mt, st_get_mt, st_set_mt, &st_cfg.mot[MOTOR_3].motor_timeout, M3_MOTOR_TIMEOUT }, #ifdef MOTOR_3_IS_TRINAMIC - { "3","3ts", _f0, 0, tx_print_nul, motor_3.get_ts_fn, set_ro, (float *)&motor_3, 0 }, - { "3","3pth", _fip, 0, tx_print_nul, motor_3.get_pth_fn, motor_3.set_pth_fn, (float *)&motor_3, M3_TMC2130_TPWMTHRS }, - { "3","3cth", _fip, 0, tx_print_nul, motor_3.get_cth_fn, motor_3.set_cth_fn, (float *)&motor_3, M3_TMC2130_TCOOLTHRS }, - { "3","3hth", _fip, 0, tx_print_nul, motor_3.get_hth_fn, motor_3.set_hth_fn, (float *)&motor_3, M3_TMC2130_THIGH }, - { "3","3sgt", _fip, 0, tx_print_nul, motor_3.get_sgt_fn, motor_3.set_sgt_fn, (float *)&motor_3, M3_TMC2130_SGT }, - { "3","3sgr", _f0, 0, tx_print_nul, motor_3.get_sgr_fn, set_ro, (float *)&motor_3, 0 }, - { "3","3csa", _f0, 0, tx_print_nul, motor_3.get_csa_fn, set_ro, (float *)&motor_3, 0 }, - { "3","3sgs", _f0, 0, tx_print_nul, motor_3.get_sgs_fn, set_ro, (float *)&motor_3, 0 }, - { "3","3tbl", _fip, 0, tx_print_nul, motor_3.get_tbl_fn, motor_3.set_tbl_fn, (float *)&motor_3, M3_TMC2130_TBL }, - { "3","3pgrd",_fip, 0, tx_print_nul, motor_3.get_pgrd_fn,motor_3.set_pgrd_fn, (float *)&motor_3, M3_TMC2130_PWM_GRAD }, - { "3","3pamp",_fip, 0, tx_print_nul, motor_3.get_pamp_fn,motor_3.set_pamp_fn, (float *)&motor_3, M3_TMC2130_PWM_AMPL }, - { "3","3hend",_fip, 0, tx_print_nul, motor_3.get_hend_fn,motor_3.set_hend_fn, (float *)&motor_3, M3_TMC2130_HEND }, - { "3","3hsrt",_fip, 0, tx_print_nul, motor_3.get_hsrt_fn,motor_3.set_hsrt_fn, (float *)&motor_3, M3_TMC2130_HSTRT }, - { "3","3smin",_fip, 0, tx_print_nul, motor_3.get_smin_fn,motor_3.set_smin_fn, (float *)&motor_3, M3_TMC2130_SMIN }, - { "3","3smax",_fip, 0, tx_print_nul, motor_3.get_smax_fn,motor_3.set_smax_fn, (float *)&motor_3, M3_TMC2130_SMAX }, - { "3","3sup", _fip, 0, tx_print_nul, motor_3.get_sup_fn, motor_3.set_sup_fn, (float *)&motor_3, M3_TMC2130_SUP }, - { "3","3sdn", _fip, 0, tx_print_nul, motor_3.get_sdn_fn, motor_3.set_sdn_fn, (float *)&motor_3, M3_TMC2130_SDN }, + { "3","3ts", _f0, 0, tx_print_nul, motor_3.get_ts_fn, set_ro, &motor_3, 0 }, + { "3","3pth", _fip, 0, tx_print_nul, motor_3.get_pth_fn, motor_3.set_pth_fn, &motor_3, M3_TMC2130_TPWMTHRS }, + { "3","3cth", _fip, 0, tx_print_nul, motor_3.get_cth_fn, motor_3.set_cth_fn, &motor_3, M3_TMC2130_TCOOLTHRS }, + { "3","3hth", _fip, 0, tx_print_nul, motor_3.get_hth_fn, motor_3.set_hth_fn, &motor_3, M3_TMC2130_THIGH }, + { "3","3sgt", _fip, 0, tx_print_nul, motor_3.get_sgt_fn, motor_3.set_sgt_fn, &motor_3, M3_TMC2130_SGT }, + { "3","3sgr", _f0, 0, tx_print_nul, motor_3.get_sgr_fn, set_ro, &motor_3, 0 }, + { "3","3csa", _f0, 0, tx_print_nul, motor_3.get_csa_fn, set_ro, &motor_3, 0 }, + { "3","3sgs", _f0, 0, tx_print_nul, motor_3.get_sgs_fn, set_ro, &motor_3, 0 }, + { "3","3tbl", _fip, 0, tx_print_nul, motor_3.get_tbl_fn, motor_3.set_tbl_fn, &motor_3, M3_TMC2130_TBL }, + { "3","3pgrd",_fip, 0, tx_print_nul, motor_3.get_pgrd_fn,motor_3.set_pgrd_fn, &motor_3, M3_TMC2130_PWM_GRAD }, + { "3","3pamp",_fip, 0, tx_print_nul, motor_3.get_pamp_fn,motor_3.set_pamp_fn, &motor_3, M3_TMC2130_PWM_AMPL }, + { "3","3hend",_fip, 0, tx_print_nul, motor_3.get_hend_fn,motor_3.set_hend_fn, &motor_3, M3_TMC2130_HEND }, + { "3","3hsrt",_fip, 0, tx_print_nul, motor_3.get_hsrt_fn,motor_3.set_hsrt_fn, &motor_3, M3_TMC2130_HSTRT }, + { "3","3smin",_fip, 0, tx_print_nul, motor_3.get_smin_fn,motor_3.set_smin_fn, &motor_3, M3_TMC2130_SMIN }, + { "3","3smax",_fip, 0, tx_print_nul, motor_3.get_smax_fn,motor_3.set_smax_fn, &motor_3, M3_TMC2130_SMAX }, + { "3","3sup", _fip, 0, tx_print_nul, motor_3.get_sup_fn, motor_3.set_sup_fn, &motor_3, M3_TMC2130_SUP }, + { "3","3sdn", _fip, 0, tx_print_nul, motor_3.get_sdn_fn, motor_3.set_sdn_fn, &motor_3, M3_TMC2130_SDN }, #endif #endif #if (MOTORS >= 4) - { "4","4ma", _fip, 0, st_print_ma, get_ui8, st_set_ma, (float *)&st_cfg.mot[MOTOR_4].motor_map, M4_MOTOR_MAP }, - { "4","4sa", _fip, 3, st_print_sa, get_flt, st_set_sa, (float *)&st_cfg.mot[MOTOR_4].step_angle, M4_STEP_ANGLE }, - { "4","4tr", _fipc, 4, st_print_tr, get_flt, st_set_tr, (float *)&st_cfg.mot[MOTOR_4].travel_rev, M4_TRAVEL_PER_REV }, - { "4","4mi", _fip, 0, st_print_mi, get_int, st_set_mi, (float *)&st_cfg.mot[MOTOR_4].microsteps, M4_MICROSTEPS }, - { "4","4su", _fipi, 5, st_print_su, st_get_su,st_set_su, (float *)&st_cfg.mot[MOTOR_4].steps_per_unit, M4_STEPS_PER_UNIT }, - { "4","4po", _fip, 0, st_print_po, get_ui8, set_01, (float *)&st_cfg.mot[MOTOR_4].polarity, M4_POLARITY }, - { "4","4pm", _fip, 0, st_print_pm, st_get_pm,st_set_pm, (float *)&cs.null, M4_POWER_MODE }, - { "4","4pl", _fip, 3, st_print_pl, get_flt, st_set_pl, (float *)&st_cfg.mot[MOTOR_4].power_level, M4_POWER_LEVEL }, - // { "4","4pi", _fip, 3, st_print_pi, get_flt, st_set_pi, (float *)&st_cfg.mot[MOTOR_4].power_idle, M4_POWER_IDLE }, - // { "4","4mt", _fip, 2, st_print_mt, get_flt, st_set_mt, (float *)&st_cfg.mot[MOTOR_4].motor_timeout, M4_MOTOR_TIMEOUT }, + { "4","4ma", _fip, 0, st_print_ma, st_get_ma, st_set_ma, &st_cfg.mot[MOTOR_4].motor_map, M4_MOTOR_MAP }, + { "4","4sa", _fip, 3, st_print_sa, st_get_sa, st_set_sa, &st_cfg.mot[MOTOR_4].step_angle, M4_STEP_ANGLE }, + { "4","4tr", _fipc, 4, st_print_tr, st_get_tr, st_set_tr, &st_cfg.mot[MOTOR_4].travel_rev, M4_TRAVEL_PER_REV }, + { "4","4su", _fipi, 5, st_print_su, st_get_su, st_set_su, &st_cfg.mot[MOTOR_4].steps_per_unit, M4_STEPS_PER_UNIT }, + { "4","4mi", _fip, 0, st_print_mi, st_get_mi, st_set_mi, &st_cfg.mot[MOTOR_4].microsteps, M4_MICROSTEPS }, + { "4","4po", _fip, 0, st_print_po, st_get_po, st_set_po, &st_cfg.mot[MOTOR_4].polarity, M4_POLARITY }, + { "4","4pm", _fip, 0, st_print_pm, st_get_pm, st_set_pm, &cs.null, M4_POWER_MODE }, + { "4","4pl", _fip, 3, st_print_pl, st_get_pl, st_set_pl, &st_cfg.mot[MOTOR_4].power_level, M4_POWER_LEVEL }, + { "4","4ep", _iip, 0, st_print_ep, st_get_ep, st_set_ep, nullptr, M4_ENABLE_POLARITY }, + { "4","4sp", _iip, 0, st_print_sp, st_get_sp, st_set_sp, nullptr, M4_STEP_POLARITY }, +// { "4","4pi", _fip, 3, st_print_pi, st_get_pi, st_set_pi, &st_cfg.mot[MOTOR_4].power_idle, M4_POWER_IDLE }, +// { "4","4mt", _fip, 2, st_print_mt, st_get_mt, st_set_mt, &st_cfg.mot[MOTOR_4].motor_timeout, M4_MOTOR_TIMEOUT }, #ifdef MOTOR_4_IS_TRINAMIC - { "4","4ts", _f0, 0, tx_print_nul, motor_4.get_ts_fn, set_ro, (float *)&motor_4, 0 }, - { "4","4pth", _fip, 0, tx_print_nul, motor_4.get_pth_fn, motor_4.set_pth_fn, (float *)&motor_4, M4_TMC2130_TPWMTHRS }, - { "4","4cth", _fip, 0, tx_print_nul, motor_4.get_cth_fn, motor_4.set_cth_fn, (float *)&motor_4, M4_TMC2130_TCOOLTHRS }, - { "4","4hth", _fip, 0, tx_print_nul, motor_4.get_hth_fn, motor_4.set_hth_fn, (float *)&motor_4, M4_TMC2130_THIGH }, - { "4","4sgt", _fip, 0, tx_print_nul, motor_4.get_sgt_fn, motor_4.set_sgt_fn, (float *)&motor_4, M4_TMC2130_SGT }, - { "4","4sgr", _f0, 0, tx_print_nul, motor_4.get_sgr_fn, set_ro, (float *)&motor_4, 0 }, - { "4","4csa", _f0, 0, tx_print_nul, motor_4.get_csa_fn, set_ro, (float *)&motor_4, 0 }, - { "4","4sgs", _f0, 0, tx_print_nul, motor_4.get_sgs_fn, set_ro, (float *)&motor_4, 0 }, - { "4","4tbl", _fip, 0, tx_print_nul, motor_4.get_tbl_fn, motor_4.set_tbl_fn, (float *)&motor_4, M4_TMC2130_TBL }, - { "4","4pgrd",_fip, 0, tx_print_nul, motor_4.get_pgrd_fn,motor_4.set_pgrd_fn, (float *)&motor_4, M4_TMC2130_PWM_GRAD }, - { "4","4pamp",_fip, 0, tx_print_nul, motor_4.get_pamp_fn,motor_4.set_pamp_fn, (float *)&motor_4, M4_TMC2130_PWM_AMPL }, - { "4","4hend",_fip, 0, tx_print_nul, motor_4.get_hend_fn,motor_4.set_hend_fn, (float *)&motor_4, M4_TMC2130_HEND }, - { "4","4hsrt",_fip, 0, tx_print_nul, motor_4.get_hsrt_fn,motor_4.set_hsrt_fn, (float *)&motor_4, M4_TMC2130_HSTRT }, - { "4","4smin",_fip, 0, tx_print_nul, motor_4.get_smin_fn,motor_4.set_smin_fn, (float *)&motor_4, M4_TMC2130_SMIN }, - { "4","4smax",_fip, 0, tx_print_nul, motor_4.get_smax_fn,motor_4.set_smax_fn, (float *)&motor_4, M4_TMC2130_SMAX }, - { "4","4sup", _fip, 0, tx_print_nul, motor_4.get_sup_fn, motor_4.set_sup_fn, (float *)&motor_4, M4_TMC2130_SUP }, - { "4","4sdn", _fip, 0, tx_print_nul, motor_4.get_sdn_fn, motor_4.set_sdn_fn, (float *)&motor_4, M4_TMC2130_SDN }, + { "4","4ts", _f0, 0, tx_print_nul, motor_4.get_ts_fn, set_ro, &motor_4, 0 }, + { "4","4pth", _fip, 0, tx_print_nul, motor_4.get_pth_fn, motor_4.set_pth_fn, &motor_4, M4_TMC2130_TPWMTHRS }, + { "4","4cth", _fip, 0, tx_print_nul, motor_4.get_cth_fn, motor_4.set_cth_fn, &motor_4, M4_TMC2130_TCOOLTHRS }, + { "4","4hth", _fip, 0, tx_print_nul, motor_4.get_hth_fn, motor_4.set_hth_fn, &motor_4, M4_TMC2130_THIGH }, + { "4","4sgt", _fip, 0, tx_print_nul, motor_4.get_sgt_fn, motor_4.set_sgt_fn, &motor_4, M4_TMC2130_SGT }, + { "4","4sgr", _f0, 0, tx_print_nul, motor_4.get_sgr_fn, set_ro, &motor_4, 0 }, + { "4","4csa", _f0, 0, tx_print_nul, motor_4.get_csa_fn, set_ro, &motor_4, 0 }, + { "4","4sgs", _f0, 0, tx_print_nul, motor_4.get_sgs_fn, set_ro, &motor_4, 0 }, + { "4","4tbl", _fip, 0, tx_print_nul, motor_4.get_tbl_fn, motor_4.set_tbl_fn, &motor_4, M4_TMC2130_TBL }, + { "4","4pgrd",_fip, 0, tx_print_nul, motor_4.get_pgrd_fn,motor_4.set_pgrd_fn, &motor_4, M4_TMC2130_PWM_GRAD }, + { "4","4pamp",_fip, 0, tx_print_nul, motor_4.get_pamp_fn,motor_4.set_pamp_fn, &motor_4, M4_TMC2130_PWM_AMPL }, + { "4","4hend",_fip, 0, tx_print_nul, motor_4.get_hend_fn,motor_4.set_hend_fn, &motor_4, M4_TMC2130_HEND }, + { "4","4hsrt",_fip, 0, tx_print_nul, motor_4.get_hsrt_fn,motor_4.set_hsrt_fn, &motor_4, M4_TMC2130_HSTRT }, + { "4","4smin",_fip, 0, tx_print_nul, motor_4.get_smin_fn,motor_4.set_smin_fn, &motor_4, M4_TMC2130_SMIN }, + { "4","4smax",_fip, 0, tx_print_nul, motor_4.get_smax_fn,motor_4.set_smax_fn, &motor_4, M4_TMC2130_SMAX }, + { "4","4sup", _fip, 0, tx_print_nul, motor_4.get_sup_fn, motor_4.set_sup_fn, &motor_4, M4_TMC2130_SUP }, + { "4","4sdn", _fip, 0, tx_print_nul, motor_4.get_sdn_fn, motor_4.set_sdn_fn, &motor_4, M4_TMC2130_SDN }, #endif #endif #if (MOTORS >= 5) - { "5","5ma", _fip, 0, st_print_ma, get_ui8, st_set_ma, (float *)&st_cfg.mot[MOTOR_5].motor_map, M5_MOTOR_MAP }, - { "5","5sa", _fip, 3, st_print_sa, get_flt, st_set_sa, (float *)&st_cfg.mot[MOTOR_5].step_angle, M5_STEP_ANGLE }, - { "5","5tr", _fipc, 4, st_print_tr, get_flt, st_set_tr, (float *)&st_cfg.mot[MOTOR_5].travel_rev, M5_TRAVEL_PER_REV }, - { "5","5mi", _fip, 0, st_print_mi, get_int, st_set_mi, (float *)&st_cfg.mot[MOTOR_5].microsteps, M5_MICROSTEPS }, - { "5","5su", _fipi, 5, st_print_su, st_get_su,st_set_su, (float *)&st_cfg.mot[MOTOR_5].steps_per_unit, M5_STEPS_PER_UNIT }, - { "5","5po", _fip, 0, st_print_po, get_ui8, set_01, (float *)&st_cfg.mot[MOTOR_5].polarity, M5_POLARITY }, - { "5","5pm", _fip, 0, st_print_pm, st_get_pm,st_set_pm, (float *)&cs.null, M5_POWER_MODE }, - { "5","5pl", _fip, 3, st_print_pl, get_flt, st_set_pl, (float *)&st_cfg.mot[MOTOR_5].power_level, M5_POWER_LEVEL }, - // { "5","5pi", _fip, 3, st_print_pi, get_flt, st_set_pi, (float *)&st_cfg.mot[MOTOR_5].power_idle, M5_POWER_IDLE }, - // { "5","5mt", _fip, 2, st_print_mt, get_flt, st_set_mt, (float *)&st_cfg.mot[MOTOR_5].motor_timeout, M5_MOTOR_TIMEOUT }, + { "5","5ma", _fip, 0, st_print_ma, st_get_ma, st_set_ma, &st_cfg.mot[MOTOR_5].motor_map, M5_MOTOR_MAP }, + { "5","5sa", _fip, 3, st_print_sa, st_get_sa, st_set_sa, &st_cfg.mot[MOTOR_5].step_angle, M5_STEP_ANGLE }, + { "5","5tr", _fipc, 4, st_print_tr, st_get_tr, st_set_tr, &st_cfg.mot[MOTOR_5].travel_rev, M5_TRAVEL_PER_REV }, + { "5","5su", _fipi, 5, st_print_su, st_get_su, st_set_su, &st_cfg.mot[MOTOR_5].steps_per_unit, M5_STEPS_PER_UNIT }, + { "5","5mi", _fip, 0, st_print_mi, st_get_mi, st_set_mi, &st_cfg.mot[MOTOR_5].microsteps, M5_MICROSTEPS }, + { "5","5po", _fip, 0, st_print_po, st_get_po, st_set_po, &st_cfg.mot[MOTOR_5].polarity, M5_POLARITY }, + { "5","5pm", _fip, 0, st_print_pm, st_get_pm, st_set_pm, &cs.null, M5_POWER_MODE }, + { "5","5pl", _fip, 3, st_print_pl, st_get_pl, st_set_pl, &st_cfg.mot[MOTOR_5].power_level, M5_POWER_LEVEL }, + { "5","5ep", _iip, 0, st_print_ep, st_get_ep, st_set_ep, nullptr, M5_ENABLE_POLARITY }, + { "5","5sp", _iip, 0, st_print_sp, st_get_sp, st_set_sp, nullptr, M5_STEP_POLARITY }, +// { "5","5pi", _fip, 3, st_print_pi, st_get_pi, st_set_pi, &st_cfg.mot[MOTOR_5].power_idle, M5_POWER_IDLE }, +// { "5","5mt", _fip, 2, st_print_mt, st_get_mt, st_set_mt, &st_cfg.mot[MOTOR_5].motor_timeout, M5_MOTOR_TIMEOUT }, #ifdef MOTOR_5_IS_TRINAMIC - { "5","5ts", _f0, 0, tx_print_nul, motor_5.get_ts_fn, set_ro, (float *)&motor_5, 0 }, - { "5","5pth", _fip, 0, tx_print_nul, motor_5.get_pth_fn, motor_5.set_pth_fn, (float *)&motor_5, M5_TMC2130_TPWMTHRS }, - { "5","5cth", _fip, 0, tx_print_nul, motor_5.get_cth_fn, motor_5.set_cth_fn, (float *)&motor_5, M5_TMC2130_TCOOLTHRS }, - { "5","5hth", _fip, 0, tx_print_nul, motor_5.get_hth_fn, motor_5.set_hth_fn, (float *)&motor_5, M5_TMC2130_THIGH }, - { "5","5sgt", _fip, 0, tx_print_nul, motor_5.get_sgt_fn, motor_5.set_sgt_fn, (float *)&motor_5, M5_TMC2130_SGT }, - { "5","5sgr", _f0, 0, tx_print_nul, motor_5.get_sgr_fn, set_ro, (float *)&motor_5, 0 }, - { "5","5csa", _f0, 0, tx_print_nul, motor_5.get_csa_fn, set_ro, (float *)&motor_5, 0 }, - { "5","5sgs", _f0, 0, tx_print_nul, motor_5.get_sgs_fn, set_ro, (float *)&motor_5, 0 }, - { "5","5tbl", _fip, 0, tx_print_nul, motor_5.get_tbl_fn, motor_5.set_tbl_fn, (float *)&motor_5, M5_TMC2130_TBL }, - { "5","5pgrd",_fip, 0, tx_print_nul, motor_5.get_pgrd_fn,motor_5.set_pgrd_fn, (float *)&motor_5, M5_TMC2130_PWM_GRAD }, - { "5","5pamp",_fip, 0, tx_print_nul, motor_5.get_pamp_fn,motor_5.set_pamp_fn, (float *)&motor_5, M5_TMC2130_PWM_AMPL }, - { "5","5hend",_fip, 0, tx_print_nul, motor_5.get_hend_fn,motor_5.set_hend_fn, (float *)&motor_5, M5_TMC2130_HEND }, - { "5","5hsrt",_fip, 0, tx_print_nul, motor_5.get_hsrt_fn,motor_5.set_hsrt_fn, (float *)&motor_5, M5_TMC2130_HSTRT }, - { "5","5smin",_fip, 0, tx_print_nul, motor_5.get_smin_fn,motor_5.set_smin_fn, (float *)&motor_5, M5_TMC2130_SMIN }, - { "5","5smax",_fip, 0, tx_print_nul, motor_5.get_smax_fn,motor_5.set_smax_fn, (float *)&motor_5, M5_TMC2130_SMAX }, - { "5","5sup", _fip, 0, tx_print_nul, motor_5.get_sup_fn, motor_5.set_sup_fn, (float *)&motor_5, M5_TMC2130_SUP }, - { "5","5sdn", _fip, 0, tx_print_nul, motor_5.get_sdn_fn, motor_5.set_sdn_fn, (float *)&motor_5, M5_TMC2130_SDN }, + { "5","5ts", _f0, 0, tx_print_nul, motor_5.get_ts_fn, set_ro, &motor_5, 0 }, + { "5","5pth", _fip, 0, tx_print_nul, motor_5.get_pth_fn, motor_5.set_pth_fn, &motor_5, M5_TMC2130_TPWMTHRS }, + { "5","5cth", _fip, 0, tx_print_nul, motor_5.get_cth_fn, motor_5.set_cth_fn, &motor_5, M5_TMC2130_TCOOLTHRS }, + { "5","5hth", _fip, 0, tx_print_nul, motor_5.get_hth_fn, motor_5.set_hth_fn, &motor_5, M5_TMC2130_THIGH }, + { "5","5sgt", _fip, 0, tx_print_nul, motor_5.get_sgt_fn, motor_5.set_sgt_fn, &motor_5, M5_TMC2130_SGT }, + { "5","5sgr", _f0, 0, tx_print_nul, motor_5.get_sgr_fn, set_ro, &motor_5, 0 }, + { "5","5csa", _f0, 0, tx_print_nul, motor_5.get_csa_fn, set_ro, &motor_5, 0 }, + { "5","5sgs", _f0, 0, tx_print_nul, motor_5.get_sgs_fn, set_ro, &motor_5, 0 }, + { "5","5tbl", _fip, 0, tx_print_nul, motor_5.get_tbl_fn, motor_5.set_tbl_fn, &motor_5, M5_TMC2130_TBL }, + { "5","5pgrd",_fip, 0, tx_print_nul, motor_5.get_pgrd_fn,motor_5.set_pgrd_fn, &motor_5, M5_TMC2130_PWM_GRAD }, + { "5","5pamp",_fip, 0, tx_print_nul, motor_5.get_pamp_fn,motor_5.set_pamp_fn, &motor_5, M5_TMC2130_PWM_AMPL }, + { "5","5hend",_fip, 0, tx_print_nul, motor_5.get_hend_fn,motor_5.set_hend_fn, &motor_5, M5_TMC2130_HEND }, + { "5","5hsrt",_fip, 0, tx_print_nul, motor_5.get_hsrt_fn,motor_5.set_hsrt_fn, &motor_5, M5_TMC2130_HSTRT }, + { "5","5smin",_fip, 0, tx_print_nul, motor_5.get_smin_fn,motor_5.set_smin_fn, &motor_5, M5_TMC2130_SMIN }, + { "5","5smax",_fip, 0, tx_print_nul, motor_5.get_smax_fn,motor_5.set_smax_fn, &motor_5, M5_TMC2130_SMAX }, + { "5","5sup", _fip, 0, tx_print_nul, motor_5.get_sup_fn, motor_5.set_sup_fn, &motor_5, M5_TMC2130_SUP }, + { "5","5sdn", _fip, 0, tx_print_nul, motor_5.get_sdn_fn, motor_5.set_sdn_fn, &motor_5, M5_TMC2130_SDN }, #endif #endif #if (MOTORS >= 6) - { "6","6ma", _fip, 0, st_print_ma, get_ui8, st_set_ma, (float *)&st_cfg.mot[MOTOR_6].motor_map, M6_MOTOR_MAP }, - { "6","6sa", _fip, 3, st_print_sa, get_flt, st_set_sa, (float *)&st_cfg.mot[MOTOR_6].step_angle, M6_STEP_ANGLE }, - { "6","6tr", _fipc, 4, st_print_tr, get_flt, st_set_tr, (float *)&st_cfg.mot[MOTOR_6].travel_rev, M6_TRAVEL_PER_REV }, - { "6","6mi", _fip, 0, st_print_mi, get_int, st_set_mi, (float *)&st_cfg.mot[MOTOR_6].microsteps, M6_MICROSTEPS }, - { "6","6su", _fipi, 5, st_print_su, st_get_su,st_set_su, (float *)&st_cfg.mot[MOTOR_6].steps_per_unit, M6_STEPS_PER_UNIT }, - { "6","6po", _fip, 0, st_print_po, get_ui8, set_01, (float *)&st_cfg.mot[MOTOR_6].polarity, M6_POLARITY }, - { "6","6pm", _fip, 0, st_print_pm, st_get_pm,st_set_pm, (float *)&cs.null, M6_POWER_MODE }, - { "6","6pl", _fip, 3, st_print_pl, get_flt, st_set_pl, (float *)&st_cfg.mot[MOTOR_6].power_level, M6_POWER_LEVEL }, - // { "6","6pi", _fip, 3, st_print_pi, get_flt, st_set_pi, (float *)&st_cfg.mot[MOTOR_6].power_idle, M6_POWER_IDLE }, - // { "6","6mt", _fip, 2, st_print_mt, get_flt, st_set_mt, (float *)&st_cfg.mot[MOTOR_6].motor_timeout, M6_MOTOR_TIMEOUT }, + { "6","6ma", _fip, 0, st_print_ma, st_get_ma, st_set_ma, &st_cfg.mot[MOTOR_6].motor_map, M6_MOTOR_MAP }, + { "6","6sa", _fip, 3, st_print_sa, st_get_sa, st_set_sa, &st_cfg.mot[MOTOR_6].step_angle, M6_STEP_ANGLE }, + { "6","6tr", _fipc, 4, st_print_tr, st_get_tr, st_set_tr, &st_cfg.mot[MOTOR_6].travel_rev, M6_TRAVEL_PER_REV }, + { "6","6su", _fipi, 5, st_print_su, st_get_su, st_set_su, &st_cfg.mot[MOTOR_6].steps_per_unit, M6_STEPS_PER_UNIT }, + { "6","6mi", _fip, 0, st_print_mi, st_get_mi, st_set_mi, &st_cfg.mot[MOTOR_6].microsteps, M6_MICROSTEPS }, + { "6","6po", _fip, 0, st_print_po, st_get_po, st_set_po, &st_cfg.mot[MOTOR_6].polarity, M6_POLARITY }, + { "6","6pm", _fip, 0, st_print_pm, st_get_pm, st_set_pm, &cs.null, M6_POWER_MODE }, + { "6","6pl", _fip, 3, st_print_pl, st_get_pl, st_set_pl, &st_cfg.mot[MOTOR_6].power_level, M6_POWER_LEVEL }, + { "6","6ep", _iip, 0, st_print_ep, st_get_ep, st_set_ep, nullptr, M6_ENABLE_POLARITY }, + { "6","6sp", _iip, 0, st_print_sp, st_get_sp, st_set_sp, nullptr, M6_STEP_POLARITY }, +// { "6","6pi", _fip, 3, st_print_pi, st_get_pi, st_set_pi, &st_cfg.mot[MOTOR_6].power_idle, M6_POWER_IDLE }, +// { "6","6mt", _fip, 2, st_print_mt, st_get_mt, st_set_mt, &st_cfg.mot[MOTOR_6].motor_timeout, M6_MOTOR_TIMEOUT }, #ifdef MOTOR_6_IS_TRINAMIC - { "6","6ts", _f0, 0, tx_print_nul, motor_6.get_ts_fn, set_ro, (float *)&motor_6, 0 }, - { "6","6pth", _fip, 0, tx_print_nul, motor_6.get_pth_fn, motor_6.set_pth_fn, (float *)&motor_6, M6_TMC2130_TPWMTHRS }, - { "6","6cth", _fip, 0, tx_print_nul, motor_6.get_cth_fn, motor_6.set_cth_fn, (float *)&motor_6, M6_TMC2130_TCOOLTHRS }, - { "6","6hth", _fip, 0, tx_print_nul, motor_6.get_hth_fn, motor_6.set_hth_fn, (float *)&motor_6, M6_TMC2130_THIGH }, - { "6","6sgt", _fip, 0, tx_print_nul, motor_6.get_sgt_fn, motor_6.set_sgt_fn, (float *)&motor_6, M6_TMC2130_SGT }, - { "6","6sgr", _f0, 0, tx_print_nul, motor_6.get_sgr_fn, set_ro, (float *)&motor_6, 0 }, - { "6","6csa", _f0, 0, tx_print_nul, motor_6.get_csa_fn, set_ro, (float *)&motor_6, 0 }, - { "6","6sgs", _f0, 0, tx_print_nul, motor_6.get_sgs_fn, set_ro, (float *)&motor_6, 0 }, - { "6","6tbl", _fip, 0, tx_print_nul, motor_6.get_tbl_fn, motor_6.set_tbl_fn, (float *)&motor_6, M6_TMC2130_TBL }, - { "6","6pgrd",_fip, 0, tx_print_nul, motor_6.get_pgrd_fn,motor_6.set_pgrd_fn, (float *)&motor_6, M6_TMC2130_PWM_GRAD }, - { "6","6pamp",_fip, 0, tx_print_nul, motor_6.get_pamp_fn,motor_6.set_pamp_fn, (float *)&motor_6, M6_TMC2130_PWM_AMPL }, - { "6","6hend",_fip, 0, tx_print_nul, motor_6.get_hend_fn,motor_6.set_hend_fn, (float *)&motor_6, M6_TMC2130_HEND }, - { "6","6hsrt",_fip, 0, tx_print_nul, motor_6.get_hsrt_fn,motor_6.set_hsrt_fn, (float *)&motor_6, M6_TMC2130_HSTRT }, - { "6","6smin",_fip, 0, tx_print_nul, motor_6.get_smin_fn,motor_6.set_smin_fn, (float *)&motor_6, M6_TMC2130_SMIN }, - { "6","6smax",_fip, 0, tx_print_nul, motor_6.get_smax_fn,motor_6.set_smax_fn, (float *)&motor_6, M6_TMC2130_SMAX }, - { "6","6sup", _fip, 0, tx_print_nul, motor_6.get_sup_fn, motor_6.set_sup_fn, (float *)&motor_6, M6_TMC2130_SUP }, - { "6","6sdn", _fip, 0, tx_print_nul, motor_6.get_sdn_fn, motor_6.set_sdn_fn, (float *)&motor_6, M6_TMC2130_SDN }, + { "6","6ts", _f0, 0, tx_print_nul, motor_6.get_ts_fn, set_ro, &motor_6, 0 }, + { "6","6pth", _fip, 0, tx_print_nul, motor_6.get_pth_fn, motor_6.set_pth_fn, &motor_6, M6_TMC2130_TPWMTHRS }, + { "6","6cth", _fip, 0, tx_print_nul, motor_6.get_cth_fn, motor_6.set_cth_fn, &motor_6, M6_TMC2130_TCOOLTHRS }, + { "6","6hth", _fip, 0, tx_print_nul, motor_6.get_hth_fn, motor_6.set_hth_fn, &motor_6, M6_TMC2130_THIGH }, + { "6","6sgt", _fip, 0, tx_print_nul, motor_6.get_sgt_fn, motor_6.set_sgt_fn, &motor_6, M6_TMC2130_SGT }, + { "6","6sgr", _f0, 0, tx_print_nul, motor_6.get_sgr_fn, set_ro, &motor_6, 0 }, + { "6","6csa", _f0, 0, tx_print_nul, motor_6.get_csa_fn, set_ro, &motor_6, 0 }, + { "6","6sgs", _f0, 0, tx_print_nul, motor_6.get_sgs_fn, set_ro, &motor_6, 0 }, + { "6","6tbl", _fip, 0, tx_print_nul, motor_6.get_tbl_fn, motor_6.set_tbl_fn, &motor_6, M6_TMC2130_TBL }, + { "6","6pgrd",_fip, 0, tx_print_nul, motor_6.get_pgrd_fn,motor_6.set_pgrd_fn, &motor_6, M6_TMC2130_PWM_GRAD }, + { "6","6pamp",_fip, 0, tx_print_nul, motor_6.get_pamp_fn,motor_6.set_pamp_fn, &motor_6, M6_TMC2130_PWM_AMPL }, + { "6","6hend",_fip, 0, tx_print_nul, motor_6.get_hend_fn,motor_6.set_hend_fn, &motor_6, M6_TMC2130_HEND }, + { "6","6hsrt",_fip, 0, tx_print_nul, motor_6.get_hsrt_fn,motor_6.set_hsrt_fn, &motor_6, M6_TMC2130_HSTRT }, + { "6","6smin",_fip, 0, tx_print_nul, motor_6.get_smin_fn,motor_6.set_smin_fn, &motor_6, M6_TMC2130_SMIN }, + { "6","6smax",_fip, 0, tx_print_nul, motor_6.get_smax_fn,motor_6.set_smax_fn, &motor_6, M6_TMC2130_SMAX }, + { "6","6sup", _fip, 0, tx_print_nul, motor_6.get_sup_fn, motor_6.set_sup_fn, &motor_6, M6_TMC2130_SUP }, + { "6","6sdn", _fip, 0, tx_print_nul, motor_6.get_sdn_fn, motor_6.set_sdn_fn, &motor_6, M6_TMC2130_SDN }, #endif #endif + + // Axis parameters - { "x","xam",_fip, 0, cm_print_am, cm_get_am, cm_set_am, (float *)&cm.a[AXIS_X].axis_mode, X_AXIS_MODE }, - { "x","xvm",_fipc, 0, cm_print_vm, get_flt, cm_set_vm, (float *)&cm.a[AXIS_X].velocity_max, X_VELOCITY_MAX }, - { "x","xfr",_fipc, 0, cm_print_fr, get_flt, cm_set_fr, (float *)&cm.a[AXIS_X].feedrate_max, X_FEEDRATE_MAX }, - { "x","xtn",_fipc, 3, cm_print_tn, get_flt, set_flu, (float *)&cm.a[AXIS_X].travel_min, X_TRAVEL_MIN }, - { "x","xtm",_fipc, 3, cm_print_tm, get_flt, set_flu, (float *)&cm.a[AXIS_X].travel_max, X_TRAVEL_MAX }, - { "x","xjm",_fipc, 0, cm_print_jm, get_flt, cm_set_jm, (float *)&cm.a[AXIS_X].jerk_max, X_JERK_MAX }, - { "x","xjh",_fipc, 0, cm_print_jh, get_flt, cm_set_jh, (float *)&cm.a[AXIS_X].jerk_high, X_JERK_HIGH_SPEED }, - { "x","xhi",_fip, 0, cm_print_hi, get_ui8, cm_set_hi, (float *)&cm.a[AXIS_X].homing_input, X_HOMING_INPUT }, - { "x","xhd",_fip, 0, cm_print_hd, get_ui8, set_01, (float *)&cm.a[AXIS_X].homing_dir, X_HOMING_DIRECTION }, - { "x","xsv",_fipc, 0, cm_print_sv, get_flt, set_flup, (float *)&cm.a[AXIS_X].search_velocity,X_SEARCH_VELOCITY }, - { "x","xlv",_fipc, 2, cm_print_lv, get_flt, set_flup, (float *)&cm.a[AXIS_X].latch_velocity, X_LATCH_VELOCITY }, - { "x","xlb",_fipc, 3, cm_print_lb, get_flt, set_flu, (float *)&cm.a[AXIS_X].latch_backoff, X_LATCH_BACKOFF }, - { "x","xzb",_fipc, 3, cm_print_zb, get_flt, set_flu, (float *)&cm.a[AXIS_X].zero_backoff, X_ZERO_BACKOFF }, + { "x","xam",_iip, 0, cm_print_am, cm_get_am, cm_set_am, nullptr, X_AXIS_MODE }, + { "x","xvm",_fipc, 0, cm_print_vm, cm_get_vm, cm_set_vm, nullptr, X_VELOCITY_MAX }, + { "x","xfr",_fipc, 0, cm_print_fr, cm_get_fr, cm_set_fr, nullptr, X_FEEDRATE_MAX }, + { "x","xtn",_fipc, 5, cm_print_tn, cm_get_tn, cm_set_tn, nullptr, X_TRAVEL_MIN }, + { "x","xtm",_fipc, 5, cm_print_tm, cm_get_tm, cm_set_tm, nullptr, X_TRAVEL_MAX }, + { "x","xjm",_fipc, 0, cm_print_jm, cm_get_jm, cm_set_jm, nullptr, X_JERK_MAX }, + { "x","xjh",_fipc, 0, cm_print_jh, cm_get_jh, cm_set_jh, nullptr, X_JERK_HIGH_SPEED }, + { "x","xhi",_iip, 0, cm_print_hi, cm_get_hi, cm_set_hi, nullptr, X_HOMING_INPUT }, + { "x","xhd",_iip, 0, cm_print_hd, cm_get_hd, cm_set_hd, nullptr, X_HOMING_DIRECTION }, + { "x","xsv",_fipc, 0, cm_print_sv, cm_get_sv, cm_set_sv, nullptr, X_SEARCH_VELOCITY }, + { "x","xlv",_fipc, 2, cm_print_lv, cm_get_lv, cm_set_lv, nullptr, X_LATCH_VELOCITY }, + { "x","xlb",_fipc, 5, cm_print_lb, cm_get_lb, cm_set_lb, nullptr, X_LATCH_BACKOFF }, + { "x","xzb",_fipc, 5, cm_print_zb, cm_get_zb, cm_set_zb, nullptr, X_ZERO_BACKOFF }, - { "y","yam",_fip, 0, cm_print_am, cm_get_am, cm_set_am, (float *)&cm.a[AXIS_Y].axis_mode, Y_AXIS_MODE }, - { "y","yvm",_fipc, 0, cm_print_vm, get_flt, cm_set_vm, (float *)&cm.a[AXIS_Y].velocity_max, Y_VELOCITY_MAX }, - { "y","yfr",_fipc, 0, cm_print_fr, get_flt, cm_set_fr, (float *)&cm.a[AXIS_Y].feedrate_max, Y_FEEDRATE_MAX }, - { "y","ytn",_fipc, 3, cm_print_tn, get_flt, set_flu, (float *)&cm.a[AXIS_Y].travel_min, Y_TRAVEL_MIN }, - { "y","ytm",_fipc, 3, cm_print_tm, get_flt, set_flu, (float *)&cm.a[AXIS_Y].travel_max, Y_TRAVEL_MAX }, - { "y","yjm",_fipc, 0, cm_print_jm, get_flt, cm_set_jm, (float *)&cm.a[AXIS_Y].jerk_max, Y_JERK_MAX }, - { "y","yjh",_fipc, 0, cm_print_jh, get_flt, cm_set_jh, (float *)&cm.a[AXIS_Y].jerk_high, Y_JERK_HIGH_SPEED }, - { "y","yhi",_fip, 0, cm_print_hi, get_ui8, cm_set_hi, (float *)&cm.a[AXIS_Y].homing_input, Y_HOMING_INPUT }, - { "y","yhd",_fip, 0, cm_print_hd, get_ui8, set_01, (float *)&cm.a[AXIS_Y].homing_dir, Y_HOMING_DIRECTION }, - { "y","ysv",_fipc, 0, cm_print_sv, get_flt, set_flup, (float *)&cm.a[AXIS_Y].search_velocity,Y_SEARCH_VELOCITY }, - { "y","ylv",_fipc, 2, cm_print_lv, get_flt, set_flup, (float *)&cm.a[AXIS_Y].latch_velocity, Y_LATCH_VELOCITY }, - { "y","ylb",_fipc, 3, cm_print_lb, get_flt, set_flu, (float *)&cm.a[AXIS_Y].latch_backoff, Y_LATCH_BACKOFF }, - { "y","yzb",_fipc, 3, cm_print_zb, get_flt, set_flu, (float *)&cm.a[AXIS_Y].zero_backoff, Y_ZERO_BACKOFF }, + { "y","yam",_iip, 0, cm_print_am, cm_get_am, cm_set_am, nullptr, Y_AXIS_MODE }, + { "y","yvm",_fipc, 0, cm_print_vm, cm_get_vm, cm_set_vm, nullptr, Y_VELOCITY_MAX }, + { "y","yfr",_fipc, 0, cm_print_fr, cm_get_fr, cm_set_fr, nullptr, Y_FEEDRATE_MAX }, + { "y","ytn",_fipc, 5, cm_print_tn, cm_get_tn, cm_set_tn, nullptr, Y_TRAVEL_MIN }, + { "y","ytm",_fipc, 5, cm_print_tm, cm_get_tm, cm_set_tm, nullptr, Y_TRAVEL_MAX }, + { "y","yjm",_fipc, 0, cm_print_jm, cm_get_jm, cm_set_jm, nullptr, Y_JERK_MAX }, + { "y","yjh",_fipc, 0, cm_print_jh, cm_get_jh, cm_set_jh, nullptr, Y_JERK_HIGH_SPEED }, + { "y","yhi",_iip, 0, cm_print_hi, cm_get_hi, cm_set_hi, nullptr, Y_HOMING_INPUT }, + { "y","yhd",_iip, 0, cm_print_hd, cm_get_hd, cm_set_hd, nullptr, Y_HOMING_DIRECTION }, + { "y","ysv",_fipc, 0, cm_print_sv, cm_get_sv, cm_set_sv, nullptr, Y_SEARCH_VELOCITY }, + { "y","ylv",_fipc, 2, cm_print_lv, cm_get_lv, cm_set_lv, nullptr, Y_LATCH_VELOCITY }, + { "y","ylb",_fipc, 5, cm_print_lb, cm_get_lb, cm_set_lb, nullptr, Y_LATCH_BACKOFF }, + { "y","yzb",_fipc, 5, cm_print_zb, cm_get_zb, cm_set_zb, nullptr, Y_ZERO_BACKOFF }, - { "z","zam",_fip, 0, cm_print_am, cm_get_am, cm_set_am, (float *)&cm.a[AXIS_Z].axis_mode, Z_AXIS_MODE }, - { "z","zvm",_fipc, 0, cm_print_vm, get_flt, cm_set_vm, (float *)&cm.a[AXIS_Z].velocity_max, Z_VELOCITY_MAX }, - { "z","zfr",_fipc, 0, cm_print_fr, get_flt, cm_set_fr, (float *)&cm.a[AXIS_Z].feedrate_max, Z_FEEDRATE_MAX }, - { "z","ztn",_fipc, 3, cm_print_tn, get_flt, set_flu, (float *)&cm.a[AXIS_Z].travel_min, Z_TRAVEL_MIN }, - { "z","ztm",_fipc, 3, cm_print_tm, get_flt, set_flu, (float *)&cm.a[AXIS_Z].travel_max, Z_TRAVEL_MAX }, - { "z","zjm",_fipc, 0, cm_print_jm, get_flt, cm_set_jm, (float *)&cm.a[AXIS_Z].jerk_max, Z_JERK_MAX }, - { "z","zjh",_fipc, 0, cm_print_jh, get_flt, cm_set_jh, (float *)&cm.a[AXIS_Z].jerk_high, Z_JERK_HIGH_SPEED }, - { "z","zhi",_fip, 0, cm_print_hi, get_ui8, cm_set_hi, (float *)&cm.a[AXIS_Z].homing_input, Z_HOMING_INPUT }, - { "z","zhd",_fip, 0, cm_print_hd, get_ui8, set_01, (float *)&cm.a[AXIS_Z].homing_dir, Z_HOMING_DIRECTION }, - { "z","zsv",_fipc, 0, cm_print_sv, get_flt, set_flup, (float *)&cm.a[AXIS_Z].search_velocity,Z_SEARCH_VELOCITY }, - { "z","zlv",_fipc, 2, cm_print_lv, get_flt, set_flup, (float *)&cm.a[AXIS_Z].latch_velocity, Z_LATCH_VELOCITY }, - { "z","zlb",_fipc, 3, cm_print_lb, get_flt, set_flu, (float *)&cm.a[AXIS_Z].latch_backoff, Z_LATCH_BACKOFF }, - { "z","zzb",_fipc, 3, cm_print_zb, get_flt, set_flu, (float *)&cm.a[AXIS_Z].zero_backoff, Z_ZERO_BACKOFF }, + { "z","zam",_iip, 0, cm_print_am, cm_get_am, cm_set_am, nullptr, Z_AXIS_MODE }, + { "z","zvm",_fipc, 0, cm_print_vm, cm_get_vm, cm_set_vm, nullptr, Z_VELOCITY_MAX }, + { "z","zfr",_fipc, 0, cm_print_fr, cm_get_fr, cm_set_fr, nullptr, Z_FEEDRATE_MAX }, + { "z","ztn",_fipc, 5, cm_print_tn, cm_get_tn, cm_set_tn, nullptr, Z_TRAVEL_MIN }, + { "z","ztm",_fipc, 5, cm_print_tm, cm_get_tm, cm_set_tm, nullptr, Z_TRAVEL_MAX }, + { "z","zjm",_fipc, 0, cm_print_jm, cm_get_jm, cm_set_jm, nullptr, Z_JERK_MAX }, + { "z","zjh",_fipc, 0, cm_print_jh, cm_get_jm, cm_set_jh, nullptr, Z_JERK_HIGH_SPEED }, + { "z","zhi",_iip, 0, cm_print_hi, cm_get_hi, cm_set_hi, nullptr, Z_HOMING_INPUT }, + { "z","zhd",_iip, 0, cm_print_hd, cm_get_hd, cm_set_hd, nullptr, Z_HOMING_DIRECTION }, + { "z","zsv",_fipc, 0, cm_print_sv, cm_get_sv, cm_set_sv, nullptr, Z_SEARCH_VELOCITY }, + { "z","zlv",_fipc, 2, cm_print_lv, cm_get_lv, cm_set_lv, nullptr, Z_LATCH_VELOCITY }, + { "z","zlb",_fipc, 5, cm_print_lb, cm_get_lb, cm_set_lb, nullptr, Z_LATCH_BACKOFF }, + { "z","zzb",_fipc, 5, cm_print_zb, cm_get_zb, cm_set_zb, nullptr, Z_ZERO_BACKOFF }, - { "a","aam",_fip, 0, cm_print_am, cm_get_am, cm_set_am, (float *)&cm.a[AXIS_A].axis_mode, A_AXIS_MODE }, - { "a","avm",_fip, 0, cm_print_vm, get_flt, cm_set_vm, (float *)&cm.a[AXIS_A].velocity_max, A_VELOCITY_MAX }, - { "a","afr",_fip, 0, cm_print_fr, get_flt, cm_set_fr, (float *)&cm.a[AXIS_A].feedrate_max, A_FEEDRATE_MAX }, - { "a","atn",_fip, 3, cm_print_tn, get_flt, set_flt, (float *)&cm.a[AXIS_A].travel_min, A_TRAVEL_MIN }, - { "a","atm",_fip, 3, cm_print_tm, get_flt, set_flt, (float *)&cm.a[AXIS_A].travel_max, A_TRAVEL_MAX }, - { "a","ajm",_fip, 0, cm_print_jm, get_flt, cm_set_jm, (float *)&cm.a[AXIS_A].jerk_max, A_JERK_MAX }, - { "a","ajh",_fip, 0, cm_print_jh, get_flt, cm_set_jh, (float *)&cm.a[AXIS_A].jerk_high, A_JERK_HIGH_SPEED }, - { "a","ara",_fipc, 3, cm_print_ra, get_flt, set_flt, (float *)&cm.a[AXIS_A].radius, A_RADIUS}, - { "a","ahi",_fip, 0, cm_print_hi, get_ui8, cm_set_hi, (float *)&cm.a[AXIS_A].homing_input, A_HOMING_INPUT }, - { "a","ahd",_fip, 0, cm_print_hd, get_ui8, set_01, (float *)&cm.a[AXIS_A].homing_dir, A_HOMING_DIRECTION }, - { "a","asv",_fip, 0, cm_print_sv, get_flt, set_fltp, (float *)&cm.a[AXIS_A].search_velocity,A_SEARCH_VELOCITY }, - { "a","alv",_fip, 2, cm_print_lv, get_flt, set_fltp, (float *)&cm.a[AXIS_A].latch_velocity, A_LATCH_VELOCITY }, - { "a","alb",_fip, 3, cm_print_lb, get_flt, set_flt, (float *)&cm.a[AXIS_A].latch_backoff, A_LATCH_BACKOFF }, - { "a","azb",_fip, 3, cm_print_zb, get_flt, set_flt, (float *)&cm.a[AXIS_A].zero_backoff, A_ZERO_BACKOFF }, + { "u","uam",_iip, 0, cm_print_am, cm_get_am, cm_set_am, nullptr, U_AXIS_MODE }, + { "u","uvm",_fipc, 0, cm_print_vm, cm_get_vm, cm_set_vm, nullptr, U_VELOCITY_MAX }, + { "u","ufr",_fipc, 0, cm_print_fr, cm_get_fr, cm_set_fr, nullptr, U_FEEDRATE_MAX }, + { "u","utn",_fipc, 5, cm_print_tn, cm_get_tn, cm_set_tn, nullptr, U_TRAVEL_MIN }, + { "u","utm",_fipc, 5, cm_print_tm, cm_get_tm, cm_set_tm, nullptr, U_TRAVEL_MAX }, + { "u","ujm",_fipc, 0, cm_print_jm, cm_get_jm, cm_set_jm, nullptr, U_JERK_MAX }, + { "u","ujh",_fipc, 0, cm_print_jh, cm_get_jh, cm_set_jh, nullptr, U_JERK_HIGH_SPEED }, + { "u","uhi",_iip, 0, cm_print_hi, cm_get_hi, cm_set_hi, nullptr, U_HOMING_INPUT }, + { "u","uhd",_iip, 0, cm_print_hd, cm_get_hd, cm_set_hd, nullptr, U_HOMING_DIRECTION }, + { "u","usv",_fipc, 0, cm_print_sv, cm_get_sv, cm_set_sv, nullptr, U_SEARCH_VELOCITY }, + { "u","ulv",_fipc, 2, cm_print_lv, cm_get_lv, cm_set_lv, nullptr, U_LATCH_VELOCITY }, + { "u","ulb",_fipc, 5, cm_print_lb, cm_get_lb, cm_set_lb, nullptr, U_LATCH_BACKOFF }, + { "u","uzb",_fipc, 5, cm_print_zb, cm_get_zb, cm_set_zb, nullptr, U_ZERO_BACKOFF }, - { "b","bam",_fip, 0, cm_print_am, cm_get_am, cm_set_am, (float *)&cm.a[AXIS_B].axis_mode, B_AXIS_MODE }, - { "b","bvm",_fip, 0, cm_print_vm, get_flt, cm_set_vm, (float *)&cm.a[AXIS_B].velocity_max, B_VELOCITY_MAX }, - { "b","bfr",_fip, 0, cm_print_fr, get_flt, cm_set_fr, (float *)&cm.a[AXIS_B].feedrate_max, B_FEEDRATE_MAX }, - { "b","btn",_fip, 3, cm_print_tn, get_flt, set_flt, (float *)&cm.a[AXIS_B].travel_min, B_TRAVEL_MIN }, - { "b","btm",_fip, 3, cm_print_tm, get_flt, set_flt, (float *)&cm.a[AXIS_B].travel_max, B_TRAVEL_MAX }, - { "b","bjm",_fip, 0, cm_print_jm, get_flt, cm_set_jm, (float *)&cm.a[AXIS_B].jerk_max, B_JERK_MAX }, - { "b","bjh",_fip, 0, cm_print_jh, get_flt, cm_set_jh, (float *)&cm.a[AXIS_B].jerk_high, B_JERK_HIGH_SPEED }, - { "b","bra",_fipc, 3, cm_print_ra, get_flt, set_flt, (float *)&cm.a[AXIS_B].radius, B_RADIUS }, - { "b","bhi",_fip, 0, cm_print_hi, get_ui8, cm_set_hi, (float *)&cm.a[AXIS_B].homing_input, B_HOMING_INPUT }, - { "b","bhd",_fip, 0, cm_print_hd, get_ui8, set_01, (float *)&cm.a[AXIS_B].homing_dir, B_HOMING_DIRECTION }, - { "b","bsv",_fip, 0, cm_print_sv, get_flt, set_fltp, (float *)&cm.a[AXIS_B].search_velocity,B_SEARCH_VELOCITY }, - { "b","blv",_fip, 2, cm_print_lv, get_flt, set_fltp, (float *)&cm.a[AXIS_B].latch_velocity, B_LATCH_VELOCITY }, - { "b","blb",_fip, 3, cm_print_lb, get_flt, set_flt, (float *)&cm.a[AXIS_B].latch_backoff, B_LATCH_BACKOFF }, - { "b","bzb",_fip, 3, cm_print_zb, get_flt, set_flt, (float *)&cm.a[AXIS_B].zero_backoff, B_ZERO_BACKOFF }, + { "v","vam",_iip, 0, cm_print_am, cm_get_am, cm_set_am, nullptr, V_AXIS_MODE }, + { "v","vvm",_fipc, 0, cm_print_vm, cm_get_vm, cm_set_vm, nullptr, V_VELOCITY_MAX }, + { "v","vfr",_fipc, 0, cm_print_fr, cm_get_fr, cm_set_fr, nullptr, V_FEEDRATE_MAX }, + { "v","vtn",_fipc, 5, cm_print_tn, cm_get_tn, cm_set_tn, nullptr, V_TRAVEL_MIN }, + { "v","vtm",_fipc, 5, cm_print_tm, cm_get_tm, cm_set_tm, nullptr, V_TRAVEL_MAX }, + { "v","vjm",_fipc, 0, cm_print_jm, cm_get_jm, cm_set_jm, nullptr, V_JERK_MAX }, + { "v","vjh",_fipc, 0, cm_print_jh, cm_get_jh, cm_set_jh, nullptr, V_JERK_HIGH_SPEED }, + { "v","vhi",_iip, 0, cm_print_hi, cm_get_hi, cm_set_hi, nullptr, V_HOMING_INPUT }, + { "v","vhd",_iip, 0, cm_print_hd, cm_get_hd, cm_set_hd, nullptr, V_HOMING_DIRECTION }, + { "v","vsv",_fipc, 0, cm_print_sv, cm_get_sv, cm_set_sv, nullptr, V_SEARCH_VELOCITY }, + { "v","vlv",_fipc, 2, cm_print_lv, cm_get_lv, cm_set_lv, nullptr, V_LATCH_VELOCITY }, + { "v","vlb",_fipc, 5, cm_print_lb, cm_get_lb, cm_set_lb, nullptr, V_LATCH_BACKOFF }, + { "v","vzb",_fipc, 5, cm_print_zb, cm_get_zb, cm_set_zb, nullptr, V_ZERO_BACKOFF }, - { "c","cam",_fip, 0, cm_print_am, cm_get_am, cm_set_am, (float *)&cm.a[AXIS_C].axis_mode, C_AXIS_MODE }, - { "c","cvm",_fip, 0, cm_print_vm, get_flt, cm_set_vm, (float *)&cm.a[AXIS_C].velocity_max, C_VELOCITY_MAX }, - { "c","cfr",_fip, 0, cm_print_fr, get_flt, cm_set_fr, (float *)&cm.a[AXIS_C].feedrate_max, C_FEEDRATE_MAX }, - { "c","ctn",_fip, 3, cm_print_tn, get_flt, set_flt, (float *)&cm.a[AXIS_C].travel_min, C_TRAVEL_MIN }, - { "c","ctm",_fip, 3, cm_print_tm, get_flt, set_flt, (float *)&cm.a[AXIS_C].travel_max, C_TRAVEL_MAX }, - { "c","cjm",_fip, 0, cm_print_jm, get_flt, cm_set_jm, (float *)&cm.a[AXIS_C].jerk_max, C_JERK_MAX }, - { "c","cjh",_fip, 0, cm_print_jh, get_flt, cm_set_jh, (float *)&cm.a[AXIS_C].jerk_high, C_JERK_HIGH_SPEED }, - { "c","cra",_fipc, 3, cm_print_ra, get_flt, set_flt, (float *)&cm.a[AXIS_C].radius, C_RADIUS }, - { "c","chi",_fip, 0, cm_print_hi, get_ui8, cm_set_hi, (float *)&cm.a[AXIS_C].homing_input, C_HOMING_INPUT }, - { "c","chd",_fip, 0, cm_print_hd, get_ui8, set_01, (float *)&cm.a[AXIS_C].homing_dir, C_HOMING_DIRECTION }, - { "c","csv",_fip, 0, cm_print_sv, get_flt, set_fltp, (float *)&cm.a[AXIS_C].search_velocity,C_SEARCH_VELOCITY }, - { "c","clv",_fip, 2, cm_print_lv, get_flt, set_fltp, (float *)&cm.a[AXIS_C].latch_velocity, C_LATCH_VELOCITY }, - { "c","clb",_fip, 3, cm_print_lb, get_flt, set_flt, (float *)&cm.a[AXIS_C].latch_backoff, C_LATCH_BACKOFF }, - { "c","czb",_fip, 3, cm_print_zb, get_flt, set_flt, (float *)&cm.a[AXIS_C].zero_backoff, C_ZERO_BACKOFF }, + { "w","wam",_iip, 0, cm_print_am, cm_get_am, cm_set_am, nullptr, W_AXIS_MODE }, + { "w","wvm",_fipc, 0, cm_print_vm, cm_get_vm, cm_set_vm, nullptr, W_VELOCITY_MAX }, + { "w","wfr",_fipc, 0, cm_print_fr, cm_get_fr, cm_set_fr, nullptr, W_FEEDRATE_MAX }, + { "w","wtn",_fipc, 5, cm_print_tn, cm_get_tn, cm_set_tn, nullptr, W_TRAVEL_MIN }, + { "w","wtm",_fipc, 5, cm_print_tm, cm_get_tm, cm_set_tm, nullptr, W_TRAVEL_MAX }, + { "w","wjm",_fipc, 0, cm_print_jm, cm_get_jm, cm_set_jm, nullptr, W_JERK_MAX }, + { "w","wjh",_fipc, 0, cm_print_jh, cm_get_jh, cm_set_jh, nullptr, W_JERK_HIGH_SPEED }, + { "w","whi",_iip, 0, cm_print_hi, cm_get_hi, cm_set_hi, nullptr, W_HOMING_INPUT }, + { "w","whd",_iip, 0, cm_print_hd, cm_get_hd, cm_set_hd, nullptr, W_HOMING_DIRECTION }, + { "w","wsv",_fipc, 0, cm_print_sv, cm_get_sv, cm_set_sv, nullptr, W_SEARCH_VELOCITY }, + { "w","wlv",_fipc, 2, cm_print_lv, cm_get_lv, cm_set_lv, nullptr, W_LATCH_VELOCITY }, + { "w","wlb",_fipc, 5, cm_print_lb, cm_get_lb, cm_set_lb, nullptr, W_LATCH_BACKOFF }, + { "w","wzb",_fipc, 5, cm_print_zb, cm_get_zb, cm_set_zb, nullptr, W_ZERO_BACKOFF }, + + { "a","aam",_iip, 0, cm_print_am, cm_get_am, cm_set_am, nullptr, A_AXIS_MODE }, + { "a","avm",_fipc, 0, cm_print_vm, cm_get_vm, cm_set_vm, nullptr, A_VELOCITY_MAX }, + { "a","afr",_fipc, 0, cm_print_fr, cm_get_fr, cm_set_fr, nullptr, A_FEEDRATE_MAX }, + { "a","atn",_fipc, 5, cm_print_tn, cm_get_tn, cm_set_tn, nullptr, A_TRAVEL_MIN }, + { "a","atm",_fipc, 5, cm_print_tm, cm_get_tm, cm_set_tm, nullptr, A_TRAVEL_MAX }, + { "a","ajm",_fipc, 0, cm_print_jm, cm_get_jm, cm_set_jm, nullptr, A_JERK_MAX }, + { "a","ajh",_fipc, 0, cm_print_jh, cm_get_jh, cm_set_jh, nullptr, A_JERK_HIGH_SPEED }, + { "a","ara",_fipc, 5, cm_print_ra, cm_get_ra, cm_set_ra, nullptr, A_RADIUS}, + { "a","ahi",_iip, 0, cm_print_hi, cm_get_hi, cm_set_hi, nullptr, A_HOMING_INPUT }, + { "a","ahd",_iip, 0, cm_print_hd, cm_get_hd, cm_set_hd, nullptr, A_HOMING_DIRECTION }, + { "a","asv",_fipc, 0, cm_print_sv, cm_get_sv, cm_set_sv, nullptr, A_SEARCH_VELOCITY }, + { "a","alv",_fipc, 2, cm_print_lv, cm_get_lv, cm_set_lv, nullptr, A_LATCH_VELOCITY }, + { "a","alb",_fipc, 5, cm_print_lb, cm_get_lb, cm_set_lb, nullptr, A_LATCH_BACKOFF }, + { "a","azb",_fipc, 5, cm_print_zb, cm_get_zb, cm_set_zb, nullptr, A_ZERO_BACKOFF }, + + { "b","bam",_iip, 0, cm_print_am, cm_get_am, cm_set_am, nullptr, B_AXIS_MODE }, + { "b","bvm",_fipc, 0, cm_print_vm, cm_get_vm, cm_set_vm, nullptr, B_VELOCITY_MAX }, + { "b","bfr",_fipc, 0, cm_print_fr, cm_get_fr, cm_set_fr, nullptr, B_FEEDRATE_MAX }, + { "b","btn",_fipc, 5, cm_print_tn, cm_get_tn, cm_set_tn, nullptr, B_TRAVEL_MIN }, + { "b","btm",_fipc, 5, cm_print_tm, cm_get_tm, cm_set_tm, nullptr, B_TRAVEL_MAX }, + { "b","bjm",_fipc, 0, cm_print_jm, cm_get_jm, cm_set_jm, nullptr, B_JERK_MAX }, + { "b","bjh",_fipc, 0, cm_print_jh, cm_get_jh, cm_set_jh, nullptr, B_JERK_HIGH_SPEED }, + { "b","bra",_fipc, 5, cm_print_ra, cm_get_ra, cm_set_ra, nullptr, B_RADIUS }, + { "b","bhi",_iip, 0, cm_print_hi, cm_get_hi, cm_set_hi, nullptr, B_HOMING_INPUT }, + { "b","bhd",_iip, 0, cm_print_hd, cm_get_hd, cm_set_hd, nullptr, B_HOMING_DIRECTION }, + { "b","bsv",_fipc, 0, cm_print_sv, cm_get_sv, cm_set_sv, nullptr, B_SEARCH_VELOCITY }, + { "b","blv",_fipc, 2, cm_print_lv, cm_get_lv, cm_set_lv, nullptr, B_LATCH_VELOCITY }, + { "b","blb",_fipc, 5, cm_print_lb, cm_get_lb, cm_set_lb, nullptr, B_LATCH_BACKOFF }, + { "b","bzb",_fipc, 5, cm_print_zb, cm_get_zb, cm_set_zb, nullptr, B_ZERO_BACKOFF }, + + { "c","cam",_iip, 0, cm_print_am, cm_get_am, cm_set_am, nullptr, C_AXIS_MODE }, + { "c","cvm",_fipc, 0, cm_print_vm, cm_get_vm, cm_set_vm, nullptr, C_VELOCITY_MAX }, + { "c","cfr",_fipc, 0, cm_print_fr, cm_get_fr, cm_set_fr, nullptr, C_FEEDRATE_MAX }, + { "c","ctn",_fipc, 5, cm_print_tn, cm_get_tn, cm_set_tn, nullptr, C_TRAVEL_MIN }, + { "c","ctm",_fipc, 5, cm_print_tm, cm_get_tm, cm_set_tm, nullptr, C_TRAVEL_MAX }, + { "c","cjm",_fipc, 0, cm_print_jm, cm_get_jm, cm_set_jm, nullptr, C_JERK_MAX }, + { "c","cjh",_fipc, 0, cm_print_jh, cm_get_jh, cm_set_jh, nullptr, C_JERK_HIGH_SPEED }, + { "c","cra",_fipc, 5, cm_print_ra, cm_get_ra, cm_set_ra, nullptr, C_RADIUS }, + { "c","chi",_iip, 0, cm_print_hi, cm_get_hi, cm_set_hi, nullptr, C_HOMING_INPUT }, + { "c","chd",_iip, 0, cm_print_hd, cm_get_hd, cm_set_hd, nullptr, C_HOMING_DIRECTION }, + { "c","csv",_fipc, 0, cm_print_sv, cm_get_sv, cm_set_sv, nullptr, C_SEARCH_VELOCITY }, + { "c","clv",_fipc, 2, cm_print_lv, cm_get_lv, cm_set_lv, nullptr, C_LATCH_VELOCITY }, + { "c","clb",_fipc, 5, cm_print_lb, cm_get_lb, cm_set_lb, nullptr, C_LATCH_BACKOFF }, + { "c","czb",_fipc, 5, cm_print_zb, cm_get_zb, cm_set_zb, nullptr, C_ZERO_BACKOFF }, // Digital input configs - { "di1","di1en",_fip, 0, din_print_en, din_get_en, din_set_en, (float *)&din1, DI1_ENABLED }, - { "di1","di1po",_fip, 0, din_print_po, din_get_po, din_set_po, (float *)&din1, DI1_POLARITY }, - { "di1","di1ac",_fip, 0, din_print_ac, din_get_ac, din_set_ac, (float *)&din1, DI1_ACTION }, - // { "di1","di1fn",_fip, 0, din_print_fn, din_get_fn, din_set_fn, (float *)&din1, DI1_FUNCTION }, - { "di1","di1in",_fip, 0, din_print_in, din_get_in, din_set_in, (float *)&din1, DI1_EXTERNAL_NUMBER }, + { "di1","di1en",_fip, 0, din_print_en, din_get_en, din_set_en, &din1, DI1_ENABLED }, + { "di1","di1po",_fip, 0, din_print_po, din_get_po, din_set_po, &din1, DI1_POLARITY }, + { "di1","di1ac",_fip, 0, din_print_ac, din_get_ac, din_set_ac, &din1, DI1_ACTION }, + // { "di1","di1fn",_fip, 0, din_print_fn, din_get_fn, din_set_fn, &din1, DI1_FUNCTION }, + { "di1","di1in",_fip, 0, din_print_in, din_get_in, din_set_in, &din1, DI1_EXTERNAL_NUMBER }, - { "di2","di2en",_fip, 0, din_print_en, din_get_en, din_set_en, (float *)&din2, DI2_ENABLED }, - { "di2","di2po",_fip, 0, din_print_po, din_get_po, din_set_po, (float *)&din2, DI2_POLARITY }, - { "di2","di2ac",_fip, 0, din_print_ac, din_get_ac, din_set_ac, (float *)&din2, DI2_ACTION }, - // { "di2","di2fn",_fip, 0, din_print_fn, din_get_fn, din_set_fn, (float *)&din2, DI2_FUNCTION }, - { "di2","di2in",_fip, 0, din_print_in, din_get_in, din_set_in, (float *)&din2, DI2_EXTERNAL_NUMBER }, + { "di2","di2en",_fip, 0, din_print_en, din_get_en, din_set_en, &din2, DI2_ENABLED }, + { "di2","di2po",_fip, 0, din_print_po, din_get_po, din_set_po, &din2, DI2_POLARITY }, + { "di2","di2ac",_fip, 0, din_print_ac, din_get_ac, din_set_ac, &din2, DI2_ACTION }, + // { "di2","di2fn",_fip, 0, din_print_fn, din_get_fn, din_set_fn, &din2, DI2_FUNCTION }, + { "di2","di2in",_fip, 0, din_print_in, din_get_in, din_set_in, &din2, DI2_EXTERNAL_NUMBER }, #if (D_IN_CHANNELS >= 3) - { "di3","di3en",_fip, 0, din_print_en, din_get_en, din_set_en, (float *)&din3, DI3_ENABLED }, - { "di3","di3po",_fip, 0, din_print_po, din_get_po, din_set_po, (float *)&din3, DI3_POLARITY }, - { "di3","di3ac",_fip, 0, din_print_ac, din_get_ac, din_set_ac, (float *)&din3, DI3_ACTION }, - // { "di3","di3fn",_fip, 0, din_print_fn, din_get_fn, din_set_fn, (float *)&din3, DI3_FUNCTION }, - { "di3","di3in",_fip, 0, din_print_in, din_get_in, din_set_in, (float *)&din3, DI3_EXTERNAL_NUMBER }, + { "di3","di3en",_fip, 0, din_print_en, din_get_en, din_set_en, &din3, DI3_ENABLED }, + { "di3","di3po",_fip, 0, din_print_po, din_get_po, din_set_po, &din3, DI3_POLARITY }, + { "di3","di3ac",_fip, 0, din_print_ac, din_get_ac, din_set_ac, &din3, DI3_ACTION }, + // { "di3","di3fn",_fip, 0, din_print_fn, din_get_fn, din_set_fn, &din3, DI3_FUNCTION }, + { "di3","di3in",_fip, 0, din_print_in, din_get_in, din_set_in, &din3, DI3_EXTERNAL_NUMBER }, #endif #if (D_IN_CHANNELS >= 4) - { "di4","di4en",_fip, 0, din_print_en, din_get_en, din_set_en, (float *)&din4, DI4_ENABLED }, - { "di4","di4po",_fip, 0, din_print_po, din_get_po, din_set_po, (float *)&din4, DI4_POLARITY }, - { "di4","di4ac",_fip, 0, din_print_ac, din_get_ac, din_set_ac, (float *)&din4, DI4_ACTION }, - // { "di4","di4fn",_fip, 0, din_print_fn, din_get_fn, din_set_fn, (float *)&din4, DI4_FUNCTION }, - { "di4","di4in",_fip, 0, din_print_in, din_get_in, din_set_in, (float *)&din4, DI4_EXTERNAL_NUMBER }, + { "di4","di4en",_fip, 0, din_print_en, din_get_en, din_set_en, &din4, DI4_ENABLED }, + { "di4","di4po",_fip, 0, din_print_po, din_get_po, din_set_po, &din4, DI4_POLARITY }, + { "di4","di4ac",_fip, 0, din_print_ac, din_get_ac, din_set_ac, &din4, DI4_ACTION }, + // { "di4","di4fn",_fip, 0, din_print_fn, din_get_fn, din_set_fn, &din4, DI4_FUNCTION }, + { "di4","di4in",_fip, 0, din_print_in, din_get_in, din_set_in, &din4, DI4_EXTERNAL_NUMBER }, #endif #if (D_IN_CHANNELS >= 5) - { "di5","di5en",_fip, 0, din_print_en, din_get_en, din_set_en, (float *)&din5, DI5_ENABLED }, - { "di5","di5po",_fip, 0, din_print_po, din_get_po, din_set_po, (float *)&din5, DI5_POLARITY }, - { "di5","di5ac",_fip, 0, din_print_ac, din_get_ac, din_set_ac, (float *)&din5, DI5_ACTION }, - // { "di5","di5fn",_fip, 0, din_print_fn, din_get_fn, din_set_fn, (float *)&din5, DI5_FUNCTION }, - { "di5","di5in",_fip, 0, din_print_in, din_get_in, din_set_in, (float *)&din5, DI5_EXTERNAL_NUMBER }, + { "di5","di5en",_fip, 0, din_print_en, din_get_en, din_set_en, &din5, DI5_ENABLED }, + { "di5","di5po",_fip, 0, din_print_po, din_get_po, din_set_po, &din5, DI5_POLARITY }, + { "di5","di5ac",_fip, 0, din_print_ac, din_get_ac, din_set_ac, &din5, DI5_ACTION }, + // { "di5","di5fn",_fip, 0, din_print_fn, din_get_fn, din_set_fn, &din5, DI5_FUNCTION }, + { "di5","di5in",_fip, 0, din_print_in, din_get_in, din_set_in, &din5, DI5_EXTERNAL_NUMBER }, #endif #if (D_IN_CHANNELS >= 6) - { "di6","di6en",_fip, 0, din_print_en, din_get_en, din_set_en, (float *)&din6, DI6_ENABLED }, - { "di6","di6po",_fip, 0, din_print_po, din_get_po, din_set_po, (float *)&din6, DI6_POLARITY }, - { "di6","di6ac",_fip, 0, din_print_ac, din_get_ac, din_set_ac, (float *)&din6, DI6_ACTION }, - // { "di6","di6fn",_fip, 0, din_print_fn, din_get_fn, din_set_fn, (float *)&din6, DI6_FUNCTION }, - { "di6","di6in",_fip, 0, din_print_in, din_get_in, din_set_in, (float *)&din6, DI6_EXTERNAL_NUMBER }, + { "di6","di6en",_fip, 0, din_print_en, din_get_en, din_set_en, &din6, DI6_ENABLED }, + { "di6","di6po",_fip, 0, din_print_po, din_get_po, din_set_po, &din6, DI6_POLARITY }, + { "di6","di6ac",_fip, 0, din_print_ac, din_get_ac, din_set_ac, &din6, DI6_ACTION }, + // { "di6","di6fn",_fip, 0, din_print_fn, din_get_fn, din_set_fn, &din6, DI6_FUNCTION }, + { "di6","di6in",_fip, 0, din_print_in, din_get_in, din_set_in, &din6, DI6_EXTERNAL_NUMBER }, #endif #if (D_IN_CHANNELS >= 7) - { "di7","di7en",_fip, 0, din_print_en, din_get_en, din_set_en, (float *)&din7, DI7_ENABLED }, - { "di7","di7po",_fip, 0, din_print_po, din_get_po, din_set_po, (float *)&din7, DI7_POLARITY }, - { "di7","di7ac",_fip, 0, din_print_ac, din_get_ac, din_set_ac, (float *)&din7, DI7_ACTION }, - // { "di7","di7fn",_fip, 0, din_print_fn, din_get_fn, din_set_fn, (float *)&din7, DI7_FUNCTION }, - { "di7","di7in",_fip, 0, din_print_in, din_get_in, din_set_in, (float *)&din7, DI7_EXTERNAL_NUMBER }, + { "di7","di7en",_fip, 0, din_print_en, din_get_en, din_set_en, &din7, DI7_ENABLED }, + { "di7","di7po",_fip, 0, din_print_po, din_get_po, din_set_po, &din7, DI7_POLARITY }, + { "di7","di7ac",_fip, 0, din_print_ac, din_get_ac, din_set_ac, &din7, DI7_ACTION }, + // { "di7","di7fn",_fip, 0, din_print_fn, din_get_fn, din_set_fn, &din7, DI7_FUNCTION }, + { "di7","di7in",_fip, 0, din_print_in, din_get_in, din_set_in, &din7, DI7_EXTERNAL_NUMBER }, #endif #if (D_IN_CHANNELS >= 8) - { "di8","di8en",_fip, 0, din_print_en, din_get_en, din_set_en, (float *)&din8, DI8_ENABLED }, - { "di3","di8po",_fip, 0, din_print_po, din_get_po, din_set_po, (float *)&din8, DI8_POLARITY }, - { "di8","di8ac",_fip, 0, din_print_ac, din_get_ac, din_set_ac, (float *)&din8, DI8_ACTION }, - // { "di8","di8fn",_fip, 0, din_print_fn, din_get_fn, din_set_fn, (float *)&din8, DI8_FUNCTION }, - { "di8","di8in",_fip, 0, din_print_in, din_get_in, din_set_in, (float *)&din8, DI8_EXTERNAL_NUMBER }, + { "di8","di8en",_fip, 0, din_print_en, din_get_en, din_set_en, &din8, DI8_ENABLED }, + { "di3","di8po",_fip, 0, din_print_po, din_get_po, din_set_po, &din8, DI8_POLARITY }, + { "di8","di8ac",_fip, 0, din_print_ac, din_get_ac, din_set_ac, &din8, DI8_ACTION }, + // { "di8","di8fn",_fip, 0, din_print_fn, din_get_fn, din_set_fn, &din8, DI8_FUNCTION }, + { "di8","di8in",_fip, 0, din_print_in, din_get_in, din_set_in, &din8, DI8_EXTERNAL_NUMBER }, #endif #if (D_IN_CHANNELS >= 9) - { "di9","di9en",_fip, 0, din_print_en, din_get_en, din_set_en, (float *)&din9, DI9_ENABLED }, - { "di9","di9po",_fip, 0, din_print_po, din_get_po, din_set_po, (float *)&din9, DI9_POLARITY }, - { "di9","di9ac",_fip, 0, din_print_ac, din_get_ac, din_set_ac, (float *)&din9, DI9_ACTION }, - // { "di9","di9fn",_fip, 0, din_print_fn, din_get_fn, din_set_fn, (float *)&din9, DI9_FUNCTION }, - { "di9","di9in",_fip, 0, din_print_in, din_get_in, din_set_in, (float *)&din9, DI9_EXTERNAL_NUMBER }, + { "di9","di9en",_fip, 0, din_print_en, din_get_en, din_set_en, &din9, DI9_ENABLED }, + { "di9","di9po",_fip, 0, din_print_po, din_get_po, din_set_po, &din9, DI9_POLARITY }, + { "di9","di9ac",_fip, 0, din_print_ac, din_get_ac, din_set_ac, &din9, DI9_ACTION }, + // { "di9","di9fn",_fip, 0, din_print_fn, din_get_fn, din_set_fn, &din9, DI9_FUNCTION }, + { "di9","di9in",_fip, 0, din_print_in, din_get_in, din_set_in, &din9, DI9_EXTERNAL_NUMBER }, #endif #if (D_IN_CHANNELS >= 10) - { "di10","di10en",_fip, 0, din_print_en, din_get_en, din_set_en, (float *)&din10, DI10_ENABLED }, - { "di10","di10po",_fip, 0, din_print_po, din_get_po, din_set_po, (float *)&din10, DI10_POLARITY }, - { "di10","di10ac",_fip, 0, din_print_ac, din_get_ac, din_set_ac, (float *)&din10, DI10_ACTION }, - // { "di10","di10fn",_fip, 0, din_print_fn, din_get_fn, din_set_fn, (float *)&din10, DI10_FUNCTION }, - { "di10","di10in",_fip, 0, din_print_in, din_get_in, din_set_in, (float *)&din10, DI10_EXTERNAL_NUMBER }, + { "di10","di10en",_fip, 0, din_print_en, din_get_en, din_set_en, &din10, DI10_ENABLED }, + { "di10","di10po",_fip, 0, din_print_po, din_get_po, din_set_po, &din10, DI10_POLARITY }, + { "di10","di10ac",_fip, 0, din_print_ac, din_get_ac, din_set_ac, &din10, DI10_ACTION }, + // { "di10","di10fn",_fip, 0, din_print_fn, din_get_fn, din_set_fn, &din10, DI10_FUNCTION }, + { "di10","di10in",_fip, 0, din_print_in, din_get_in, din_set_in, &din10, DI10_EXTERNAL_NUMBER }, #endif #if (D_IN_CHANNELS >= 11) - { "di11","di11en",_fip, 0, din_print_en, din_get_en, din_set_en, (float *)&din11, DI11_ENABLED }, - { "di11","di11po",_fip, 0, din_print_po, din_get_po, din_set_po, (float *)&din11, DI11_POLARITY }, - { "di11","di11ac",_fip, 0, din_print_ac, din_get_ac, din_set_ac, (float *)&din11, DI11_ACTION }, - // { "di11","di11fn",_fip, 0, din_print_fn, din_get_fn, din_set_fn, (float *)&din11, DI11_FUNCTION }, - { "di11","di11in",_fip, 0, din_print_in, din_get_in, din_set_in, (float *)&din11, DI11_EXTERNAL_NUMBER }, + { "di11","di11en",_fip, 0, din_print_en, din_get_en, din_set_en, &din11, DI11_ENABLED }, + { "di11","di11po",_fip, 0, din_print_po, din_get_po, din_set_po, &din11, DI11_POLARITY }, + { "di11","di11ac",_fip, 0, din_print_ac, din_get_ac, din_set_ac, &din11, DI11_ACTION }, + // { "di11","di11fn",_fip, 0, din_print_fn, din_get_fn, din_set_fn, &din11, DI11_FUNCTION }, + { "di11","di11in",_fip, 0, din_print_in, din_get_in, din_set_in, &din11, DI11_EXTERNAL_NUMBER }, #endif #if (D_IN_CHANNELS >= 12) - { "di12","di12en",_fip, 0, din_print_en, din_get_en, din_set_en, (float *)&din12, DI12_ENABLED }, - { "di12","di12po",_fip, 0, din_print_po, din_get_po, din_set_po, (float *)&din12, DI12_POLARITY }, - { "di12","di12ac",_fip, 0, din_print_ac, din_get_ac, din_set_ac, (float *)&din12, DI12_ACTION }, - // { "di12","di12fn",_fip, 0, din_print_fn, din_get_fn, din_set_fn, (float *)&din12, DI12_FUNCTION }, - { "di12","di12in",_fip, 0, din_print_in, din_get_in, din_set_in, (float *)&din12, DI12_EXTERNAL_NUMBER }, + { "di12","di12en",_fip, 0, din_print_en, din_get_en, din_set_en, &din12, DI12_ENABLED }, + { "di12","di12po",_fip, 0, din_print_po, din_get_po, din_set_po, &din12, DI12_POLARITY }, + { "di12","di12ac",_fip, 0, din_print_ac, din_get_ac, din_set_ac, &din12, DI12_ACTION }, + // { "di12","di12fn",_fip, 0, din_print_fn, din_get_fn, din_set_fn, &din12, DI12_FUNCTION }, + { "di12","di12in",_fip, 0, din_print_in, din_get_in, din_set_in, &din12, DI12_EXTERNAL_NUMBER }, #endif // Digital input state readers - { "in","in1", _f0, 0, din_print_state, din_get_input, set_ro, (float *)&in1, 0 }, - { "in","in2", _f0, 0, din_print_state, din_get_input, set_ro, (float *)&in2, 0 }, - { "in","in3", _f0, 0, din_print_state, din_get_input, set_ro, (float *)&in3, 0 }, - { "in","in4", _f0, 0, din_print_state, din_get_input, set_ro, (float *)&in4, 0 }, - { "in","in5", _f0, 0, din_print_state, din_get_input, set_ro, (float *)&in5, 0 }, - { "in","in6", _f0, 0, din_print_state, din_get_input, set_ro, (float *)&in6, 0 }, - { "in","in7", _f0, 0, din_print_state, din_get_input, set_ro, (float *)&in7, 0 }, - { "in","in8", _f0, 0, din_print_state, din_get_input, set_ro, (float *)&in8, 0 }, - { "in","in9", _f0, 0, din_print_state, din_get_input, set_ro, (float *)&in9, 0 }, - { "in","in10", _f0, 0, din_print_state, din_get_input, set_ro, (float *)&in10, 0 }, - { "in","in11", _f0, 0, din_print_state, din_get_input, set_ro, (float *)&in11, 0 }, - { "in","in12", _f0, 0, din_print_state, din_get_input, set_ro, (float *)&in12, 0 }, - { "in","in13", _f0, 0, din_print_state, din_get_input, set_ro, (float *)&in13, 0 }, - { "in","in14", _f0, 0, din_print_state, din_get_input, set_ro, (float *)&in14, 0 }, - { "in","in15", _f0, 0, din_print_state, din_get_input, set_ro, (float *)&in15, 0 }, - { "in","in16", _f0, 0, din_print_state, din_get_input, set_ro, (float *)&in16, 0 }, + { "in","in1", _f0, 0, din_print_state, din_get_input, set_ro, &in1, 0 }, + { "in","in2", _f0, 0, din_print_state, din_get_input, set_ro, &in2, 0 }, + { "in","in3", _f0, 0, din_print_state, din_get_input, set_ro, &in3, 0 }, + { "in","in4", _f0, 0, din_print_state, din_get_input, set_ro, &in4, 0 }, + { "in","in5", _f0, 0, din_print_state, din_get_input, set_ro, &in5, 0 }, + { "in","in6", _f0, 0, din_print_state, din_get_input, set_ro, &in6, 0 }, + { "in","in7", _f0, 0, din_print_state, din_get_input, set_ro, &in7, 0 }, + { "in","in8", _f0, 0, din_print_state, din_get_input, set_ro, &in8, 0 }, + { "in","in9", _f0, 0, din_print_state, din_get_input, set_ro, &in9, 0 }, + { "in","in10", _f0, 0, din_print_state, din_get_input, set_ro, &in10, 0 }, + { "in","in11", _f0, 0, din_print_state, din_get_input, set_ro, &in11, 0 }, + { "in","in12", _f0, 0, din_print_state, din_get_input, set_ro, &in12, 0 }, + { "in","in13", _f0, 0, din_print_state, din_get_input, set_ro, &in13, 0 }, + { "in","in14", _f0, 0, din_print_state, din_get_input, set_ro, &in14, 0 }, + { "in","in15", _f0, 0, din_print_state, din_get_input, set_ro, &in15, 0 }, + { "in","in16", _f0, 0, din_print_state, din_get_input, set_ro, &in16, 0 }, // digital output configs - { "do1", "do1en", _fip, 0, dout_print_en, dout_get_en, dout_set_en, (float *)&dout1, DO1_ENABLED }, - { "do1", "do1po", _fip, 0, dout_print_po, dout_get_po, dout_set_po, (float *)&dout1, DO1_POLARITY }, - { "do1", "do1out",_fip, 0, dout_print_out, dout_get_out, dout_set_out, (float *)&dout1, DO1_EXTERNAL_NUMBER }, + { "do1", "do1en", _fip, 0, dout_print_en, dout_get_en, dout_set_en, &dout1, DO1_ENABLED }, + { "do1", "do1po", _fip, 0, dout_print_po, dout_get_po, dout_set_po, &dout1, DO1_POLARITY }, + { "do1", "do1out",_fip, 0, dout_print_out, dout_get_out, dout_set_out, &dout1, DO1_EXTERNAL_NUMBER }, #if (D_OUT_CHANNELS >= 2) - { "do2", "do2en", _fip, 0, dout_print_en, dout_get_en, dout_set_en, (float *)&dout2, DO2_ENABLED }, - { "do2", "do2po", _fip, 0, dout_print_po, dout_get_po, dout_set_po, (float *)&dout2, DO2_POLARITY }, - { "do2", "do2out",_fip, 0, dout_print_out, dout_get_out, dout_set_out, (float *)&dout2, DO2_EXTERNAL_NUMBER }, + { "do2", "do2en", _fip, 0, dout_print_en, dout_get_en, dout_set_en, &dout2, DO2_ENABLED }, + { "do2", "do2po", _fip, 0, dout_print_po, dout_get_po, dout_set_po, &dout2, DO2_POLARITY }, + { "do2", "do2out",_fip, 0, dout_print_out, dout_get_out, dout_set_out, &dout2, DO2_EXTERNAL_NUMBER }, #endif #if (D_OUT_CHANNELS >= 3) - { "do3", "do3en", _fip, 0, dout_print_en, dout_get_en, dout_set_en, (float *)&dout3, DO3_ENABLED }, - { "do3", "do3po", _fip, 0, dout_print_po, dout_get_po, dout_set_po, (float *)&dout3, DO3_POLARITY }, - { "do3", "do3out",_fip, 0, dout_print_out, dout_get_out, dout_set_out, (float *)&dout3, DO3_EXTERNAL_NUMBER }, + { "do3", "do3en", _fip, 0, dout_print_en, dout_get_en, dout_set_en, &dout3, DO3_ENABLED }, + { "do3", "do3po", _fip, 0, dout_print_po, dout_get_po, dout_set_po, &dout3, DO3_POLARITY }, + { "do3", "do3out",_fip, 0, dout_print_out, dout_get_out, dout_set_out, &dout3, DO3_EXTERNAL_NUMBER }, #endif #if (D_OUT_CHANNELS >= 4) - { "do4", "do4en", _fip, 0, dout_print_en, dout_get_en, dout_set_en, (float *)&dout4, DO4_ENABLED }, - { "do4", "do4po", _fip, 0, dout_print_po, dout_get_po, dout_set_po, (float *)&dout4, DO4_POLARITY }, - { "do4", "do4out",_fip, 0, dout_print_out, dout_get_out, dout_set_out, (float *)&dout4, DO4_EXTERNAL_NUMBER }, + { "do4", "do4en", _fip, 0, dout_print_en, dout_get_en, dout_set_en, &dout4, DO4_ENABLED }, + { "do4", "do4po", _fip, 0, dout_print_po, dout_get_po, dout_set_po, &dout4, DO4_POLARITY }, + { "do4", "do4out",_fip, 0, dout_print_out, dout_get_out, dout_set_out, &dout4, DO4_EXTERNAL_NUMBER }, #endif #if (D_OUT_CHANNELS >= 5) - { "do5", "do5en", _fip, 0, dout_print_en, dout_get_en, dout_set_en, (float *)&dout5, DO5_ENABLED }, - { "do5", "do5po", _fip, 0, dout_print_po, dout_get_po, dout_set_po, (float *)&dout5, DO5_POLARITY }, - { "do5", "do5out",_fip, 0, dout_print_out, dout_get_out, dout_set_out, (float *)&dout5, DO5_EXTERNAL_NUMBER }, + { "do5", "do5en", _fip, 0, dout_print_en, dout_get_en, dout_set_en, &dout5, DO5_ENABLED }, + { "do5", "do5po", _fip, 0, dout_print_po, dout_get_po, dout_set_po, &dout5, DO5_POLARITY }, + { "do5", "do5out",_fip, 0, dout_print_out, dout_get_out, dout_set_out, &dout5, DO5_EXTERNAL_NUMBER }, #endif #if (D_OUT_CHANNELS >= 6) - { "do6", "do6en", _fip, 0, dout_print_en, dout_get_en, dout_set_en, (float *)&dout6, DO6_ENABLED }, - { "do6", "do6po", _fip, 0, dout_print_po, dout_get_po, dout_set_po, (float *)&dout6, DO6_POLARITY }, - { "do6", "do6out",_fip, 0, dout_print_out, dout_get_out, dout_set_out, (float *)&dout6, DO6_EXTERNAL_NUMBER }, + { "do6", "do6en", _fip, 0, dout_print_en, dout_get_en, dout_set_en, &dout6, DO6_ENABLED }, + { "do6", "do6po", _fip, 0, dout_print_po, dout_get_po, dout_set_po, &dout6, DO6_POLARITY }, + { "do6", "do6out",_fip, 0, dout_print_out, dout_get_out, dout_set_out, &dout6, DO6_EXTERNAL_NUMBER }, #endif #if (D_OUT_CHANNELS >= 7) - { "do7", "do7en", _fip, 0, dout_print_en, dout_get_en, dout_set_en, (float *)&dout7, DO7_ENABLED }, - { "do7", "do7po", _fip, 0, dout_print_po, dout_get_po, dout_set_po, (float *)&dout7, DO7_POLARITY }, - { "do7", "do7out",_fip, 0, dout_print_out, dout_get_out, dout_set_out, (float *)&dout7, DO7_EXTERNAL_NUMBER }, + { "do7", "do7en", _fip, 0, dout_print_en, dout_get_en, dout_set_en, &dout7, DO7_ENABLED }, + { "do7", "do7po", _fip, 0, dout_print_po, dout_get_po, dout_set_po, &dout7, DO7_POLARITY }, + { "do7", "do7out",_fip, 0, dout_print_out, dout_get_out, dout_set_out, &dout7, DO7_EXTERNAL_NUMBER }, #endif #if (D_OUT_CHANNELS >= 8) - { "do8", "do8en", _fip, 0, dout_print_en, dout_get_en, dout_set_en, (float *)&dout8, DO8_ENABLED }, - { "do8", "do8po", _fip, 0, dout_print_po, dout_get_po, dout_set_po, (float *)&dout8, DO8_POLARITY }, - { "do8", "do8out",_fip, 0, dout_print_out, dout_get_out, dout_set_out, (float *)&dout8, DO8_EXTERNAL_NUMBER }, + { "do8", "do8en", _fip, 0, dout_print_en, dout_get_en, dout_set_en, &dout8, DO8_ENABLED }, + { "do8", "do8po", _fip, 0, dout_print_po, dout_get_po, dout_set_po, &dout8, DO8_POLARITY }, + { "do8", "do8out",_fip, 0, dout_print_out, dout_get_out, dout_set_out, &dout8, DO8_EXTERNAL_NUMBER }, #endif #if (D_OUT_CHANNELS >= 9) - { "do9", "do9en", _fip, 0, dout_print_en, dout_get_en, dout_set_en, (float *)&dout9, DO9_ENABLED }, - { "do9", "do9po", _fip, 0, dout_print_po, dout_get_po, dout_set_po, (float *)&dout9, DO9_POLARITY }, - { "do9", "do9out",_fip, 0, dout_print_out, dout_get_out, dout_set_out, (float *)&dout9, DO9_EXTERNAL_NUMBER }, + { "do9", "do9en", _fip, 0, dout_print_en, dout_get_en, dout_set_en, &dout9, DO9_ENABLED }, + { "do9", "do9po", _fip, 0, dout_print_po, dout_get_po, dout_set_po, &dout9, DO9_POLARITY }, + { "do9", "do9out",_fip, 0, dout_print_out, dout_get_out, dout_set_out, &dout9, DO9_EXTERNAL_NUMBER }, #endif #if (D_OUT_CHANNELS >= 10) - { "do10", "do10en", _fip, 0, dout_print_en, dout_get_en, dout_set_en, (float *)&dout10, DO10_ENABLED }, - { "do10", "do10po", _fip, 0, dout_print_po, dout_get_po, dout_set_po, (float *)&dout10, DO10_POLARITY }, - { "do10", "do10out",_fip, 0, dout_print_out, dout_get_out, dout_set_out, (float *)&dout10, DO10_EXTERNAL_NUMBER }, + { "do10", "do10en", _fip, 0, dout_print_en, dout_get_en, dout_set_en, &dout10, DO10_ENABLED }, + { "do10", "do10po", _fip, 0, dout_print_po, dout_get_po, dout_set_po, &dout10, DO10_POLARITY }, + { "do10", "do10out",_fip, 0, dout_print_out, dout_get_out, dout_set_out, &dout10, DO10_EXTERNAL_NUMBER }, #endif #if (D_OUT_CHANNELS >= 11) - { "do11", "do11en", _fip, 0, dout_print_en, dout_get_en, dout_set_en, (float *)&dout11, DO11_ENABLED }, - { "do11", "do11po", _fip, 0, dout_print_po, dout_get_po, dout_set_po, (float *)&dout11, DO11_POLARITY }, - { "do11", "do11out",_fip, 0, dout_print_out, dout_get_out, dout_set_out, (float *)&dout11, DO11_EXTERNAL_NUMBER }, + { "do11", "do11en", _fip, 0, dout_print_en, dout_get_en, dout_set_en, &dout11, DO11_ENABLED }, + { "do11", "do11po", _fip, 0, dout_print_po, dout_get_po, dout_set_po, &dout11, DO11_POLARITY }, + { "do11", "do11out",_fip, 0, dout_print_out, dout_get_out, dout_set_out, &dout11, DO11_EXTERNAL_NUMBER }, #endif #if (D_OUT_CHANNELS >= 12) - { "do12", "do12en", _fip, 0, dout_print_en, dout_get_en, dout_set_en, (float *)&dout12, DO12_ENABLED }, - { "do12", "do12po", _fip, 0, dout_print_po, dout_get_po, dout_set_po, (float *)&dout12, DO12_POLARITY }, - { "do12", "do12out",_fip, 0, dout_print_out, dout_get_out, dout_set_out, (float *)&dout12, DO12_EXTERNAL_NUMBER }, + { "do12", "do12en", _fip, 0, dout_print_en, dout_get_en, dout_set_en, &dout12, DO12_ENABLED }, + { "do12", "do12po", _fip, 0, dout_print_po, dout_get_po, dout_set_po, &dout12, DO12_POLARITY }, + { "do12", "do12out",_fip, 0, dout_print_out, dout_get_out, dout_set_out, &dout12, DO12_EXTERNAL_NUMBER }, #endif #if (D_OUT_CHANNELS >= 13) - { "do13", "do13en", _fip, 0, dout_print_en, dout_get_en, dout_set_en, (float *)&dout13, DO13_ENABLED }, - { "do13", "do13po", _fip, 0, dout_print_po, dout_get_po, dout_set_po, (float *)&dout13, DO13_POLARITY }, - { "do13", "do13out",_fip, 0, dout_print_out, dout_get_out, dout_set_out, (float *)&dout13, DO13_EXTERNAL_NUMBER }, + { "do13", "do13en", _fip, 0, dout_print_en, dout_get_en, dout_set_en, &dout13, DO13_ENABLED }, + { "do13", "do13po", _fip, 0, dout_print_po, dout_get_po, dout_set_po, &dout13, DO13_POLARITY }, + { "do13", "do13out",_fip, 0, dout_print_out, dout_get_out, dout_set_out, &dout13, DO13_EXTERNAL_NUMBER }, #endif #if (D_OUT_CHANNELS >= 14) - { "do14", "do14en", _fip, 0, dout_print_en, dout_get_en, dout_set_en, (float *)&dout14, DO14_ENABLED }, - { "do14", "do14po", _fip, 0, dout_print_po, dout_get_po, dout_set_po, (float *)&dout14, DO14_POLARITY }, - { "do14", "do14out",_fip, 0, dout_print_out, dout_get_out, dout_set_out, (float *)&dout14, DO14_EXTERNAL_NUMBER }, + { "do14", "do14en", _fip, 0, dout_print_en, dout_get_en, dout_set_en, &dout14, DO14_ENABLED }, + { "do14", "do14po", _fip, 0, dout_print_po, dout_get_po, dout_set_po, &dout14, DO14_POLARITY }, + { "do14", "do14out",_fip, 0, dout_print_out, dout_get_out, dout_set_out, &dout14, DO14_EXTERNAL_NUMBER }, #endif // Digital output state readers (default to non-active) - { "out","out1", _f0, 2, dout_print_out, dout_get_output, dout_set_output, (float *)&out1, 0 }, - { "out","out2", _f0, 2, dout_print_out, dout_get_output, dout_set_output, (float *)&out2, 0 }, - { "out","out3", _f0, 2, dout_print_out, dout_get_output, dout_set_output, (float *)&out3, 0 }, - { "out","out4", _f0, 2, dout_print_out, dout_get_output, dout_set_output, (float *)&out4, 0 }, - { "out","out5", _f0, 2, dout_print_out, dout_get_output, dout_set_output, (float *)&out5, 0 }, - { "out","out6", _f0, 2, dout_print_out, dout_get_output, dout_set_output, (float *)&out6, 0 }, - { "out","out7", _f0, 2, dout_print_out, dout_get_output, dout_set_output, (float *)&out7, 0 }, - { "out","out8", _f0, 2, dout_print_out, dout_get_output, dout_set_output, (float *)&out8, 0 }, - { "out","out9", _f0, 2, dout_print_out, dout_get_output, dout_set_output, (float *)&out9, 0 }, - { "out","out10", _f0, 2, dout_print_out, dout_get_output, dout_set_output, (float *)&out10, 0 }, - { "out","out11", _f0, 2, dout_print_out, dout_get_output, dout_set_output, (float *)&out11, 0 }, - { "out","out12", _f0, 2, dout_print_out, dout_get_output, dout_set_output, (float *)&out12, 0 }, - { "out","out13", _f0, 2, dout_print_out, dout_get_output, dout_set_output, (float *)&out13, 0 }, - { "out","out14", _f0, 2, dout_print_out, dout_get_output, dout_set_output, (float *)&out14, 0 }, - { "out","out15", _f0, 2, dout_print_out, dout_get_output, dout_set_output, (float *)&out15, 0 }, - { "out","out16", _f0, 2, dout_print_out, dout_get_output, dout_set_output, (float *)&out16, 0 }, + { "out","out1", _f0, 2, dout_print_out, dout_get_output, dout_set_output, &out1, 0 }, + { "out","out2", _f0, 2, dout_print_out, dout_get_output, dout_set_output, &out2, 0 }, + { "out","out3", _f0, 2, dout_print_out, dout_get_output, dout_set_output, &out3, 0 }, + { "out","out4", _f0, 2, dout_print_out, dout_get_output, dout_set_output, &out4, 0 }, + { "out","out5", _f0, 2, dout_print_out, dout_get_output, dout_set_output, &out5, 0 }, + { "out","out6", _f0, 2, dout_print_out, dout_get_output, dout_set_output, &out6, 0 }, + { "out","out7", _f0, 2, dout_print_out, dout_get_output, dout_set_output, &out7, 0 }, + { "out","out8", _f0, 2, dout_print_out, dout_get_output, dout_set_output, &out8, 0 }, + { "out","out9", _f0, 2, dout_print_out, dout_get_output, dout_set_output, &out9, 0 }, + { "out","out10", _f0, 2, dout_print_out, dout_get_output, dout_set_output, &out10, 0 }, + { "out","out11", _f0, 2, dout_print_out, dout_get_output, dout_set_output, &out11, 0 }, + { "out","out12", _f0, 2, dout_print_out, dout_get_output, dout_set_output, &out12, 0 }, + { "out","out13", _f0, 2, dout_print_out, dout_get_output, dout_set_output, &out13, 0 }, + { "out","out14", _f0, 2, dout_print_out, dout_get_output, dout_set_output, &out14, 0 }, + { "out","out15", _f0, 2, dout_print_out, dout_get_output, dout_set_output, &out15, 0 }, + { "out","out16", _f0, 2, dout_print_out, dout_get_output, dout_set_output, &out16, 0 }, // Analog input configs #if (A_IN_CHANNELS >= 1) - { "ai1","ai1en",_fip, 0, ai_print_en, ai_get_en, ai_set_en, (float *)&ai1, AI1_ENABLED }, - { "ai1","ai1ain",_fip,0, ai_print_ain, ai_get_ain, ai_set_ain, (float *)&ai1, AI1_EXTERNAL_NUMBER }, - { "ai1","ai1ty",_fip, 0, ai_print_type, ai_get_type, ai_set_type, (float *)&ai1, AI1_TYPE }, - { "ai1","ai1ct",_fip, 0, ai_print_circuit, ai_get_circuit, ai_set_circuit, (float *)&ai1, AI1_CIRCUIT }, - { "ai1","ai1p1",_fip, 4, ai_print_p, ai_get_p1, ai_set_p1, (float *)&ai1, AI1_P1 }, - { "ai1","ai1p2",_fip, 4, ai_print_p, ai_get_p2, ai_set_p2, (float *)&ai1, AI1_P2 }, - { "ai1","ai1p3",_fip, 4, ai_print_p, ai_get_p3, ai_set_p3, (float *)&ai1, AI1_P3 }, - { "ai1","ai1p4",_fip, 4, ai_print_p, ai_get_p4, ai_set_p4, (float *)&ai1, AI1_P4 }, - { "ai1","ai1p5",_fip, 4, ai_print_p, ai_get_p5, ai_set_p5, (float *)&ai1, AI1_P5 }, + { "ai1","ai1en",_fip, 0, ai_print_en, ai_get_en, ai_set_en, &ai1, AI1_ENABLED }, + { "ai1","ai1ain",_fip,0, ai_print_ain, ai_get_ain, ai_set_ain, &ai1, AI1_EXTERNAL_NUMBER }, + { "ai1","ai1ty",_fip, 0, ai_print_type, ai_get_type, ai_set_type, &ai1, AI1_TYPE }, + { "ai1","ai1ct",_fip, 0, ai_print_circuit, ai_get_circuit, ai_set_circuit, &ai1, AI1_CIRCUIT }, + { "ai1","ai1p1",_fip, 4, ai_print_p, ai_get_p1, ai_set_p1, &ai1, AI1_P1 }, + { "ai1","ai1p2",_fip, 4, ai_print_p, ai_get_p2, ai_set_p2, &ai1, AI1_P2 }, + { "ai1","ai1p3",_fip, 4, ai_print_p, ai_get_p3, ai_set_p3, &ai1, AI1_P3 }, + { "ai1","ai1p4",_fip, 4, ai_print_p, ai_get_p4, ai_set_p4, &ai1, AI1_P4 }, + { "ai1","ai1p5",_fip, 4, ai_print_p, ai_get_p5, ai_set_p5, &ai1, AI1_P5 }, #endif #if (A_IN_CHANNELS >= 2) - { "ai2","ai2en",_fip, 0, ai_print_en, ai_get_en, ai_set_en, (float *)&ai2, AI2_ENABLED }, - { "ai2","ai2ain",_fip,0, ai_print_ain, ai_get_ain, ai_set_ain, (float *)&ai2, AI2_EXTERNAL_NUMBER }, - { "ai2","ai2ty",_fip, 0, ai_print_type, ai_get_type, ai_set_type, (float *)&ai2, AI2_TYPE }, - { "ai2","ai2ct",_fip, 0, ai_print_circuit, ai_get_circuit, ai_set_circuit, (float *)&ai2, AI2_CIRCUIT }, - { "ai2","ai2p1",_fip, 4, ai_print_p, ai_get_p1, ai_set_p1, (float *)&ai2, AI2_P1 }, - { "ai2","ai2p2",_fip, 4, ai_print_p, ai_get_p2, ai_set_p2, (float *)&ai2, AI2_P2 }, - { "ai2","ai2p3",_fip, 4, ai_print_p, ai_get_p3, ai_set_p3, (float *)&ai2, AI2_P3 }, - { "ai2","ai2p4",_fip, 4, ai_print_p, ai_get_p4, ai_set_p4, (float *)&ai2, AI2_P4 }, - { "ai2","ai2p5",_fip, 4, ai_print_p, ai_get_p5, ai_set_p5, (float *)&ai2, AI2_P5 }, + { "ai2","ai2en",_fip, 0, ai_print_en, ai_get_en, ai_set_en, &ai2, AI2_ENABLED }, + { "ai2","ai2ain",_fip,0, ai_print_ain, ai_get_ain, ai_set_ain, &ai2, AI2_EXTERNAL_NUMBER }, + { "ai2","ai2ty",_fip, 0, ai_print_type, ai_get_type, ai_set_type, &ai2, AI2_TYPE }, + { "ai2","ai2ct",_fip, 0, ai_print_circuit, ai_get_circuit, ai_set_circuit, &ai2, AI2_CIRCUIT }, + { "ai2","ai2p1",_fip, 4, ai_print_p, ai_get_p1, ai_set_p1, &ai2, AI2_P1 }, + { "ai2","ai2p2",_fip, 4, ai_print_p, ai_get_p2, ai_set_p2, &ai2, AI2_P2 }, + { "ai2","ai2p3",_fip, 4, ai_print_p, ai_get_p3, ai_set_p3, &ai2, AI2_P3 }, + { "ai2","ai2p4",_fip, 4, ai_print_p, ai_get_p4, ai_set_p4, &ai2, AI2_P4 }, + { "ai2","ai2p5",_fip, 4, ai_print_p, ai_get_p5, ai_set_p5, &ai2, AI2_P5 }, #endif #if (A_IN_CHANNELS >= 3) - { "ai3","ai3en",_fip, 0, ai_print_en, ai_get_en, ai_set_en, (float *)&ai3, AI3_ENABLED }, - { "ai3","ai3ain",_fip,0, ai_print_ain, ai_get_ain, ai_set_ain, (float *)&ai3, AI3_EXTERNAL_NUMBER }, - { "ai3","ai3ty",_fip, 0, ai_print_type, ai_get_type, ai_set_type, (float *)&ai3, AI3_TYPE }, - { "ai3","ai3ct",_fip, 0, ai_print_circuit, ai_get_circuit, ai_set_circuit, (float *)&ai3, AI3_CIRCUIT }, - { "ai3","ai3p1",_fip, 4, ai_print_p, ai_get_p1, ai_set_p1, (float *)&ai3, AI3_P1 }, - { "ai3","ai3p2",_fip, 4, ai_print_p, ai_get_p2, ai_set_p2, (float *)&ai3, AI3_P2 }, - { "ai3","ai3p3",_fip, 4, ai_print_p, ai_get_p3, ai_set_p3, (float *)&ai3, AI3_P3 }, - { "ai3","ai3p4",_fip, 4, ai_print_p, ai_get_p4, ai_set_p4, (float *)&ai3, AI3_P4 }, - { "ai3","ai3p5",_fip, 4, ai_print_p, ai_get_p5, ai_set_p5, (float *)&ai3, AI3_P5 }, + { "ai3","ai3en",_fip, 0, ai_print_en, ai_get_en, ai_set_en, &ai3, AI3_ENABLED }, + { "ai3","ai3ain",_fip,0, ai_print_ain, ai_get_ain, ai_set_ain, &ai3, AI3_EXTERNAL_NUMBER }, + { "ai3","ai3ty",_fip, 0, ai_print_type, ai_get_type, ai_set_type, &ai3, AI3_TYPE }, + { "ai3","ai3ct",_fip, 0, ai_print_circuit, ai_get_circuit, ai_set_circuit, &ai3, AI3_CIRCUIT }, + { "ai3","ai3p1",_fip, 4, ai_print_p, ai_get_p1, ai_set_p1, &ai3, AI3_P1 }, + { "ai3","ai3p2",_fip, 4, ai_print_p, ai_get_p2, ai_set_p2, &ai3, AI3_P2 }, + { "ai3","ai3p3",_fip, 4, ai_print_p, ai_get_p3, ai_set_p3, &ai3, AI3_P3 }, + { "ai3","ai3p4",_fip, 4, ai_print_p, ai_get_p4, ai_set_p4, &ai3, AI3_P4 }, + { "ai3","ai3p5",_fip, 4, ai_print_p, ai_get_p5, ai_set_p5, &ai3, AI3_P5 }, #endif #if (A_IN_CHANNELS >= 4) - { "ai4","ai4en",_fip, 0, ai_print_en, ai_get_en, ai_set_en, (float *)&ai4, AI4_ENABLED }, - { "ai4","ai4ain",_fip,0, ai_print_ain, ai_get_ain, ai_set_ain, (float *)&ai4, AI4_EXTERNAL_NUMBER }, - { "ai4","ai4ty",_fip, 0, ai_print_type, ai_get_type, ai_set_type, (float *)&ai4, AI4_TYPE }, - { "ai4","ai4ct",_fip, 0, ai_print_circuit, ai_get_circuit, ai_set_circuit, (float *)&ai4, AI4_CIRCUIT }, - { "ai4","ai4p1",_fip, 4, ai_print_p, ai_get_p1, ai_set_p1, (float *)&ai4, AI4_P1 }, - { "ai4","ai4p2",_fip, 4, ai_print_p, ai_get_p2, ai_set_p2, (float *)&ai4, AI4_P2 }, - { "ai4","ai4p3",_fip, 4, ai_print_p, ai_get_p3, ai_set_p3, (float *)&ai4, AI4_P3 }, - { "ai4","ai4p4",_fip, 4, ai_print_p, ai_get_p4, ai_set_p4, (float *)&ai4, AI4_P4 }, - { "ai4","ai4p5",_fip, 4, ai_print_p, ai_get_p5, ai_set_p5, (float *)&ai4, AI4_P5 }, + { "ai4","ai4en",_fip, 0, ai_print_en, ai_get_en, ai_set_en, &ai4, AI4_ENABLED }, + { "ai4","ai4ain",_fip,0, ai_print_ain, ai_get_ain, ai_set_ain, &ai4, AI4_EXTERNAL_NUMBER }, + { "ai4","ai4ty",_fip, 0, ai_print_type, ai_get_type, ai_set_type, &ai4, AI4_TYPE }, + { "ai4","ai4ct",_fip, 0, ai_print_circuit, ai_get_circuit, ai_set_circuit, &ai4, AI4_CIRCUIT }, + { "ai4","ai4p1",_fip, 4, ai_print_p, ai_get_p1, ai_set_p1, &ai4, AI4_P1 }, + { "ai4","ai4p2",_fip, 4, ai_print_p, ai_get_p2, ai_set_p2, &ai4, AI4_P2 }, + { "ai4","ai4p3",_fip, 4, ai_print_p, ai_get_p3, ai_set_p3, &ai4, AI4_P3 }, + { "ai4","ai4p4",_fip, 4, ai_print_p, ai_get_p4, ai_set_p4, &ai4, AI4_P4 }, + { "ai4","ai4p5",_fip, 4, ai_print_p, ai_get_p5, ai_set_p5, &ai4, AI4_P5 }, #endif - { "ain1","ain1vv",_f0, 4, ain_print_value, ain_get_value, set_ro, (float *)&ain1, 0 }, - { "ain1","ain1rv",_f0, 2, ain_print_resistance, ain_get_resistance, set_ro, (float *)&ain1, 0 }, - { "ain2","ain2vv",_f0, 4, ain_print_value, ain_get_value, set_ro, (float *)&ain2, 0 }, - { "ain2","ain2rv",_f0, 2, ain_print_resistance, ain_get_resistance, set_ro, (float *)&ain2, 0 }, - { "ain3","ain3vv",_f0, 4, ain_print_value, ain_get_value, set_ro, (float *)&ain3, 0 }, - { "ain3","ain3rv",_f0, 2, ain_print_resistance, ain_get_resistance, set_ro, (float *)&ain3, 0 }, - { "ain4","ain4vv",_f0, 4, ain_print_value, ain_get_value, set_ro, (float *)&ain4, 0 }, - { "ain4","ain4rv",_f0, 2, ain_print_resistance, ain_get_resistance, set_ro, (float *)&ain4, 0 }, - { "ain5","ain5vv",_f0, 4, ain_print_value, ain_get_value, set_ro, (float *)&ain5, 0 }, - { "ain5","ain5rv",_f0, 2, ain_print_resistance, ain_get_resistance, set_ro, (float *)&ain5, 0 }, - { "ain6","ain6vv",_f0, 4, ain_print_value, ain_get_value, set_ro, (float *)&ain6, 0 }, - { "ain6","ain6rv",_f0, 2, ain_print_resistance, ain_get_resistance, set_ro, (float *)&ain6, 0 }, - { "ain7","ain7vv",_f0, 4, ain_print_value, ain_get_value, set_ro, (float *)&ain7, 0 }, - { "ain7","ain7rv",_f0, 2, ain_print_resistance, ain_get_resistance, set_ro, (float *)&ain7, 0 }, - { "ain8","ain8vv",_f0, 4, ain_print_value, ain_get_value, set_ro, (float *)&ain8, 0 }, - { "ain8","ain8rv",_f0, 2, ain_print_resistance, ain_get_resistance, set_ro, (float *)&ain8, 0 }, + { "ain1","ain1vv",_f0, 4, ain_print_value, ain_get_value, set_ro, &ain1, 0 }, + { "ain1","ain1rv",_f0, 2, ain_print_resistance, ain_get_resistance, set_ro, &ain1, 0 }, + { "ain2","ain2vv",_f0, 4, ain_print_value, ain_get_value, set_ro, &ain2, 0 }, + { "ain2","ain2rv",_f0, 2, ain_print_resistance, ain_get_resistance, set_ro, &ain2, 0 }, + { "ain3","ain3vv",_f0, 4, ain_print_value, ain_get_value, set_ro, &ain3, 0 }, + { "ain3","ain3rv",_f0, 2, ain_print_resistance, ain_get_resistance, set_ro, &ain3, 0 }, + { "ain4","ain4vv",_f0, 4, ain_print_value, ain_get_value, set_ro, &ain4, 0 }, + { "ain4","ain4rv",_f0, 2, ain_print_resistance, ain_get_resistance, set_ro, &ain4, 0 }, + { "ain5","ain5vv",_f0, 4, ain_print_value, ain_get_value, set_ro, &ain5, 0 }, + { "ain5","ain5rv",_f0, 2, ain_print_resistance, ain_get_resistance, set_ro, &ain5, 0 }, + { "ain6","ain6vv",_f0, 4, ain_print_value, ain_get_value, set_ro, &ain6, 0 }, + { "ain6","ain6rv",_f0, 2, ain_print_resistance, ain_get_resistance, set_ro, &ain6, 0 }, + { "ain7","ain7vv",_f0, 4, ain_print_value, ain_get_value, set_ro, &ain7, 0 }, + { "ain7","ain7rv",_f0, 2, ain_print_resistance, ain_get_resistance, set_ro, &ain7, 0 }, + { "ain8","ain8vv",_f0, 4, ain_print_value, ain_get_value, set_ro, &ain8, 0 }, + { "ain8","ain8rv",_f0, 2, ain_print_resistance, ain_get_resistance, set_ro, &ain8, 0 }, // PWM settings - { "p1","p1frq",_fip, 0, pwm_print_p1frq, get_flt, pwm_set_pwm,(float *)&pwm.c[PWM_1].frequency, P1_PWM_FREQUENCY }, - { "p1","p1csl",_fip, 0, pwm_print_p1csl, get_flt, pwm_set_pwm,(float *)&pwm.c[PWM_1].cw_speed_lo, P1_CW_SPEED_LO }, - { "p1","p1csh",_fip, 0, pwm_print_p1csh, get_flt, pwm_set_pwm,(float *)&pwm.c[PWM_1].cw_speed_hi, P1_CW_SPEED_HI }, - { "p1","p1cpl",_fip, 3, pwm_print_p1cpl, get_flt, pwm_set_pwm,(float *)&pwm.c[PWM_1].cw_phase_lo, P1_CW_PHASE_LO }, - { "p1","p1cph",_fip, 3, pwm_print_p1cph, get_flt, pwm_set_pwm,(float *)&pwm.c[PWM_1].cw_phase_hi, P1_CW_PHASE_HI }, - { "p1","p1wsl",_fip, 0, pwm_print_p1wsl, get_flt, pwm_set_pwm,(float *)&pwm.c[PWM_1].ccw_speed_lo, P1_CCW_SPEED_LO }, - { "p1","p1wsh",_fip, 0, pwm_print_p1wsh, get_flt, pwm_set_pwm,(float *)&pwm.c[PWM_1].ccw_speed_hi, P1_CCW_SPEED_HI }, - { "p1","p1wpl",_fip, 3, pwm_print_p1wpl, get_flt, pwm_set_pwm,(float *)&pwm.c[PWM_1].ccw_phase_lo, P1_CCW_PHASE_LO }, - { "p1","p1wph",_fip, 3, pwm_print_p1wph, get_flt, pwm_set_pwm,(float *)&pwm.c[PWM_1].ccw_phase_hi, P1_CCW_PHASE_HI }, - { "p1","p1pof",_fip, 3, pwm_print_p1pof, get_flt, pwm_set_pwm,(float *)&pwm.c[PWM_1].phase_off, P1_PWM_PHASE_OFF }, + { "p1","p1frq",_fip, 0, pwm_print_p1frq, get_flt, pwm_set_pwm,&pwm.c[PWM_1].frequency, P1_PWM_FREQUENCY }, + { "p1","p1csl",_fip, 0, pwm_print_p1csl, get_flt, pwm_set_pwm,&pwm.c[PWM_1].cw_speed_lo, P1_CW_SPEED_LO }, + { "p1","p1csh",_fip, 0, pwm_print_p1csh, get_flt, pwm_set_pwm,&pwm.c[PWM_1].cw_speed_hi, P1_CW_SPEED_HI }, + { "p1","p1cpl",_fip, 3, pwm_print_p1cpl, get_flt, pwm_set_pwm,&pwm.c[PWM_1].cw_phase_lo, P1_CW_PHASE_LO }, + { "p1","p1cph",_fip, 3, pwm_print_p1cph, get_flt, pwm_set_pwm,&pwm.c[PWM_1].cw_phase_hi, P1_CW_PHASE_HI }, + { "p1","p1wsl",_fip, 0, pwm_print_p1wsl, get_flt, pwm_set_pwm,&pwm.c[PWM_1].ccw_speed_lo, P1_CCW_SPEED_LO }, + { "p1","p1wsh",_fip, 0, pwm_print_p1wsh, get_flt, pwm_set_pwm,&pwm.c[PWM_1].ccw_speed_hi, P1_CCW_SPEED_HI }, + { "p1","p1wpl",_fip, 3, pwm_print_p1wpl, get_flt, pwm_set_pwm,&pwm.c[PWM_1].ccw_phase_lo, P1_CCW_PHASE_LO }, + { "p1","p1wph",_fip, 3, pwm_print_p1wph, get_flt, pwm_set_pwm,&pwm.c[PWM_1].ccw_phase_hi, P1_CCW_PHASE_HI }, + { "p1","p1pof",_fip, 3, pwm_print_p1pof, get_flt, pwm_set_pwm,&pwm.c[PWM_1].phase_off, P1_PWM_PHASE_OFF }, // temperature configs - pid active values (read-only) - // NOTICE: If you change these PID group keys, you MUST change the get/set functions too!! - { "pid1","pid1p",_f0, 3, tx_print_nul, cm_get_pid_p, set_ro, (float *)&cs.null, 0 }, - { "pid1","pid1i",_f0, 5, tx_print_nul, cm_get_pid_i, set_ro, (float *)&cs.null, 0 }, - { "pid1","pid1d",_f0, 5, tx_print_nul, cm_get_pid_d, set_ro, (float *)&cs.null, 0 }, - { "pid1","pid1f",_f0, 5, tx_print_nul, cm_get_pid_f, set_ro, (float *)&cs.null, 0 }, + // NOTICE: If you change these PID group keys, you MUST change the get/set functions too! + { "pid1","pid1p",_fip, 3, tx_print_nul, cm_get_pid_p, set_ro, nullptr, 0 }, + { "pid1","pid1i",_fip, 5, tx_print_nul, cm_get_pid_i, set_ro, nullptr, 0 }, + { "pid1","pid1d",_fip, 5, tx_print_nul, cm_get_pid_d, set_ro, nullptr, 0 }, - { "pid2","pid2p",_f0, 3, tx_print_nul, cm_get_pid_p, set_ro, (float *)&cs.null, 0 }, - { "pid2","pid2i",_f0, 5, tx_print_nul, cm_get_pid_i, set_ro, (float *)&cs.null, 0 }, - { "pid2","pid2d",_f0, 5, tx_print_nul, cm_get_pid_d, set_ro, (float *)&cs.null, 0 }, - { "pid2","pid2f",_f0, 5, tx_print_nul, cm_get_pid_f, set_ro, (float *)&cs.null, 0 }, + { "pid2","pid2p",_fip, 3, tx_print_nul, cm_get_pid_p, set_ro, nullptr, 0 }, + { "pid2","pid2i",_fip, 5, tx_print_nul, cm_get_pid_i, set_ro, nullptr, 0 }, + { "pid2","pid2d",_fip, 5, tx_print_nul, cm_get_pid_d, set_ro, nullptr, 0 }, - { "pid3","pid3p",_f0, 3, tx_print_nul, cm_get_pid_p, set_ro, (float *)&cs.null, 0 }, - { "pid3","pid3i",_f0, 5, tx_print_nul, cm_get_pid_i, set_ro, (float *)&cs.null, 0 }, - { "pid3","pid3d",_f0, 5, tx_print_nul, cm_get_pid_d, set_ro, (float *)&cs.null, 0 }, - { "pid3","pid3f",_f0, 5, tx_print_nul, cm_get_pid_f, set_ro, (float *)&cs.null, 0 }, + { "pid3","pid3p",_fip, 3, tx_print_nul, cm_get_pid_p, set_ro, nullptr, 0 }, + { "pid3","pid3i",_fip, 5, tx_print_nul, cm_get_pid_i, set_ro, nullptr, 0 }, + { "pid3","pid3d",_fip, 5, tx_print_nul, cm_get_pid_d, set_ro, nullptr, 0 }, // temperature configs - heater set values (read-write) - // NOTICE: If you change these heater group keys, you MUST change the get/set functions too!! - { "he1","he1e", _fip, 0, tx_print_nul, cm_get_heater_enable, cm_set_heater_enable, (float *)&cs.null, H1_DEFAULT_ENABLE }, - { "he1","he1p", _fi, 3, tx_print_nul, cm_get_heater_p, cm_set_heater_p, (float *)&cs.null, H1_DEFAULT_P }, - { "he1","he1i", _fi, 5, tx_print_nul, cm_get_heater_i, cm_set_heater_i, (float *)&cs.null, H1_DEFAULT_I }, - { "he1","he1d", _fi, 5, tx_print_nul, cm_get_heater_d, cm_set_heater_d, (float *)&cs.null, H1_DEFAULT_D }, - { "he1","he1f", _fi, 5, tx_print_nul, cm_get_heater_f, cm_set_heater_f, (float *)&cs.null, H1_DEFAULT_F }, - { "he1","he1st",_f0, 1, tx_print_nul, cm_get_set_temperature, cm_set_set_temperature, (float *)&cs.null, 0 }, - { "he1","he1t", _f0, 1, tx_print_nul, cm_get_temperature, set_ro, (float *)&cs.null, 0 }, - { "he1","he1op",_f0, 3, tx_print_nul, cm_get_heater_output, set_ro, (float *)&cs.null, 0 }, - { "he1","he1tr",_f0, 3, tx_print_nul, cm_get_thermistor_resistance, set_ro, (float *)&cs.null, 0 }, - { "he1","he1tv",_f0, 6, tx_print_nul, cm_get_thermistor_voltage, set_ro, (float *)&cs.null, 0 }, - { "he1","he1at",_f0, 0, tx_print_nul, cm_get_at_temperature, set_ro, (float *)&cs.null, 0 }, - { "he1","he1an",_f0, 0, tx_print_nul, cm_get_heater_adc, set_ro, (float *)&cs.null, 0 }, - { "he1","he1fp",_f0, 1, tx_print_nul, cm_get_fan_power, cm_set_fan_power, (float *)&cs.null, 0 }, - { "he1","he1fm",_f0, 1, tx_print_nul, cm_get_fan_min_power, cm_set_fan_min_power, (float *)&cs.null, 0 }, - { "he1","he1fl",_f0, 1, tx_print_nul, cm_get_fan_low_temp, cm_set_fan_low_temp, (float *)&cs.null, 0 }, - { "he1","he1fh",_f0, 1, tx_print_nul, cm_get_fan_high_temp, cm_set_fan_high_temp, (float *)&cs.null, 0 }, + // NOTICE: If you change these heater group keys, you MUST change the get/set functions too! + { "he1","he1e", _bip, 0, tx_print_nul, cm_get_heater_enable, cm_set_heater_enable, nullptr, H1_DEFAULT_ENABLE }, + { "he1","he1at",_b0, 0, tx_print_nul, cm_get_at_temperature, set_ro, nullptr, 0 }, + { "he1","he1p", _fip, 3, tx_print_nul, cm_get_heater_p, cm_set_heater_p, nullptr, H1_DEFAULT_P }, + { "he1","he1i", _fip, 5, tx_print_nul, cm_get_heater_i, cm_set_heater_i, nullptr, H1_DEFAULT_I }, + { "he1","he1d", _fip, 5, tx_print_nul, cm_get_heater_d, cm_set_heater_d, nullptr, H1_DEFAULT_D }, + { "he1","he1f", _fi, 5, tx_print_nul, cm_get_heater_f, cm_set_heater_f, nullptr, H1_DEFAULT_F }, + { "he1","he1st",_fi, 1, tx_print_nul, cm_get_set_temperature, cm_set_set_temperature, nullptr, 0 }, + { "he1","he1t", _fi, 1, tx_print_nul, cm_get_temperature, set_ro, nullptr, 0 }, + { "he1","he1op",_fi, 3, tx_print_nul, cm_get_heater_output, set_ro, nullptr, 0 }, + { "he1","he1tr",_fi, 3, tx_print_nul, cm_get_thermistor_resistance, set_ro, nullptr, 0 }, + { "he1","he1tv",_f0, 6, tx_print_nul, cm_get_thermistor_voltage, set_ro, nullptr, 0 }, + { "he1","he1an",_fi, 0, tx_print_nul, cm_get_heater_adc, set_ro, nullptr, 0 }, + { "he1","he1fp",_fi, 1, tx_print_nul, cm_get_fan_power, cm_set_fan_power, nullptr, 0 }, + { "he1","he1fm",_fi, 1, tx_print_nul, cm_get_fan_min_power, cm_set_fan_min_power, nullptr, 0 }, + { "he1","he1fl",_fi, 1, tx_print_nul, cm_get_fan_low_temp, cm_set_fan_low_temp, nullptr, 0 }, + { "he1","he1fh",_fi, 1, tx_print_nul, cm_get_fan_high_temp, cm_set_fan_high_temp, nullptr, 0 }, - { "he2","he2e", _fip, 0, tx_print_nul, cm_get_heater_enable, cm_set_heater_enable, (float *)&cs.null, H2_DEFAULT_ENABLE }, - { "he2","he2p", _fi, 3, tx_print_nul, cm_get_heater_p, cm_set_heater_p, (float *)&cs.null, H2_DEFAULT_P }, - { "he2","he2i", _fi, 5, tx_print_nul, cm_get_heater_i, cm_set_heater_i, (float *)&cs.null, H2_DEFAULT_I }, - { "he2","he2d", _fi, 5, tx_print_nul, cm_get_heater_d, cm_set_heater_d, (float *)&cs.null, H2_DEFAULT_D }, - { "he2","he2f", _fi, 5, tx_print_nul, cm_get_heater_f, cm_set_heater_f, (float *)&cs.null, H2_DEFAULT_F }, - { "he2","he2st",_f0, 0, tx_print_nul, cm_get_set_temperature, cm_set_set_temperature, (float *)&cs.null, 0 }, - { "he2","he2t", _f0, 1, tx_print_nul, cm_get_temperature, set_ro, (float *)&cs.null, 0 }, - { "he2","he2op",_f0, 3, tx_print_nul, cm_get_heater_output, set_ro, (float *)&cs.null, 0 }, - { "he2","he2tr",_f0, 3, tx_print_nul, cm_get_thermistor_resistance, set_ro, (float *)&cs.null, 0 }, - { "he2","he2tv",_f0, 6, tx_print_nul, cm_get_thermistor_voltage, set_ro, (float *)&cs.null, 0 }, - { "he2","he2at",_f0, 0, tx_print_nul, cm_get_at_temperature, set_ro, (float *)&cs.null, 0 }, - { "he2","he2an",_f0, 0, tx_print_nul, cm_get_heater_adc, set_ro, (float *)&cs.null, 0 }, - { "he2","he2fp",_f0, 1, tx_print_nul, cm_get_fan_power, cm_set_fan_power, (float *)&cs.null, 0 }, - { "he2","he2fm",_f0, 1, tx_print_nul, cm_get_fan_min_power, cm_set_fan_min_power, (float *)&cs.null, 0 }, - { "he2","he2fl",_f0, 1, tx_print_nul, cm_get_fan_low_temp, cm_set_fan_low_temp, (float *)&cs.null, 0 }, - { "he2","he2fh",_f0, 1, tx_print_nul, cm_get_fan_high_temp, cm_set_fan_high_temp, (float *)&cs.null, 0 }, + { "he2","he2e", _iip, 0, tx_print_nul, cm_get_heater_enable, cm_set_heater_enable, nullptr, H2_DEFAULT_ENABLE }, + { "he2","he2at",_b0, 0, tx_print_nul, cm_get_at_temperature, set_ro, nullptr, 0 }, + { "he2","he2p", _fip, 3, tx_print_nul, cm_get_heater_p, cm_set_heater_p, nullptr, H2_DEFAULT_P }, + { "he2","he2i", _fip, 5, tx_print_nul, cm_get_heater_i, cm_set_heater_i, nullptr, H2_DEFAULT_I }, + { "he2","he2d", _fip, 5, tx_print_nul, cm_get_heater_d, cm_set_heater_d, nullptr, H2_DEFAULT_D }, + { "he2","he2f", _fi, 5, tx_print_nul, cm_get_heater_f, cm_set_heater_f, nullptr, H2_DEFAULT_F }, + { "he2","he2st",_fi, 0, tx_print_nul, cm_get_set_temperature, cm_set_set_temperature, nullptr, 0 }, + { "he2","he2t", _fi, 1, tx_print_nul, cm_get_temperature, set_ro, nullptr, 0 }, + { "he2","he2op",_fi, 3, tx_print_nul, cm_get_heater_output, set_ro, nullptr, 0 }, + { "he2","he2tr",_fi, 3, tx_print_nul, cm_get_thermistor_resistance, set_ro, nullptr, 0 }, + { "he1","he1tv",_f0, 6, tx_print_nul, cm_get_thermistor_voltage, set_ro, nullptr, 0 }, + { "he2","he2an",_fi, 0, tx_print_nul, cm_get_heater_adc, set_ro, nullptr, 0 }, + { "he2","he2fp",_fi, 1, tx_print_nul, cm_get_fan_power, cm_set_fan_power, nullptr, 0 }, + { "he2","he2fm",_fi, 1, tx_print_nul, cm_get_fan_min_power, cm_set_fan_min_power, nullptr, 0 }, + { "he2","he2fl",_fi, 1, tx_print_nul, cm_get_fan_low_temp, cm_set_fan_low_temp, nullptr, 0 }, + { "he2","he2fh",_fi, 1, tx_print_nul, cm_get_fan_high_temp, cm_set_fan_high_temp, nullptr, 0 }, - { "he3","he3e", _fip, 0, tx_print_nul, cm_get_heater_enable, cm_set_heater_enable, (float *)&cs.null, H3_DEFAULT_ENABLE }, - { "he3","he3p", _fi, 3, tx_print_nul, cm_get_heater_p, cm_set_heater_p, (float *)&cs.null, H3_DEFAULT_P }, - { "he3","he3i", _fi, 5, tx_print_nul, cm_get_heater_i, cm_set_heater_i, (float *)&cs.null, H3_DEFAULT_I }, - { "he3","he3d", _fi, 5, tx_print_nul, cm_get_heater_d, cm_set_heater_d, (float *)&cs.null, H3_DEFAULT_D }, - { "he3","he3f", _fi, 5, tx_print_nul, cm_get_heater_f, cm_set_heater_f, (float *)&cs.null, H3_DEFAULT_F }, - { "he3","he3st",_f0, 0, tx_print_nul, cm_get_set_temperature, cm_set_set_temperature, (float *)&cs.null, 0 }, - { "he3","he3t", _f0, 1, tx_print_nul, cm_get_temperature, set_ro, (float *)&cs.null, 0 }, - { "he3","he3op",_f0, 3, tx_print_nul, cm_get_heater_output, set_ro, (float *)&cs.null, 0 }, - { "he3","he3tr",_f0, 3, tx_print_nul, cm_get_thermistor_resistance, set_ro, (float *)&cs.null, 0 }, - { "he3","he3tv",_f0, 6, tx_print_nul, cm_get_thermistor_voltage, set_ro, (float *)&cs.null, 0 }, - { "he3","he3at",_f0, 0, tx_print_nul, cm_get_at_temperature, set_ro, (float *)&cs.null, 0 }, - { "he3","he3an",_f0, 0, tx_print_nul, cm_get_heater_adc, set_ro, (float *)&cs.null, 0 }, - { "he3","he3fp",_f0, 1, tx_print_nul, cm_get_fan_power, cm_set_fan_power, (float *)&cs.null, 0 }, - { "he3","he3fm",_f0, 1, tx_print_nul, cm_get_fan_min_power, cm_set_fan_min_power, (float *)&cs.null, 0 }, - { "he3","he3fl",_f0, 1, tx_print_nul, cm_get_fan_low_temp, cm_set_fan_low_temp, (float *)&cs.null, 0 }, - { "he3","he3fh",_f0, 1, tx_print_nul, cm_get_fan_high_temp, cm_set_fan_high_temp, (float *)&cs.null, 0 }, + { "he3","he3e", _iip, 0, tx_print_nul, cm_get_heater_enable, cm_set_heater_enable, nullptr, H3_DEFAULT_ENABLE }, + { "he3","he3at",_b0, 0, tx_print_nul, cm_get_at_temperature, set_ro, nullptr, 0 }, + { "he3","he3p", _fip, 3, tx_print_nul, cm_get_heater_p, cm_set_heater_p, nullptr, H3_DEFAULT_P }, + { "he3","he3i", _fip, 5, tx_print_nul, cm_get_heater_i, cm_set_heater_i, nullptr, H3_DEFAULT_I }, + { "he3","he3d", _fip, 5, tx_print_nul, cm_get_heater_d, cm_set_heater_d, nullptr, H3_DEFAULT_D }, + { "he3","he3f", _fi, 5, tx_print_nul, cm_get_heater_f, cm_set_heater_f, nullptr, H3_DEFAULT_F }, + { "he3","he3st",_fi, 0, tx_print_nul, cm_get_set_temperature, cm_set_set_temperature, nullptr, 0 }, + { "he3","he3t", _fi, 1, tx_print_nul, cm_get_temperature, set_ro, nullptr, 0 }, + { "he3","he3op",_fi, 3, tx_print_nul, cm_get_heater_output, set_ro, nullptr, 0 }, + { "he3","he3tr",_fi, 3, tx_print_nul, cm_get_thermistor_resistance, set_ro, nullptr, 0 }, + { "he1","he1tv",_f0, 6, tx_print_nul, cm_get_thermistor_voltage, set_ro, nullptr, 0 }, + { "he3","he3an",_fi, 0, tx_print_nul, cm_get_heater_adc, set_ro, nullptr, 0 }, + { "he3","he3fp",_fi, 1, tx_print_nul, cm_get_fan_power, cm_set_fan_power, nullptr, 0 }, + { "he3","he3fm",_fi, 1, tx_print_nul, cm_get_fan_min_power, cm_set_fan_min_power, nullptr, 0 }, + { "he3","he3fl",_fi, 1, tx_print_nul, cm_get_fan_low_temp, cm_set_fan_low_temp, nullptr, 0 }, + { "he3","he3fh",_fi, 1, tx_print_nul, cm_get_fan_high_temp, cm_set_fan_high_temp, nullptr, 0 }, // Coordinate system offsets (G54-G59 and G92) - { "g54","g54x",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G54][AXIS_X], G54_X_OFFSET }, - { "g54","g54y",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G54][AXIS_Y], G54_Y_OFFSET }, - { "g54","g54z",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G54][AXIS_Z], G54_Z_OFFSET }, - { "g54","g54a",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.offset[G54][AXIS_A], G54_A_OFFSET }, - { "g54","g54b",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.offset[G54][AXIS_B], G54_B_OFFSET }, - { "g54","g54c",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.offset[G54][AXIS_C], G54_C_OFFSET }, + { "g54","g54x",_fipc, 5, cm_print_cofs, cm_get_coord, cm_set_coord, nullptr, G54_X_OFFSET }, + { "g54","g54y",_fipc, 5, cm_print_cofs, cm_get_coord, cm_set_coord, nullptr, G54_Y_OFFSET }, + { "g54","g54z",_fipc, 5, cm_print_cofs, cm_get_coord, cm_set_coord, nullptr, G54_Z_OFFSET }, + { "g54","g54u",_fipc, 5, cm_print_cofs, cm_get_coord, cm_set_coord, nullptr, G54_U_OFFSET }, + { "g54","g54v",_fipc, 5, cm_print_cofs, cm_get_coord, cm_set_coord, nullptr, G54_V_OFFSET }, + { "g54","g54w",_fipc, 5, cm_print_cofs, cm_get_coord, cm_set_coord, nullptr, G54_W_OFFSET }, + { "g54","g54a",_fipc, 5, cm_print_cofs, cm_get_coord, cm_set_coord, nullptr, G54_A_OFFSET }, + { "g54","g54b",_fipc, 5, cm_print_cofs, cm_get_coord, cm_set_coord, nullptr, G54_B_OFFSET }, + { "g54","g54c",_fipc, 5, cm_print_cofs, cm_get_coord, cm_set_coord, nullptr, G54_C_OFFSET }, - { "g55","g55x",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G55][AXIS_X], G55_X_OFFSET }, - { "g55","g55y",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G55][AXIS_Y], G55_Y_OFFSET }, - { "g55","g55z",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G55][AXIS_Z], G55_Z_OFFSET }, - { "g55","g55a",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.offset[G55][AXIS_A], G55_A_OFFSET }, - { "g55","g55b",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.offset[G55][AXIS_B], G55_B_OFFSET }, - { "g55","g55c",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.offset[G55][AXIS_C], G55_C_OFFSET }, + { "g55","g55x",_fipc, 5, cm_print_cofs, cm_get_coord, cm_set_coord, nullptr, G55_X_OFFSET }, + { "g55","g55y",_fipc, 5, cm_print_cofs, cm_get_coord, cm_set_coord, nullptr, G55_Y_OFFSET }, + { "g55","g55z",_fipc, 5, cm_print_cofs, cm_get_coord, cm_set_coord, nullptr, G55_Z_OFFSET }, + { "g55","g55u",_fipc, 5, cm_print_cofs, cm_get_coord, cm_set_coord, nullptr, G55_U_OFFSET }, + { "g55","g55v",_fipc, 5, cm_print_cofs, cm_get_coord, cm_set_coord, nullptr, G55_V_OFFSET }, + { "g55","g55w",_fipc, 5, cm_print_cofs, cm_get_coord, cm_set_coord, nullptr, G55_W_OFFSET }, + { "g55","g55a",_fipc, 5, cm_print_cofs, cm_get_coord, cm_set_coord, nullptr, G55_A_OFFSET }, + { "g55","g55b",_fipc, 5, cm_print_cofs, cm_get_coord, cm_set_coord, nullptr, G55_B_OFFSET }, + { "g55","g55c",_fipc, 5, cm_print_cofs, cm_get_coord, cm_set_coord, nullptr, G55_C_OFFSET }, - { "g56","g56x",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G56][AXIS_X], G56_X_OFFSET }, - { "g56","g56y",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G56][AXIS_Y], G56_Y_OFFSET }, - { "g56","g56z",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G56][AXIS_Z], G56_Z_OFFSET }, - { "g56","g56a",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.offset[G56][AXIS_A], G56_A_OFFSET }, - { "g56","g56b",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.offset[G56][AXIS_B], G56_B_OFFSET }, - { "g56","g56c",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.offset[G56][AXIS_C], G56_C_OFFSET }, + { "g56","g56x",_fipc, 5, cm_print_cofs, cm_get_coord, cm_set_coord, nullptr, G56_X_OFFSET }, + { "g56","g56y",_fipc, 5, cm_print_cofs, cm_get_coord, cm_set_coord, nullptr, G56_Y_OFFSET }, + { "g56","g56z",_fipc, 5, cm_print_cofs, cm_get_coord, cm_set_coord, nullptr, G56_Z_OFFSET }, + { "g56","g56u",_fipc, 5, cm_print_cofs, cm_get_coord, cm_set_coord, nullptr, G56_U_OFFSET }, + { "g56","g56v",_fipc, 5, cm_print_cofs, cm_get_coord, cm_set_coord, nullptr, G56_V_OFFSET }, + { "g56","g56w",_fipc, 5, cm_print_cofs, cm_get_coord, cm_set_coord, nullptr, G56_W_OFFSET }, + { "g56","g56a",_fipc, 5, cm_print_cofs, cm_get_coord, cm_set_coord, nullptr, G56_A_OFFSET }, + { "g56","g56b",_fipc, 5, cm_print_cofs, cm_get_coord, cm_set_coord, nullptr, G56_B_OFFSET }, + { "g56","g56c",_fipc, 5, cm_print_cofs, cm_get_coord, cm_set_coord, nullptr, G56_C_OFFSET }, - { "g57","g57x",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G57][AXIS_X], G57_X_OFFSET }, - { "g57","g57y",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G57][AXIS_Y], G57_Y_OFFSET }, - { "g57","g57z",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G57][AXIS_Z], G57_Z_OFFSET }, - { "g57","g57a",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.offset[G57][AXIS_A], G57_A_OFFSET }, - { "g57","g57b",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.offset[G57][AXIS_B], G57_B_OFFSET }, - { "g57","g57c",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.offset[G57][AXIS_C], G57_C_OFFSET }, + { "g57","g57x",_fipc, 5, cm_print_cofs, cm_get_coord, cm_set_coord, nullptr, G57_X_OFFSET }, + { "g57","g57y",_fipc, 5, cm_print_cofs, cm_get_coord, cm_set_coord, nullptr, G57_Y_OFFSET }, + { "g57","g57z",_fipc, 5, cm_print_cofs, cm_get_coord, cm_set_coord, nullptr, G57_Z_OFFSET }, + { "g57","g57u",_fipc, 5, cm_print_cofs, cm_get_coord, cm_set_coord, nullptr, G57_U_OFFSET }, + { "g57","g57v",_fipc, 5, cm_print_cofs, cm_get_coord, cm_set_coord, nullptr, G57_V_OFFSET }, + { "g57","g57w",_fipc, 5, cm_print_cofs, cm_get_coord, cm_set_coord, nullptr, G57_W_OFFSET }, + { "g57","g57a",_fipc, 5, cm_print_cofs, cm_get_coord, cm_set_coord, nullptr, G57_A_OFFSET }, + { "g57","g57b",_fipc, 5, cm_print_cofs, cm_get_coord, cm_set_coord, nullptr, G57_B_OFFSET }, + { "g57","g57c",_fipc, 5, cm_print_cofs, cm_get_coord, cm_set_coord, nullptr, G57_C_OFFSET }, - { "g58","g58x",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G58][AXIS_X], G58_X_OFFSET }, - { "g58","g58y",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G58][AXIS_Y], G58_Y_OFFSET }, - { "g58","g58z",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G58][AXIS_Z], G58_Z_OFFSET }, - { "g58","g58a",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.offset[G58][AXIS_A], G58_A_OFFSET }, - { "g58","g58b",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.offset[G58][AXIS_B], G58_B_OFFSET }, - { "g58","g58c",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.offset[G58][AXIS_C], G58_C_OFFSET }, + { "g58","g58x",_fipc, 5, cm_print_cofs, cm_get_coord, cm_set_coord, nullptr, G58_X_OFFSET }, + { "g58","g58y",_fipc, 5, cm_print_cofs, cm_get_coord, cm_set_coord, nullptr, G58_Y_OFFSET }, + { "g58","g58z",_fipc, 5, cm_print_cofs, cm_get_coord, cm_set_coord, nullptr, G58_Z_OFFSET }, + { "g58","g58u",_fipc, 5, cm_print_cofs, cm_get_coord, cm_set_coord, nullptr, G58_U_OFFSET }, + { "g58","g58v",_fipc, 5, cm_print_cofs, cm_get_coord, cm_set_coord, nullptr, G58_V_OFFSET }, + { "g58","g58w",_fipc, 5, cm_print_cofs, cm_get_coord, cm_set_coord, nullptr, G58_W_OFFSET }, + { "g58","g58a",_fipc, 5, cm_print_cofs, cm_get_coord, cm_set_coord, nullptr, G58_A_OFFSET }, + { "g58","g58b",_fipc, 5, cm_print_cofs, cm_get_coord, cm_set_coord, nullptr, G58_B_OFFSET }, + { "g58","g58c",_fipc, 5, cm_print_cofs, cm_get_coord, cm_set_coord, nullptr, G58_C_OFFSET }, - { "g59","g59x",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G59][AXIS_X], G59_X_OFFSET }, - { "g59","g59y",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G59][AXIS_Y], G59_Y_OFFSET }, - { "g59","g59z",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G59][AXIS_Z], G59_Z_OFFSET }, - { "g59","g59a",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.offset[G59][AXIS_A], G59_A_OFFSET }, - { "g59","g59b",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.offset[G59][AXIS_B], G59_B_OFFSET }, - { "g59","g59c",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.offset[G59][AXIS_C], G59_C_OFFSET }, + { "g59","g59x",_fipc, 5, cm_print_cofs, cm_get_coord, cm_set_coord, nullptr, G59_X_OFFSET }, + { "g59","g59y",_fipc, 5, cm_print_cofs, cm_get_coord, cm_set_coord, nullptr, G59_Y_OFFSET }, + { "g59","g59z",_fipc, 5, cm_print_cofs, cm_get_coord, cm_set_coord, nullptr, G59_Z_OFFSET }, + { "g59","g59u",_fipc, 5, cm_print_cofs, cm_get_coord, cm_set_coord, nullptr, G59_U_OFFSET }, + { "g59","g59v",_fipc, 5, cm_print_cofs, cm_get_coord, cm_set_coord, nullptr, G59_V_OFFSET }, + { "g59","g59w",_fipc, 5, cm_print_cofs, cm_get_coord, cm_set_coord, nullptr, G59_W_OFFSET }, + { "g59","g59a",_fipc, 5, cm_print_cofs, cm_get_coord, cm_set_coord, nullptr, G59_A_OFFSET }, + { "g59","g59b",_fipc, 5, cm_print_cofs, cm_get_coord, cm_set_coord, nullptr, G59_B_OFFSET }, + { "g59","g59c",_fipc, 5, cm_print_cofs, cm_get_coord, cm_set_coord, nullptr, G59_C_OFFSET }, - { "g92","g92x",_fic, 3, cm_print_cofs, get_flt, set_ro, (float *)&cm.gmx.origin_offset[AXIS_X], 0 },// G92 handled differently - { "g92","g92y",_fic, 3, cm_print_cofs, get_flt, set_ro, (float *)&cm.gmx.origin_offset[AXIS_Y], 0 }, - { "g92","g92z",_fic, 3, cm_print_cofs, get_flt, set_ro, (float *)&cm.gmx.origin_offset[AXIS_Z], 0 }, - { "g92","g92a",_fi, 3, cm_print_cofs, get_flt, set_ro, (float *)&cm.gmx.origin_offset[AXIS_A], 0 }, - { "g92","g92b",_fi, 3, cm_print_cofs, get_flt, set_ro, (float *)&cm.gmx.origin_offset[AXIS_B], 0 }, - { "g92","g92c",_fi, 3, cm_print_cofs, get_flt, set_ro, (float *)&cm.gmx.origin_offset[AXIS_C], 0 }, + { "g92","g92x",_fic, 5, cm_print_cofs, cm_get_g92, set_ro, nullptr, 0 },// G92 handled differently + { "g92","g92y",_fic, 5, cm_print_cofs, cm_get_g92, set_ro, nullptr, 0 }, + { "g92","g92z",_fic, 5, cm_print_cofs, cm_get_g92, set_ro, nullptr, 0 }, + { "g92","g92u",_fic, 5, cm_print_cofs, cm_get_g92, set_ro, nullptr, 0 }, + { "g92","g92v",_fic, 5, cm_print_cofs, cm_get_g92, set_ro, nullptr, 0 }, + { "g92","g92w",_fic, 5, cm_print_cofs, cm_get_g92, set_ro, nullptr, 0 }, + { "g92","g92a",_fic, 5, cm_print_cofs, cm_get_g92, set_ro, nullptr, 0 }, + { "g92","g92b",_fic, 5, cm_print_cofs, cm_get_g92, set_ro, nullptr, 0 }, + { "g92","g92c",_fic, 5, cm_print_cofs, cm_get_g92, set_ro, nullptr, 0 }, // Coordinate positions (G28, G30) - { "g28","g28x",_fic, 3, cm_print_cpos, get_flt, set_ro, (float *)&cm.gmx.g28_position[AXIS_X], 0 },// g28 handled differently - { "g28","g28y",_fic, 3, cm_print_cpos, get_flt, set_ro, (float *)&cm.gmx.g28_position[AXIS_Y], 0 }, - { "g28","g28z",_fic, 3, cm_print_cpos, get_flt, set_ro, (float *)&cm.gmx.g28_position[AXIS_Z], 0 }, - { "g28","g28a",_fi, 3, cm_print_cpos, get_flt, set_ro, (float *)&cm.gmx.g28_position[AXIS_A], 0 }, - { "g28","g28b",_fi, 3, cm_print_cpos, get_flt, set_ro, (float *)&cm.gmx.g28_position[AXIS_B], 0 }, - { "g28","g28c",_fi, 3, cm_print_cpos, get_flt, set_ro, (float *)&cm.gmx.g28_position[AXIS_C], 0 }, + { "g28","g28x",_fic, 5, cm_print_cpos, cm_get_g28, set_ro, nullptr, 0 },// g28 handled differently + { "g28","g28y",_fic, 5, cm_print_cpos, cm_get_g28, set_ro, nullptr, 0 }, + { "g28","g28z",_fic, 5, cm_print_cpos, cm_get_g28, set_ro, nullptr, 0 }, + { "g28","g28u",_fic, 5, cm_print_cpos, cm_get_g28, set_ro, nullptr, 0 }, + { "g28","g28v",_fic, 5, cm_print_cpos, cm_get_g28, set_ro, nullptr, 0 }, + { "g28","g28w",_fic, 5, cm_print_cpos, cm_get_g28, set_ro, nullptr, 0 }, + { "g28","g28a",_fic, 5, cm_print_cpos, cm_get_g28, set_ro, nullptr, 0 }, + { "g28","g28b",_fic, 5, cm_print_cpos, cm_get_g28, set_ro, nullptr, 0 }, + { "g28","g28c",_fic, 5, cm_print_cpos, cm_get_g28, set_ro, nullptr, 0 }, - { "g30","g30x",_fic, 3, cm_print_cpos, get_flt, set_ro, (float *)&cm.gmx.g30_position[AXIS_X], 0 },// g30 handled differently - { "g30","g30y",_fic, 3, cm_print_cpos, get_flt, set_ro, (float *)&cm.gmx.g30_position[AXIS_Y], 0 }, - { "g30","g30z",_fic, 3, cm_print_cpos, get_flt, set_ro, (float *)&cm.gmx.g30_position[AXIS_Z], 0 }, - { "g30","g30a",_fi, 3, cm_print_cpos, get_flt, set_ro, (float *)&cm.gmx.g30_position[AXIS_A], 0 }, - { "g30","g30b",_fi, 3, cm_print_cpos, get_flt, set_ro, (float *)&cm.gmx.g30_position[AXIS_B], 0 }, - { "g30","g30c",_fi, 3, cm_print_cpos, get_flt, set_ro, (float *)&cm.gmx.g30_position[AXIS_C], 0 }, - - // Default values for current tool length offsets (not configurable, set to zero) - { "tof","tofx",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.tl_offset[AXIS_X], 0 }, - { "tof","tofy",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.tl_offset[AXIS_Y], 0 }, - { "tof","tofz",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.tl_offset[AXIS_Z], 0 }, - { "tof","tofa",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.tl_offset[AXIS_A], 0 }, - { "tof","tofb",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.tl_offset[AXIS_B], 0 }, - { "tof","tofc",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.tl_offset[AXIS_C], 0 }, - - // Tool table offsets - { "tt1","tt1x",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.tt_offset[1][AXIS_X], TT1_X_OFFSET }, - { "tt1","tt1y",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.tt_offset[1][AXIS_Y], TT1_Y_OFFSET }, - { "tt1","tt1z",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.tt_offset[1][AXIS_Z], TT1_Z_OFFSET }, - { "tt1","tt1a",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.tt_offset[1][AXIS_A], TT1_A_OFFSET }, - { "tt1","tt1b",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.tt_offset[1][AXIS_B], TT1_B_OFFSET }, - { "tt1","tt1c",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.tt_offset[1][AXIS_C], TT1_C_OFFSET }, - - { "tt2","tt2x",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.tt_offset[2][AXIS_X], TT2_X_OFFSET }, - { "tt2","tt2y",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.tt_offset[2][AXIS_Y], TT2_Y_OFFSET }, - { "tt2","tt2z",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.tt_offset[2][AXIS_Z], TT2_Z_OFFSET }, - { "tt2","tt2a",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.tt_offset[2][AXIS_A], TT2_A_OFFSET }, - { "tt2","tt2b",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.tt_offset[2][AXIS_B], TT2_B_OFFSET }, - { "tt2","tt2c",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.tt_offset[2][AXIS_C], TT2_C_OFFSET }, - - { "tt3","tt3x",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.tt_offset[3][AXIS_X], TT3_X_OFFSET }, - { "tt3","tt3y",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.tt_offset[3][AXIS_Y], TT3_Y_OFFSET }, - { "tt3","tt3z",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.tt_offset[3][AXIS_Z], TT3_Z_OFFSET }, - { "tt3","tt3a",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.tt_offset[3][AXIS_A], TT3_A_OFFSET }, - { "tt3","tt3b",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.tt_offset[3][AXIS_B], TT3_B_OFFSET }, - { "tt3","tt3c",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.tt_offset[3][AXIS_C], TT1_C_OFFSET }, - - { "tt4","tt4x",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.tt_offset[4][AXIS_X], TT4_X_OFFSET }, - { "tt4","tt4y",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.tt_offset[4][AXIS_Y], TT4_Y_OFFSET }, - { "tt4","tt4z",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.tt_offset[4][AXIS_Z], TT4_Z_OFFSET }, - { "tt4","tt4a",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.tt_offset[4][AXIS_A], TT4_A_OFFSET }, - { "tt4","tt4b",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.tt_offset[4][AXIS_B], TT4_B_OFFSET }, - { "tt4","tt4c",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.tt_offset[4][AXIS_C], TT4_C_OFFSET }, - - { "tt5","tt5x",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.tt_offset[5][AXIS_X], TT5_X_OFFSET }, - { "tt5","tt5y",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.tt_offset[5][AXIS_Y], TT5_Y_OFFSET }, - { "tt5","tt5z",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.tt_offset[5][AXIS_Z], TT5_Z_OFFSET }, - { "tt5","tt5a",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.tt_offset[5][AXIS_A], TT5_A_OFFSET }, - { "tt5","tt5b",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.tt_offset[5][AXIS_B], TT5_B_OFFSET }, - { "tt5","tt5c",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.tt_offset[5][AXIS_C], TT5_C_OFFSET }, - - { "tt6","tt6x",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.tt_offset[6][AXIS_X], TT6_X_OFFSET }, - { "tt6","tt6y",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.tt_offset[6][AXIS_Y], TT6_Y_OFFSET }, - { "tt6","tt6z",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.tt_offset[6][AXIS_Z], TT6_Z_OFFSET }, - { "tt6","tt6a",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.tt_offset[6][AXIS_A], TT6_A_OFFSET }, - { "tt6","tt6b",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.tt_offset[6][AXIS_B], TT6_B_OFFSET }, - { "tt6","tt6c",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.tt_offset[6][AXIS_C], TT6_C_OFFSET }, - - { "tt7","tt7x",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.tt_offset[7][AXIS_X], TT7_X_OFFSET }, - { "tt7","tt7y",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.tt_offset[7][AXIS_Y], TT7_Y_OFFSET }, - { "tt7","tt7z",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.tt_offset[7][AXIS_Z], TT7_Z_OFFSET }, - { "tt7","tt7a",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.tt_offset[7][AXIS_A], TT7_A_OFFSET }, - { "tt7","tt7b",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.tt_offset[7][AXIS_B], TT7_B_OFFSET }, - { "tt7","tt7c",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.tt_offset[7][AXIS_C], TT7_C_OFFSET }, - - { "tt8","tt8x",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.tt_offset[8][AXIS_X], TT8_X_OFFSET }, - { "tt8","tt8y",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.tt_offset[8][AXIS_Y], TT8_Y_OFFSET }, - { "tt8","tt8z",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.tt_offset[8][AXIS_Z], TT8_Z_OFFSET }, - { "tt8","tt8a",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.tt_offset[8][AXIS_A], TT8_A_OFFSET }, - { "tt8","tt8b",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.tt_offset[8][AXIS_B], TT8_B_OFFSET }, - { "tt8","tt8c",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.tt_offset[8][AXIS_C], TT8_C_OFFSET }, - - { "tt9","tt9x",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.tt_offset[9][AXIS_X], TT9_X_OFFSET }, - { "tt9","tt9y",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.tt_offset[9][AXIS_Y], TT9_Y_OFFSET }, - { "tt9","tt9z",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.tt_offset[9][AXIS_Z], TT9_Z_OFFSET }, - { "tt9","tt9a",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.tt_offset[9][AXIS_A], TT9_A_OFFSET }, - { "tt9","tt9b",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.tt_offset[9][AXIS_B], TT9_B_OFFSET }, - { "tt9","tt9c",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.tt_offset[9][AXIS_C], TT9_C_OFFSET }, - - { "tt10","tt10x",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.tt_offset[10][AXIS_X], TT10_X_OFFSET }, - { "tt10","tt10y",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.tt_offset[10][AXIS_Y], TT10_Y_OFFSET }, - { "tt10","tt10z",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.tt_offset[10][AXIS_Z], TT10_Z_OFFSET }, - { "tt10","tt10a",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.tt_offset[10][AXIS_A], TT10_A_OFFSET }, - { "tt10","tt10b",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.tt_offset[10][AXIS_B], TT10_B_OFFSET }, - { "tt10","tt10c",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.tt_offset[10][AXIS_C], TT10_C_OFFSET }, - - { "tt11","tt11x",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.tt_offset[11][AXIS_X], TT11_X_OFFSET }, - { "tt11","tt11y",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.tt_offset[11][AXIS_Y], TT11_Y_OFFSET }, - { "tt11","tt11z",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.tt_offset[11][AXIS_Z], TT11_Z_OFFSET }, - { "tt11","tt11a",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.tt_offset[11][AXIS_A], TT11_A_OFFSET }, - { "tt11","tt11b",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.tt_offset[11][AXIS_B], TT11_B_OFFSET }, - { "tt11","tt11c",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.tt_offset[11][AXIS_C], TT11_C_OFFSET }, - - { "tt12","tt12x",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.tt_offset[12][AXIS_X], TT12_X_OFFSET }, - { "tt12","tt12y",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.tt_offset[12][AXIS_Y], TT12_Y_OFFSET }, - { "tt12","tt12z",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.tt_offset[12][AXIS_Z], TT12_Z_OFFSET }, - { "tt12","tt12a",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.tt_offset[12][AXIS_A], TT12_A_OFFSET }, - { "tt12","tt12b",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.tt_offset[12][AXIS_B], TT12_B_OFFSET }, - { "tt12","tt12c",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.tt_offset[12][AXIS_C], TT12_C_OFFSET }, - - { "tt13","tt13x",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.tt_offset[13][AXIS_X], TT13_X_OFFSET }, - { "tt13","tt13y",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.tt_offset[13][AXIS_Y], TT13_Y_OFFSET }, - { "tt13","tt13z",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.tt_offset[13][AXIS_Z], TT13_Z_OFFSET }, - { "tt13","tt13a",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.tt_offset[13][AXIS_A], TT13_A_OFFSET }, - { "tt13","tt13b",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.tt_offset[13][AXIS_B], TT13_B_OFFSET }, - { "tt13","tt13c",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.tt_offset[13][AXIS_C], TT13_C_OFFSET }, - - { "tt14","tt14x",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.tt_offset[14][AXIS_X], TT14_X_OFFSET }, - { "tt14","tt14y",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.tt_offset[14][AXIS_Y], TT14_Y_OFFSET }, - { "tt14","tt14z",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.tt_offset[14][AXIS_Z], TT14_Z_OFFSET }, - { "tt14","tt14a",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.tt_offset[14][AXIS_A], TT14_A_OFFSET }, - { "tt14","tt14b",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.tt_offset[14][AXIS_B], TT14_B_OFFSET }, - { "tt14","tt14c",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.tt_offset[14][AXIS_C], TT14_C_OFFSET }, - - { "tt15","tt15x",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.tt_offset[15][AXIS_X], TT15_X_OFFSET }, - { "tt15","tt15y",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.tt_offset[15][AXIS_Y], TT15_Y_OFFSET }, - { "tt15","tt15z",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.tt_offset[15][AXIS_Z], TT15_Z_OFFSET }, - { "tt15","tt15a",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.tt_offset[15][AXIS_A], TT15_A_OFFSET }, - { "tt15","tt15b",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.tt_offset[15][AXIS_B], TT15_B_OFFSET }, - { "tt15","tt15c",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.tt_offset[15][AXIS_C], TT15_C_OFFSET }, - - { "tt16","tt16x",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.tt_offset[16][AXIS_X], TT16_X_OFFSET }, - { "tt16","tt16y",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.tt_offset[16][AXIS_Y], TT16_Y_OFFSET }, - { "tt16","tt16z",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.tt_offset[16][AXIS_Z], TT16_Z_OFFSET }, - { "tt16","tt16a",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.tt_offset[16][AXIS_A], TT16_A_OFFSET }, - { "tt16","tt16b",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.tt_offset[16][AXIS_B], TT16_B_OFFSET }, - { "tt16","tt16c",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.tt_offset[16][AXIS_C], TT16_C_OFFSET }, - - { "tt17","tt17x",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.tt_offset[17][AXIS_X], TT17_X_OFFSET }, - { "tt17","tt17y",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.tt_offset[17][AXIS_Y], TT17_Y_OFFSET }, - { "tt17","tt17z",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.tt_offset[17][AXIS_Z], TT17_Z_OFFSET }, - { "tt17","tt17a",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.tt_offset[17][AXIS_A], TT17_A_OFFSET }, - { "tt17","tt17b",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.tt_offset[17][AXIS_B], TT17_B_OFFSET }, - { "tt17","tt17c",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.tt_offset[17][AXIS_C], TT17_C_OFFSET }, - - { "tt18","tt18x",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.tt_offset[18][AXIS_X], TT18_X_OFFSET }, - { "tt18","tt18y",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.tt_offset[18][AXIS_Y], TT18_Y_OFFSET }, - { "tt18","tt18z",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.tt_offset[18][AXIS_Z], TT18_Z_OFFSET }, - { "tt18","tt18a",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.tt_offset[18][AXIS_A], TT18_A_OFFSET }, - { "tt18","tt18b",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.tt_offset[18][AXIS_B], TT18_B_OFFSET }, - { "tt18","tt18c",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.tt_offset[18][AXIS_C], TT18_C_OFFSET }, - - { "tt19","tt19x",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.tt_offset[19][AXIS_X], TT19_X_OFFSET }, - { "tt19","tt19y",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.tt_offset[19][AXIS_Y], TT19_Y_OFFSET }, - { "tt19","tt19z",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.tt_offset[19][AXIS_Z], TT19_Z_OFFSET }, - { "tt19","tt19a",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.tt_offset[19][AXIS_A], TT19_A_OFFSET }, - { "tt19","tt19b",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.tt_offset[19][AXIS_B], TT19_B_OFFSET }, - { "tt19","tt19c",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.tt_offset[19][AXIS_C], TT19_C_OFFSET }, - - { "tt20","tt20x",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.tt_offset[20][AXIS_X], TT20_X_OFFSET }, - { "tt20","tt20y",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.tt_offset[20][AXIS_Y], TT20_Y_OFFSET }, - { "tt20","tt20z",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.tt_offset[20][AXIS_Z], TT20_Z_OFFSET }, - { "tt20","tt20a",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.tt_offset[20][AXIS_A], TT20_A_OFFSET }, - { "tt20","tt20b",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.tt_offset[20][AXIS_B], TT20_B_OFFSET }, - { "tt20","tt20c",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.tt_offset[20][AXIS_C], TT20_C_OFFSET }, - - { "tt21","tt21x",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.tt_offset[21][AXIS_X], TT21_X_OFFSET }, - { "tt21","tt21y",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.tt_offset[21][AXIS_Y], TT21_Y_OFFSET }, - { "tt21","tt21z",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.tt_offset[21][AXIS_Z], TT21_Z_OFFSET }, - { "tt21","tt21a",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.tt_offset[21][AXIS_A], TT21_A_OFFSET }, - { "tt21","tt21b",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.tt_offset[21][AXIS_B], TT21_B_OFFSET }, - { "tt21","tt21c",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.tt_offset[21][AXIS_C], TT21_C_OFFSET }, - - { "tt22","tt22x",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.tt_offset[22][AXIS_X], TT22_X_OFFSET }, - { "tt22","tt22y",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.tt_offset[22][AXIS_Y], TT22_Y_OFFSET }, - { "tt22","tt22z",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.tt_offset[22][AXIS_Z], TT22_Z_OFFSET }, - { "tt22","tt22a",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.tt_offset[22][AXIS_A], TT22_A_OFFSET }, - { "tt22","tt22b",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.tt_offset[22][AXIS_B], TT22_B_OFFSET }, - { "tt22","tt22c",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.tt_offset[22][AXIS_C], TT22_C_OFFSET }, - - { "tt23","tt23x",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.tt_offset[23][AXIS_X], TT23_X_OFFSET }, - { "tt23","tt23y",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.tt_offset[23][AXIS_Y], TT23_Y_OFFSET }, - { "tt23","tt23z",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.tt_offset[23][AXIS_Z], TT23_Z_OFFSET }, - { "tt23","tt23a",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.tt_offset[23][AXIS_A], TT23_A_OFFSET }, - { "tt23","tt23b",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.tt_offset[23][AXIS_B], TT23_B_OFFSET }, - { "tt23","tt23c",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.tt_offset[23][AXIS_C], TT23_C_OFFSET }, - - { "tt24","tt24x",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.tt_offset[24][AXIS_X], TT24_X_OFFSET }, - { "tt24","tt24y",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.tt_offset[24][AXIS_Y], TT24_Y_OFFSET }, - { "tt24","tt24z",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.tt_offset[24][AXIS_Z], TT24_Z_OFFSET }, - { "tt24","tt24a",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.tt_offset[24][AXIS_A], TT24_A_OFFSET }, - { "tt24","tt24b",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.tt_offset[24][AXIS_B], TT24_B_OFFSET }, - { "tt24","tt24c",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.tt_offset[24][AXIS_C], TT24_C_OFFSET }, - - { "tt25","tt25x",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.tt_offset[25][AXIS_X], TT25_X_OFFSET }, - { "tt25","tt25y",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.tt_offset[25][AXIS_Y], TT25_Y_OFFSET }, - { "tt25","tt25z",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.tt_offset[25][AXIS_Z], TT25_Z_OFFSET }, - { "tt25","tt25a",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.tt_offset[25][AXIS_A], TT25_A_OFFSET }, - { "tt25","tt25b",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.tt_offset[25][AXIS_B], TT25_B_OFFSET }, - { "tt25","tt25c",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.tt_offset[25][AXIS_C], TT25_C_OFFSET }, - - { "tt26","tt26x",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.tt_offset[26][AXIS_X], TT26_X_OFFSET }, - { "tt26","tt26y",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.tt_offset[26][AXIS_Y], TT26_Y_OFFSET }, - { "tt26","tt26z",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.tt_offset[26][AXIS_Z], TT26_Z_OFFSET }, - { "tt26","tt26a",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.tt_offset[26][AXIS_A], TT26_A_OFFSET }, - { "tt26","tt26b",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.tt_offset[26][AXIS_B], TT26_B_OFFSET }, - { "tt26","tt26c",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.tt_offset[26][AXIS_C], TT26_C_OFFSET }, - - { "tt27","tt27x",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.tt_offset[27][AXIS_X], TT27_X_OFFSET }, - { "tt27","tt27y",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.tt_offset[27][AXIS_Y], TT27_Y_OFFSET }, - { "tt27","tt27z",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.tt_offset[27][AXIS_Z], TT27_Z_OFFSET }, - { "tt27","tt27a",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.tt_offset[27][AXIS_A], TT27_A_OFFSET }, - { "tt27","tt27b",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.tt_offset[27][AXIS_B], TT27_B_OFFSET }, - { "tt27","tt27c",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.tt_offset[27][AXIS_C], TT27_C_OFFSET }, - - { "tt28","tt28x",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.tt_offset[28][AXIS_X], TT28_X_OFFSET }, - { "tt28","tt28y",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.tt_offset[28][AXIS_Y], TT28_Y_OFFSET }, - { "tt28","tt28z",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.tt_offset[28][AXIS_Z], TT28_Z_OFFSET }, - { "tt28","tt28a",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.tt_offset[28][AXIS_A], TT28_A_OFFSET }, - { "tt28","tt28b",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.tt_offset[28][AXIS_B], TT28_B_OFFSET }, - { "tt28","tt28c",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.tt_offset[28][AXIS_C], TT28_C_OFFSET }, - - { "tt29","tt29x",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.tt_offset[29][AXIS_X], TT29_X_OFFSET }, - { "tt29","tt29y",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.tt_offset[29][AXIS_Y], TT29_Y_OFFSET }, - { "tt29","tt29z",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.tt_offset[29][AXIS_Z], TT29_Z_OFFSET }, - { "tt29","tt29a",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.tt_offset[29][AXIS_A], TT29_A_OFFSET }, - { "tt29","tt29b",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.tt_offset[29][AXIS_B], TT29_B_OFFSET }, - { "tt29","tt29c",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.tt_offset[29][AXIS_C], TT29_C_OFFSET }, - - { "tt30","tt30x",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.tt_offset[30][AXIS_X], TT30_X_OFFSET }, - { "tt30","tt30y",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.tt_offset[30][AXIS_Y], TT30_Y_OFFSET }, - { "tt30","tt30z",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.tt_offset[30][AXIS_Z], TT30_Z_OFFSET }, - { "tt30","tt30a",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.tt_offset[30][AXIS_A], TT30_A_OFFSET }, - { "tt30","tt30b",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.tt_offset[30][AXIS_B], TT30_B_OFFSET }, - { "tt30","tt30c",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.tt_offset[30][AXIS_C], TT30_C_OFFSET }, - - { "tt31","tt31x",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.tt_offset[31][AXIS_X], TT31_X_OFFSET }, - { "tt31","tt31y",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.tt_offset[31][AXIS_Y], TT31_Y_OFFSET }, - { "tt31","tt31z",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.tt_offset[31][AXIS_Z], TT31_Z_OFFSET }, - { "tt31","tt31a",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.tt_offset[31][AXIS_A], TT31_A_OFFSET }, - { "tt31","tt31b",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.tt_offset[31][AXIS_B], TT31_B_OFFSET }, - { "tt31","tt31c",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.tt_offset[31][AXIS_C], TT31_C_OFFSET }, - - { "tt32","tt32x",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.tt_offset[32][AXIS_X], TT32_X_OFFSET }, - { "tt32","tt32y",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.tt_offset[32][AXIS_Y], TT32_Y_OFFSET }, - { "tt32","tt32z",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.tt_offset[32][AXIS_Z], TT32_Z_OFFSET }, - { "tt32","tt32a",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.tt_offset[32][AXIS_A], TT32_A_OFFSET }, - { "tt32","tt32b",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.tt_offset[32][AXIS_B], TT32_B_OFFSET }, - { "tt32","tt32c",_fip, 3, cm_print_cofs, get_flt, set_flt,(float *)&cm.tt_offset[32][AXIS_C], TT32_C_OFFSET }, + { "g30","g30x",_fic, 5, cm_print_cpos, cm_get_g30, set_ro, nullptr, 0 },// g30 handled differently + { "g30","g30y",_fic, 5, cm_print_cpos, cm_get_g30, set_ro, nullptr, 0 }, + { "g30","g30z",_fic, 5, cm_print_cpos, cm_get_g30, set_ro, nullptr, 0 }, + { "g30","g30u",_fic, 5, cm_print_cpos, cm_get_g30, set_ro, nullptr, 0 }, + { "g30","g30v",_fic, 5, cm_print_cpos, cm_get_g30, set_ro, nullptr, 0 }, + { "g30","g30w",_fic, 5, cm_print_cpos, cm_get_g30, set_ro, nullptr, 0 }, + { "g30","g30a",_fic, 5, cm_print_cpos, cm_get_g30, set_ro, nullptr, 0 }, + { "g30","g30b",_fic, 5, cm_print_cpos, cm_get_g30, set_ro, nullptr, 0 }, + { "g30","g30c",_fic, 5, cm_print_cpos, cm_get_g30, set_ro, nullptr, 0 }, // this is a 128bit UUID for identifying a previously committed job state - { "jid","jida",_f0, 0, tx_print_nul, get_data, set_data, (float *)&cfg.job_id[0], 0}, - { "jid","jidb",_f0, 0, tx_print_nul, get_data, set_data, (float *)&cfg.job_id[1], 0}, - { "jid","jidc",_f0, 0, tx_print_nul, get_data, set_data, (float *)&cfg.job_id[2], 0}, - { "jid","jidd",_f0, 0, tx_print_nul, get_data, set_data, (float *)&cfg.job_id[3], 0}, - - // General system parameters - { "sys","jt", _fipn, 2, cm_print_jt, get_flt, cm_set_jt,(float *)&cm.junction_integration_time,JUNCTION_INTEGRATION_TIME }, - { "sys","ct", _fipnc,4, cm_print_ct, get_flt, set_flup, (float *)&cm.chordal_tolerance, CHORDAL_TOLERANCE }, - { "sys","sl", _fipn, 0, cm_print_sl, get_ui8, set_01, (float *)&cm.soft_limit_enable, SOFT_LIMIT_ENABLE }, - { "sys","lim", _fipn,0, cm_print_lim, get_ui8, set_01, (float *)&cm.limit_enable, HARD_LIMIT_ENABLE }, - { "sys","saf", _fipn,0, cm_print_saf, get_ui8, set_01, (float *)&cm.safety_interlock_enable, SAFETY_INTERLOCK_ENABLE }, - { "sys","m48e",_fipn,0, cm_print_m48e,get_ui8, set_01, (float *)&cm.gmx.m48_enable, 0 }, // M48/M49 feedrate & spindle override enable - { "sys","mfoe",_fipn,0, cm_print_mfoe,get_ui8, set_01, (float *)&cm.gmx.mfo_enable, FEED_OVERRIDE_ENABLE}, - { "sys","mfo", _fipn,3, cm_print_mfo, get_flt,cm_set_mfo,(float *)&cm.gmx.mfo_factor, FEED_OVERRIDE_FACTOR}, - { "sys","mtoe",_fipn,0, cm_print_mtoe,get_ui8, set_01, (float *)&cm.gmx.mto_enable, TRAVERSE_OVERRIDE_ENABLE}, - { "sys","mto", _fipn,3, cm_print_mto, get_flt,cm_set_mto,(float *)&cm.gmx.mto_factor, TRAVERSE_OVERRIDE_FACTOR}, - - // Power management - { "sys","mt", _fipn,2, st_print_mt, get_flt, st_set_mt, (float *)&st_cfg.motor_power_timeout, MOTOR_POWER_TIMEOUT}, - { "", "me", _f0, 0, st_print_me, st_set_me, st_set_me,(float *)&cs.null, 0 }, // SET to enable motors (null value sets to maintain compatability) - { "", "md", _f0, 0, st_print_md, st_set_md, st_set_md,(float *)&cs.null, 0 }, // SET to disable motors (null value sets to maintain compatability) + { "jid","jida",_d0, 0, tx_print_nul, get_data, set_data, &cfg.job_id[0], 0 }, + { "jid","jidb",_d0, 0, tx_print_nul, get_data, set_data, &cfg.job_id[1], 0 }, + { "jid","jidc",_d0, 0, tx_print_nul, get_data, set_data, &cfg.job_id[2], 0 }, + { "jid","jidd",_d0, 0, tx_print_nul, get_data, set_data, &cfg.job_id[3], 0 }, // Spindle functions - { "sys","spep",_fipn,0, cm_print_spep,get_ui8, set_01, (float *)&spindle.enable_polarity, SPINDLE_ENABLE_POLARITY }, - { "sys","spdp",_fipn,0, cm_print_spdp,get_ui8, set_01, (float *)&spindle.dir_polarity, SPINDLE_DIR_POLARITY }, - { "sys","spph",_fipn,0, cm_print_spph,get_ui8, set_01, (float *)&spindle.pause_on_hold, SPINDLE_PAUSE_ON_HOLD }, - { "sys","spdw",_fipn,2, cm_print_spdw,get_flt, set_flt, (float *)&spindle.dwell_seconds, SPINDLE_DWELL_TIME }, - { "sys","ssoe",_fipn,0, cm_print_ssoe,get_ui8, set_01, (float *)&spindle.sso_enable, SPINDLE_OVERRIDE_ENABLE}, - { "sys","sso", _fipn,3, cm_print_sso, get_flt,cm_set_sso,(float *)&spindle.sso_factor, SPINDLE_OVERRIDE_FACTOR}, - { "", "spe", _f0, 0, cm_print_spe, get_ui8, set_nul, (float *)&spindle.enable, 0 }, // get spindle enable - { "", "spd", _f0, 0, cm_print_spd, get_ui8,cm_set_dir,(float *)&spindle.direction, 0 }, // get spindle direction - { "", "sps", _f0, 0, cm_print_sps, get_flt, set_nul, (float *)&spindle.speed, 0 }, // get spindle speed + { "sp","spmo", _iip, 0, sp_print_spmo, sp_get_spmo, sp_set_spmo, nullptr, SPINDLE_MODE }, + { "sp","spph", _bip, 0, sp_print_spph, sp_get_spph, sp_set_spph, nullptr, SPINDLE_PAUSE_ON_HOLD }, + { "sp","spde", _fip, 2, sp_print_spde, sp_get_spde, sp_set_spde, nullptr, SPINDLE_SPINUP_DELAY }, + { "sp","spsn", _fip, 2, sp_print_spsn, sp_get_spsn, sp_set_spsn, nullptr, SPINDLE_SPEED_MIN}, + { "sp","spsm", _fip, 2, sp_print_spsm, sp_get_spsm, sp_set_spsm, nullptr, SPINDLE_SPEED_MAX}, + { "sp","spep", _iip, 0, sp_print_spep, sp_get_spep, sp_set_spep, nullptr, SPINDLE_ENABLE_POLARITY }, + { "sp","spdp", _iip, 0, sp_print_spdp, sp_get_spdp, sp_set_spdp, nullptr, SPINDLE_DIR_POLARITY }, + { "sp","spoe", _bip, 0, sp_print_spoe, sp_get_spoe, sp_set_spoe, nullptr, SPINDLE_OVERRIDE_ENABLE}, + { "sp","spo", _fip, 3, sp_print_spo, sp_get_spo, sp_set_spo, nullptr, SPINDLE_OVERRIDE_FACTOR}, + { "sp","spc", _i0, 0, sp_print_spc, sp_get_spc, sp_set_spc, nullptr, 0 }, // spindle state + { "sp","sps", _f0, 0, sp_print_sps, sp_get_sps, sp_set_sps, nullptr, 0 }, // spindle speed // Coolant functions - { "sys","cofp",_fipn,0, cm_print_cofp,get_ui8, set_01, (float *)&coolant.flood_polarity, COOLANT_FLOOD_POLARITY }, - { "sys","comp",_fipn,0, cm_print_comp,get_ui8, set_01, (float *)&coolant.mist_polarity, COOLANT_MIST_POLARITY }, - { "sys","coph",_fipn,0, cm_print_coph,get_ui8, set_01, (float *)&coolant.pause_on_hold, COOLANT_PAUSE_ON_HOLD }, - { "", "com", _f0, 0, cm_print_com, get_ui8, set_nul, (float *)&coolant.mist_enable, 0 }, // get mist coolant enable - { "", "cof", _f0, 0, cm_print_cof, get_ui8, set_nul, (float *)&coolant.flood_enable, 0 }, // get flood coolant enable + { "co","coph", _bip, 0, co_print_coph, co_get_coph, co_set_coph, nullptr, COOLANT_PAUSE_ON_HOLD }, + { "co","comp", _iip, 0, co_print_comp, co_get_comp, co_set_comp, nullptr, COOLANT_MIST_POLARITY }, + { "co","cofp", _iip, 0, co_print_cofp, co_get_cofp, co_set_cofp, nullptr, COOLANT_FLOOD_POLARITY }, + { "co","com", _i0, 0, co_print_com, co_get_com, co_set_com, nullptr, 0 }, // mist coolant enable + { "co","cof", _i0, 0, co_print_cof, co_get_cof, co_set_cof, nullptr, 0 }, // flood coolant enable + + // General system parameters + { "sys","jt", _fipn, 2, cm_print_jt, cm_get_jt, cm_set_jt, nullptr, JUNCTION_INTEGRATION_TIME }, + { "sys","ct", _fipnc,4, cm_print_ct, cm_get_ct, cm_set_ct, nullptr, CHORDAL_TOLERANCE }, + { "sys","zl", _fipnc,3, cm_print_zl, cm_get_zl, cm_set_zl, nullptr, FEEDHOLD_Z_LIFT }, + { "sys","sl", _bipn, 0, cm_print_sl, cm_get_sl, cm_set_sl, nullptr, SOFT_LIMIT_ENABLE }, + { "sys","lim", _bipn, 0, cm_print_lim, cm_get_lim, cm_set_lim, nullptr, HARD_LIMIT_ENABLE }, + { "sys","saf", _bipn, 0, cm_print_saf, cm_get_saf, cm_set_saf, nullptr, SAFETY_INTERLOCK_ENABLE }, + { "sys","m48", _bin, 0, cm_print_m48, cm_get_m48, cm_get_m48, nullptr, 1 }, // M48/M49 feedrate & spindle override enable + { "sys","froe",_bin, 0, cm_print_froe, cm_get_froe,cm_get_froe,nullptr, FEED_OVERRIDE_ENABLE}, + { "sys","fro", _fin, 3, cm_print_fro, cm_get_fro, cm_set_fro, nullptr, FEED_OVERRIDE_FACTOR}, + { "sys","troe",_bin, 0, cm_print_troe, cm_get_troe,cm_get_troe,nullptr, TRAVERSE_OVERRIDE_ENABLE}, + { "sys","tro", _fin, 3, cm_print_tro, cm_get_tro, cm_set_tro, nullptr, TRAVERSE_OVERRIDE_FACTOR}, + { "sys","mt", _fipn, 2, st_print_mt, st_get_mt, st_set_mt, nullptr, MOTOR_POWER_TIMEOUT}, // N is seconds of timeout + { "", "me", _f0, 0, st_print_me, get_nul, st_set_me, nullptr, 0 }, // SET to enable motors + { "", "md", _f0, 0, st_print_md, get_nul, st_set_md, nullptr, 0 }, // SET to disable motors // kinematics controls #if KINEMATICS==KINE_FOUR_CABLE - { "sys","knfc", _f0, 4, tx_print_nul, kn_get_force, kn_set_force, (float *)&cs.null, 0 }, - { "sys","knan", _f0, 0, tx_print_nul, kn_get_anchored, kn_set_anchored, (float *)&cs.null, 0 }, - { "sys","knpa", _f0, 4, tx_print_nul, kn_get_pos_a, set_nul, (float *)&cs.null, 0 }, - { "sys","knpb", _f0, 4, tx_print_nul, kn_get_pos_b, set_nul, (float *)&cs.null, 0 }, - { "sys","knpc", _f0, 4, tx_print_nul, kn_get_pos_c, set_nul, (float *)&cs.null, 0 }, - { "sys","knpd", _f0, 4, tx_print_nul, kn_get_pos_d, set_nul, (float *)&cs.null, 0 }, + { "sys","knfc", _f0, 4, tx_print_nul, kn_get_force, kn_set_force, nullptr, 0 }, + { "sys","knan", _f0, 0, tx_print_nul, kn_get_anchored, kn_set_anchored, nullptr, 0 }, + { "sys","knpa", _f0, 4, tx_print_nul, kn_get_pos_a, set_nul, nullptr, 0 }, + { "sys","knpb", _f0, 4, tx_print_nul, kn_get_pos_b, set_nul, nullptr, 0 }, + { "sys","knpc", _f0, 4, tx_print_nul, kn_get_pos_c, set_nul, nullptr, 0 }, + { "sys","knpd", _f0, 4, tx_print_nul, kn_get_pos_d, set_nul, nullptr, 0 }, #endif // Communications and reporting parameters #ifdef __TEXT_MODE - { "sys","tv", _fipn, 0, tx_print_tv, get_ui8, set_01, (float *)&txt.text_verbosity, TEXT_VERBOSITY }, + { "sys","tv", _iipn, 0, tx_print_tv, txt_get_tv, txt_set_tv, nullptr, TEXT_VERBOSITY }, #endif - { "sys","ej", _fipn, 0, js_print_ej, get_ui8, json_set_ej,(float *)&cs.comm_mode, COMM_MODE }, - { "sys","jv", _fipn, 0, js_print_jv, get_ui8, json_set_jv,(float *)&js.json_verbosity, JSON_VERBOSITY }, - { "sys","qv", _fipn, 0, qr_print_qv, get_ui8, set_012, (float *)&qr.queue_report_verbosity, QR_OFF}, // default to OFF, set to QUEUE_REPORT_VERBOSITY after connected - { "sys","sv", _fipn, 0, sr_print_sv, get_ui8, set_012, (float *)&sr.status_report_verbosity, SR_OFF}, // default to OFF, set to STATUS_REPORT_VERBOSITY after connectied - { "sys","si", _fipn, 0, sr_print_si, get_int, sr_set_si, (float *)&sr.status_report_interval, STATUS_REPORT_INTERVAL_MS }, - { "", "nxln", _f0, 0, cm_print_nxln,cm_get_nxln,cm_set_nxln,(float *)&cs.null, 0 }, + { "sys","ej", _iipn, 0, js_print_ej, js_get_ej, js_set_ej, nullptr, COMM_MODE }, + { "sys","jv", _iipn, 0, js_print_jv, js_get_jv, js_set_jv, nullptr, JSON_VERBOSITY }, + { "sys","qv", _iipn, 0, qr_print_qv, qr_get_qv, qr_set_qv, nullptr, QUEUE_REPORT_VERBOSITY }, + { "sys","sv", _iipn, 0, sr_print_sv, sr_get_sv, sr_set_sv, nullptr, STATUS_REPORT_VERBOSITY }, + { "sys","si", _iipn, 0, sr_print_si, sr_get_si, sr_set_si, nullptr, STATUS_REPORT_INTERVAL_MS }, // Gcode defaults // NOTE: The ordering within the gcode defaults is important for token resolution. gc must follow gco - { "sys","gpl", _fipn, 0, cm_print_gpl, get_ui8, set_012, (float *)&cm.default_select_plane, GCODE_DEFAULT_PLANE }, - { "sys","gun", _fipn, 0, cm_print_gun, get_ui8, set_01, (float *)&cm.default_units_mode, GCODE_DEFAULT_UNITS }, - { "sys","gco", _fipn, 0, cm_print_gco, get_ui8, set_ui8, (float *)&cm.default_coord_system, GCODE_DEFAULT_COORD_SYSTEM }, - { "sys","gpa", _fipn, 0, cm_print_gpa, get_ui8, set_012, (float *)&cm.default_path_control, GCODE_DEFAULT_PATH_CONTROL }, - { "sys","gdi", _fipn, 0, cm_print_gdi, get_ui8, set_01, (float *)&cm.default_distance_mode, GCODE_DEFAULT_DISTANCE_MODE }, - { "", "gc", _f0, 0, tx_print_nul, gc_get_gc,gc_run_gc,(float *)&cs.null, 0 }, // gcode block - must be last in this group + { "sys","gpl", _iipn, 0, cm_print_gpl, cm_get_gpl, cm_set_gpl, nullptr, GCODE_DEFAULT_PLANE }, + { "sys","gun", _iipn, 0, cm_print_gun, cm_get_gun, cm_set_gun, nullptr, GCODE_DEFAULT_UNITS }, + { "sys","gco", _iipn, 0, cm_print_gco, cm_get_gco, cm_set_gco, nullptr, GCODE_DEFAULT_COORD_SYSTEM }, + { "sys","gpa", _iipn, 0, cm_print_gpa, cm_get_gpa, cm_set_gpa, nullptr, GCODE_DEFAULT_PATH_CONTROL }, + { "sys","gdi", _iipn, 0, cm_print_gdi, cm_get_gdi, cm_set_gdi, nullptr, GCODE_DEFAULT_DISTANCE_MODE }, + { "", "gc2", _s0, 0, tx_print_nul, gc_get_gc, gc_run_gc, nullptr, 0 }, // send gcode to secondary planner + { "", "gc", _s0, 0, tx_print_nul, gc_get_gc, gc_run_gc, nullptr, 0 }, // gcode block - must be last in this group // Actions and Reports - { "", "sr", _f0, 0, sr_print_sr, sr_get, sr_set, (float *)&cs.null, 0 }, // request and set status reports - { "", "qr", _f0, 0, qr_print_qr, qr_get, set_ro, (float *)&cs.null, 0 }, // get queue value - planner buffers available - { "", "qi", _f0, 0, qr_print_qi, qi_get, set_ro, (float *)&cs.null, 0 }, // get queue value - buffers added to queue - { "", "qo", _f0, 0, qr_print_qo, qo_get, set_ro, (float *)&cs.null, 0 }, // get queue value - buffers removed from queue - { "", "er", _f0, 0, tx_print_nul, rpt_er, set_nul, (float *)&cs.null, 0 }, // get bogus exception report for testing - { "", "qf", _f0, 0, tx_print_nul, get_nul, cm_run_qf, (float *)&cs.null, 0 }, // SET to invoke queue flush - { "", "rx", _f0, 0, tx_print_int, get_rx, set_ro, (float *)&cs.null, 0 }, // get RX buffer bytes or packets - { "", "msg", _f0, 0, tx_print_str, get_nul, set_nul, (float *)&cs.null, 0 }, // string for generic messages - { "", "alarm",_f0,0, tx_print_nul, cm_alrm, cm_alrm, (float *)&cs.null, 0 }, // trigger alarm - { "", "panic",_f0,0, tx_print_nul, cm_pnic, cm_pnic, (float *)&cs.null, 0 }, // trigger panic - { "", "shutd",_f0,0, tx_print_nul, cm_shutd, cm_shutd, (float *)&cs.null, 0 }, // trigger shutdown - { "", "clear",_f0,0, tx_print_nul, cm_clr, cm_clr, (float *)&cs.null, 0 }, // GET "clear" to clear alarm state - { "", "clr", _f0,0, tx_print_nul, cm_clr, cm_clr, (float *)&cs.null, 0 }, // synonym for "clear" - { "", "tick", _f0,0, tx_print_int, get_tick, set_nul, (float *)&cs.null, 0 }, // get system time tic - { "", "tram", _f0,0, cm_print_tram, cm_get_tram, cm_set_tram, (float *)&cs.null, 0 },// SET to attempt setting rotation matrix from probes - { "", "defa",_f0, 0, tx_print_nul, help_defa, set_defaults,(float *)&cs.null,0 }, // set/print defaults / help screen - { "", "flash",_f0,0, tx_print_nul, help_flash,hw_flash, (float *)&cs.null,0 }, + { "", "sr", _n0, 0, sr_print_sr, sr_get, sr_set, nullptr, 0 }, // request and set status reports + { "", "qr", _n0, 0, qr_print_qr, qr_get, set_nul, nullptr, 0 }, // get queue value - planner buffers available + { "", "qi", _n0, 0, qr_print_qi, qi_get, set_nul, nullptr, 0 }, // get queue value - buffers added to queue + { "", "qo", _n0, 0, qr_print_qo, qo_get, set_nul, nullptr, 0 }, // get queue value - buffers removed from queue + { "", "er", _n0, 0, tx_print_nul, rpt_er, set_nul, nullptr, 0 }, // get bogus exception report for testing + { "", "rx", _n0, 0, tx_print_int, get_rx, set_nul, nullptr, 0 }, // get RX buffer bytes or packets + { "", "dw", _i0, 0, tx_print_int, st_get_dw, set_noop, nullptr, 0 }, // get dwell time remaining + { "", "msg", _s0, 0, tx_print_str, get_nul, set_noop, nullptr, 0 }, // no operation on messages + { "", "alarm",_n0, 0, tx_print_nul, cm_alrm, cm_alrm, nullptr, 0 }, // trigger alarm + { "", "panic",_n0, 0, tx_print_nul, cm_pnic, cm_pnic, nullptr, 0 }, // trigger panic + { "", "shutd",_n0, 0, tx_print_nul, cm_shutd, cm_shutd, nullptr, 0 }, // trigger shutdown + { "", "clear",_n0, 0, tx_print_nul, cm_clr, cm_clr, nullptr, 0 }, // GET "clear" to clear alarm state + { "", "clr", _n0, 0, tx_print_nul, cm_clr, cm_clr, nullptr, 0 }, // synonym for "clear" + { "", "tick", _n0, 0, tx_print_int, get_tick, set_nul, nullptr, 0 }, // get system time tic + { "", "tram", _b0, 0, cm_print_tram,cm_get_tram,cm_set_tram,nullptr,0 }, // SET to attempt setting rotation matrix from probes + { "", "defa", _b0, 0, tx_print_nul, help_defa,set_defaults,nullptr,0 }, // set/print defaults / help screen + { "", "flash",_b0, 0, tx_print_nul, help_flash,hw_flash, nullptr, 0 }, #ifdef __HELP_SCREENS - { "", "help",_f0, 0, tx_print_nul, help_config, set_nul, (float *)&cs.null,0 }, // prints config help screen - { "", "h", _f0, 0, tx_print_nul, help_config, set_nul, (float *)&cs.null,0 }, // alias for "help" + { "", "help",_b0, 0, tx_print_nul, help_config, set_nul, nullptr, 0 }, // prints config help screen + { "", "h", _b0, 0, tx_print_nul, help_config, set_nul, nullptr, 0 }, // alias for "help" #endif #ifdef __USER_DATA // User defined data groups - { "uda","uda0", _fip, 0, tx_print_int, get_data, set_data,(float *)&cfg.user_data_a[0], USER_DATA_A0 }, - { "uda","uda1", _fip, 0, tx_print_int, get_data, set_data,(float *)&cfg.user_data_a[1], USER_DATA_A1 }, - { "uda","uda2", _fip, 0, tx_print_int, get_data, set_data,(float *)&cfg.user_data_a[2], USER_DATA_A2 }, - { "uda","uda3", _fip, 0, tx_print_int, get_data, set_data,(float *)&cfg.user_data_a[3], USER_DATA_A3 }, + { "uda","uda0", _fip, 0, tx_print_int, get_data, set_data, &cfg.user_data_a[0], USER_DATA_A0 }, + { "uda","uda1", _fip, 0, tx_print_int, get_data, set_data, &cfg.user_data_a[1], USER_DATA_A1 }, + { "uda","uda2", _fip, 0, tx_print_int, get_data, set_data, &cfg.user_data_a[2], USER_DATA_A2 }, + { "uda","uda3", _fip, 0, tx_print_int, get_data, set_data, &cfg.user_data_a[3], USER_DATA_A3 }, - { "udb","udb0", _fip, 0, tx_print_int, get_data, set_data,(float *)&cfg.user_data_b[0], USER_DATA_B0 }, - { "udb","udb1", _fip, 0, tx_print_int, get_data, set_data,(float *)&cfg.user_data_b[1], USER_DATA_B1 }, - { "udb","udb2", _fip, 0, tx_print_int, get_data, set_data,(float *)&cfg.user_data_b[2], USER_DATA_B2 }, - { "udb","udb3", _fip, 0, tx_print_int, get_data, set_data,(float *)&cfg.user_data_b[3], USER_DATA_B3 }, + { "udb","udb0", _fip, 0, tx_print_int, get_data, set_data, &cfg.user_data_b[0], USER_DATA_B0 }, + { "udb","udb1", _fip, 0, tx_print_int, get_data, set_data, &cfg.user_data_b[1], USER_DATA_B1 }, + { "udb","udb2", _fip, 0, tx_print_int, get_data, set_data, &cfg.user_data_b[2], USER_DATA_B2 }, + { "udb","udb3", _fip, 0, tx_print_int, get_data, set_data, &cfg.user_data_b[3], USER_DATA_B3 }, - { "udc","udc0", _fip, 0, tx_print_int, get_data, set_data,(float *)&cfg.user_data_c[0], USER_DATA_C0 }, - { "udc","udc1", _fip, 0, tx_print_int, get_data, set_data,(float *)&cfg.user_data_c[1], USER_DATA_C1 }, - { "udc","udc2", _fip, 0, tx_print_int, get_data, set_data,(float *)&cfg.user_data_c[2], USER_DATA_C2 }, - { "udc","udc3", _fip, 0, tx_print_int, get_data, set_data,(float *)&cfg.user_data_c[3], USER_DATA_C3 }, + { "udc","udc0", _fip, 0, tx_print_int, get_data, set_data, &cfg.user_data_c[0], USER_DATA_C0 }, + { "udc","udc1", _fip, 0, tx_print_int, get_data, set_data, &cfg.user_data_c[1], USER_DATA_C1 }, + { "udc","udc2", _fip, 0, tx_print_int, get_data, set_data, &cfg.user_data_c[2], USER_DATA_C2 }, + { "udc","udc3", _fip, 0, tx_print_int, get_data, set_data, &cfg.user_data_c[3], USER_DATA_C3 }, - { "udd","udd0", _fip, 0, tx_print_int, get_data, set_data,(float *)&cfg.user_data_d[0], USER_DATA_D0 }, - { "udd","udd1", _fip, 0, tx_print_int, get_data, set_data,(float *)&cfg.user_data_d[1], USER_DATA_D1 }, - { "udd","udd2", _fip, 0, tx_print_int, get_data, set_data,(float *)&cfg.user_data_d[2], USER_DATA_D2 }, - { "udd","udd3", _fip, 0, tx_print_int, get_data, set_data,(float *)&cfg.user_data_d[3], USER_DATA_D3 }, + { "udd","udd0", _fip, 0, tx_print_int, get_data, set_data, &cfg.user_data_d[0], USER_DATA_D0 }, + { "udd","udd1", _fip, 0, tx_print_int, get_data, set_data, &cfg.user_data_d[1], USER_DATA_D1 }, + { "udd","udd2", _fip, 0, tx_print_int, get_data, set_data, &cfg.user_data_d[2], USER_DATA_D2 }, + { "udd","udd3", _fip, 0, tx_print_int, get_data, set_data, &cfg.user_data_d[3], USER_DATA_D3 }, #endif + // Tool table offsets + { "tof","tofx",_fipc, 5, cm_print_cofs, cm_get_tof, cm_set_tof, nullptr, 0 }, + { "tof","tofy",_fipc, 5, cm_print_cofs, cm_get_tof, cm_set_tof, nullptr, 0 }, + { "tof","tofz",_fipc, 5, cm_print_cofs, cm_get_tof, cm_set_tof, nullptr, 0 }, + { "tof","tofu",_fipc, 5, cm_print_cofs, cm_get_tof, cm_set_tof, nullptr, 0 }, + { "tof","tofv",_fipc, 5, cm_print_cofs, cm_get_tof, cm_set_tof, nullptr, 0 }, + { "tof","tofw",_fipc, 5, cm_print_cofs, cm_get_tof, cm_set_tof, nullptr, 0 }, + { "tof","tofa",_fipc, 5, cm_print_cofs, cm_get_tof, cm_set_tof, nullptr, 0 }, + { "tof","tofb",_fipc, 5, cm_print_cofs, cm_get_tof, cm_set_tof, nullptr, 0 }, + { "tof","tofc",_fipc, 5, cm_print_cofs, cm_get_tof, cm_set_tof, nullptr, 0 }, + + // Tool table + { "tt1","tt1x",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT1_X_OFFSET }, + { "tt1","tt1y",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT1_Y_OFFSET }, + { "tt1","tt1z",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT1_Z_OFFSET }, + { "tt1","tt1u",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT1_U_OFFSET }, + { "tt1","tt1v",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT1_V_OFFSET }, + { "tt1","tt1w",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT1_W_OFFSET }, + { "tt1","tt1a",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT1_A_OFFSET }, + { "tt1","tt1b",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT1_B_OFFSET }, + { "tt1","tt1c",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT1_C_OFFSET }, + + { "tt2","tt2x",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT2_X_OFFSET }, + { "tt2","tt2y",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT2_Y_OFFSET }, + { "tt2","tt2z",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT2_Z_OFFSET }, + { "tt2","tt2u",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT2_U_OFFSET }, + { "tt2","tt2v",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT2_V_OFFSET }, + { "tt2","tt2w",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT2_W_OFFSET }, + { "tt2","tt2a",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT2_A_OFFSET }, + { "tt2","tt2b",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT2_B_OFFSET }, + { "tt2","tt2c",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT2_C_OFFSET }, + + { "tt3","tt3x",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT3_X_OFFSET }, + { "tt3","tt3y",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT3_Y_OFFSET }, + { "tt3","tt3z",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT3_Z_OFFSET }, + { "tt3","tt3u",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT3_U_OFFSET }, + { "tt3","tt3v",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT3_V_OFFSET }, + { "tt3","tt3w",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT3_W_OFFSET }, + { "tt3","tt3a",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT3_A_OFFSET }, + { "tt3","tt3b",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT3_B_OFFSET }, + { "tt3","tt3c",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT1_C_OFFSET }, + + { "tt4","tt4x",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT4_X_OFFSET }, + { "tt4","tt4y",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT4_Y_OFFSET }, + { "tt4","tt4z",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT4_Z_OFFSET }, + { "tt4","tt4u",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT4_U_OFFSET }, + { "tt4","tt4v",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT4_V_OFFSET }, + { "tt4","tt4w",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT4_W_OFFSET }, + { "tt4","tt4a",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT4_A_OFFSET }, + { "tt4","tt4b",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT4_B_OFFSET }, + { "tt4","tt4c",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT4_C_OFFSET }, + + { "tt5","tt5x",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT5_X_OFFSET }, + { "tt5","tt5y",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT5_Y_OFFSET }, + { "tt5","tt5z",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT5_Z_OFFSET }, + { "tt5","tt5u",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT5_U_OFFSET }, + { "tt5","tt5v",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT5_V_OFFSET }, + { "tt5","tt5w",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT5_W_OFFSET }, + { "tt5","tt5a",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT5_A_OFFSET }, + { "tt5","tt5b",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT5_B_OFFSET }, + { "tt5","tt5c",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT5_C_OFFSET }, + + { "tt6","tt6x",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT6_X_OFFSET }, + { "tt6","tt6y",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT6_Y_OFFSET }, + { "tt6","tt6z",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT6_Z_OFFSET }, + { "tt6","tt6u",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT6_U_OFFSET }, + { "tt6","tt6v",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT6_V_OFFSET }, + { "tt6","tt6w",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT6_W_OFFSET }, + { "tt6","tt6a",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT6_A_OFFSET }, + { "tt6","tt6b",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT6_B_OFFSET }, + { "tt6","tt6c",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT6_C_OFFSET }, + + { "tt7","tt7x",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT7_X_OFFSET }, + { "tt7","tt7y",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT7_Y_OFFSET }, + { "tt7","tt7z",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT7_Z_OFFSET }, + { "tt7","tt7u",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT7_U_OFFSET }, + { "tt7","tt7v",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT7_V_OFFSET }, + { "tt7","tt7w",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT7_W_OFFSET }, + { "tt7","tt7a",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT7_A_OFFSET }, + { "tt7","tt7b",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT7_B_OFFSET }, + { "tt7","tt7c",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT7_C_OFFSET }, + + { "tt8","tt8x",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT8_X_OFFSET }, + { "tt8","tt8y",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT8_Y_OFFSET }, + { "tt8","tt8z",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT8_Z_OFFSET }, + { "tt8","tt8u",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT8_U_OFFSET }, + { "tt8","tt8v",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT8_V_OFFSET }, + { "tt8","tt8w",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT8_W_OFFSET }, + { "tt8","tt8a",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT8_A_OFFSET }, + { "tt8","tt8b",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT8_B_OFFSET }, + { "tt8","tt8c",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT8_C_OFFSET }, + + { "tt9","tt9x",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT9_X_OFFSET }, + { "tt9","tt9y",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT9_Y_OFFSET }, + { "tt9","tt9z",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT9_Z_OFFSET }, + { "tt9","tt9u",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT9_U_OFFSET }, + { "tt9","tt9v",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT9_V_OFFSET }, + { "tt9","tt9w",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT9_W_OFFSET }, + { "tt9","tt9a",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT9_A_OFFSET }, + { "tt9","tt9b",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT9_B_OFFSET }, + { "tt9","tt9c",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT9_C_OFFSET }, + + { "tt10","tt10x",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT10_X_OFFSET }, + { "tt10","tt10y",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT10_Y_OFFSET }, + { "tt10","tt10z",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT10_Z_OFFSET }, + { "tt10","tt10u",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT10_U_OFFSET }, + { "tt10","tt10v",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT10_V_OFFSET }, + { "tt10","tt10w",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT10_W_OFFSET }, + { "tt10","tt10a",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT10_A_OFFSET }, + { "tt10","tt10b",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT10_B_OFFSET }, + { "tt10","tt10c",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT10_C_OFFSET }, + + { "tt11","tt11x",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT11_X_OFFSET }, + { "tt11","tt11y",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT11_Y_OFFSET }, + { "tt11","tt11z",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT11_Z_OFFSET }, + { "tt11","tt11u",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT11_U_OFFSET }, + { "tt11","tt11v",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT11_V_OFFSET }, + { "tt11","tt11w",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT11_W_OFFSET }, + { "tt11","tt11a",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT11_A_OFFSET }, + { "tt11","tt11b",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT11_B_OFFSET }, + { "tt11","tt11c",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT11_C_OFFSET }, + + { "tt12","tt12x",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT12_X_OFFSET }, + { "tt12","tt12y",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT12_Y_OFFSET }, + { "tt12","tt12z",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT12_Z_OFFSET }, + { "tt12","tt12u",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT12_U_OFFSET }, + { "tt12","tt12v",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT12_V_OFFSET }, + { "tt12","tt12w",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT12_W_OFFSET }, + { "tt12","tt12a",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT12_A_OFFSET }, + { "tt12","tt12b",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT12_B_OFFSET }, + { "tt12","tt12c",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT12_C_OFFSET }, + + { "tt13","tt13x",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT13_X_OFFSET }, + { "tt13","tt13y",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT13_Y_OFFSET }, + { "tt13","tt13z",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT13_Z_OFFSET }, + { "tt13","tt13u",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT13_U_OFFSET }, + { "tt13","tt13v",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT13_V_OFFSET }, + { "tt13","tt13w",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT13_W_OFFSET }, + { "tt13","tt13a",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT13_A_OFFSET }, + { "tt13","tt13b",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT13_B_OFFSET }, + { "tt13","tt13c",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT13_C_OFFSET }, + + { "tt14","tt14x",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT14_X_OFFSET }, + { "tt14","tt14y",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT14_Y_OFFSET }, + { "tt14","tt14z",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT14_Z_OFFSET }, + { "tt14","tt14u",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT14_U_OFFSET }, + { "tt14","tt14v",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT14_V_OFFSET }, + { "tt14","tt14w",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT14_W_OFFSET }, + { "tt14","tt14a",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT14_A_OFFSET }, + { "tt14","tt14b",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT14_B_OFFSET }, + { "tt14","tt14c",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT14_C_OFFSET }, + + { "tt15","tt15x",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT15_X_OFFSET }, + { "tt15","tt15y",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT15_Y_OFFSET }, + { "tt15","tt15z",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT15_Z_OFFSET }, + { "tt15","tt15u",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT15_U_OFFSET }, + { "tt15","tt15v",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT15_V_OFFSET }, + { "tt15","tt15w",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT15_W_OFFSET }, + { "tt15","tt15a",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT15_A_OFFSET }, + { "tt15","tt15b",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT15_B_OFFSET }, + { "tt15","tt15c",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT15_C_OFFSET }, + + { "tt16","tt16x",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT16_X_OFFSET }, + { "tt16","tt16y",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT16_Y_OFFSET }, + { "tt16","tt16z",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT16_Z_OFFSET }, + { "tt16","tt16u",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT16_U_OFFSET }, + { "tt16","tt16v",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT16_V_OFFSET }, + { "tt16","tt16w",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT16_W_OFFSET }, + { "tt16","tt16a",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT16_A_OFFSET }, + { "tt16","tt16b",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT16_B_OFFSET }, + { "tt16","tt16c",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT16_C_OFFSET }, + + { "tt17","tt17x",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT17_X_OFFSET }, + { "tt17","tt17y",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT17_Y_OFFSET }, + { "tt17","tt17z",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT17_Z_OFFSET }, + { "tt17","tt17u",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT17_U_OFFSET }, + { "tt17","tt17v",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT17_V_OFFSET }, + { "tt17","tt17w",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT17_W_OFFSET }, + { "tt17","tt17a",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT17_A_OFFSET }, + { "tt17","tt17b",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT17_B_OFFSET }, + { "tt17","tt17c",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT17_C_OFFSET }, + + { "tt18","tt18x",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT18_X_OFFSET }, + { "tt18","tt18y",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT18_Y_OFFSET }, + { "tt18","tt18z",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT18_Z_OFFSET }, + { "tt18","tt18u",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT18_U_OFFSET }, + { "tt18","tt18v",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT18_V_OFFSET }, + { "tt18","tt18w",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT18_W_OFFSET }, + { "tt18","tt18a",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT18_A_OFFSET }, + { "tt18","tt18b",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT18_B_OFFSET }, + { "tt18","tt18c",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT18_C_OFFSET }, + + { "tt19","tt19x",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT19_X_OFFSET }, + { "tt19","tt19y",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT19_Y_OFFSET }, + { "tt19","tt19z",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT19_Z_OFFSET }, + { "tt19","tt19u",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT19_U_OFFSET }, + { "tt19","tt19v",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT19_V_OFFSET }, + { "tt19","tt19w",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT19_W_OFFSET }, + { "tt19","tt19a",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT19_A_OFFSET }, + { "tt19","tt19b",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT19_B_OFFSET }, + { "tt19","tt19c",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT19_C_OFFSET }, + + { "tt20","tt20x",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT20_X_OFFSET }, + { "tt20","tt20y",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT20_Y_OFFSET }, + { "tt20","tt20z",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT20_Z_OFFSET }, + { "tt20","tt20u",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT20_U_OFFSET }, + { "tt20","tt20v",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT20_V_OFFSET }, + { "tt20","tt20w",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT20_W_OFFSET }, + { "tt20","tt20a",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT20_A_OFFSET }, + { "tt20","tt20b",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT20_B_OFFSET }, + { "tt20","tt20c",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT20_C_OFFSET }, + + { "tt21","tt21x",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT21_X_OFFSET }, + { "tt21","tt21y",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT21_Y_OFFSET }, + { "tt21","tt21z",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT21_Z_OFFSET }, + { "tt21","tt21u",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT21_U_OFFSET }, + { "tt21","tt21v",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT21_V_OFFSET }, + { "tt21","tt21w",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT21_W_OFFSET }, + { "tt21","tt21a",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT21_A_OFFSET }, + { "tt21","tt21b",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT21_B_OFFSET }, + { "tt21","tt21c",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT21_C_OFFSET }, + + { "tt22","tt22x",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT22_X_OFFSET }, + { "tt22","tt22y",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT22_Y_OFFSET }, + { "tt22","tt22z",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT22_Z_OFFSET }, + { "tt22","tt22u",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT22_U_OFFSET }, + { "tt22","tt22v",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT22_V_OFFSET }, + { "tt22","tt22w",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT22_W_OFFSET }, + { "tt22","tt22a",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT22_A_OFFSET }, + { "tt22","tt22b",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT22_B_OFFSET }, + { "tt22","tt22c",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT22_C_OFFSET }, + + { "tt23","tt23x",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT23_X_OFFSET }, + { "tt23","tt23y",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT23_Y_OFFSET }, + { "tt23","tt23z",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT23_Z_OFFSET }, + { "tt23","tt23u",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT23_U_OFFSET }, + { "tt23","tt23v",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT23_V_OFFSET }, + { "tt23","tt23w",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT23_W_OFFSET }, + { "tt23","tt23a",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT23_A_OFFSET }, + { "tt23","tt23b",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT23_B_OFFSET }, + { "tt23","tt23c",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT23_C_OFFSET }, + + { "tt24","tt24x",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT24_X_OFFSET }, + { "tt24","tt24y",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT24_Y_OFFSET }, + { "tt24","tt24z",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT24_Z_OFFSET }, + { "tt24","tt24u",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT24_U_OFFSET }, + { "tt24","tt24v",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT24_V_OFFSET }, + { "tt24","tt24w",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT24_W_OFFSET }, + { "tt24","tt24a",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT24_A_OFFSET }, + { "tt24","tt24b",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT24_B_OFFSET }, + { "tt24","tt24c",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT24_C_OFFSET }, + + { "tt25","tt25x",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT25_X_OFFSET }, + { "tt25","tt25y",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT25_Y_OFFSET }, + { "tt25","tt25z",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT25_Z_OFFSET }, + { "tt25","tt25u",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT25_U_OFFSET }, + { "tt25","tt25v",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT25_V_OFFSET }, + { "tt25","tt25w",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT25_W_OFFSET }, + { "tt25","tt25a",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT25_A_OFFSET }, + { "tt25","tt25b",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT25_B_OFFSET }, + { "tt25","tt25c",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT25_C_OFFSET }, + + { "tt26","tt26x",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT26_X_OFFSET }, + { "tt26","tt26y",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT26_Y_OFFSET }, + { "tt26","tt26z",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT26_Z_OFFSET }, + { "tt26","tt26u",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT26_U_OFFSET }, + { "tt26","tt26v",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT26_V_OFFSET }, + { "tt26","tt26w",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT26_W_OFFSET }, + { "tt26","tt26a",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT26_A_OFFSET }, + { "tt26","tt26b",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT26_B_OFFSET }, + { "tt26","tt26c",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT26_C_OFFSET }, + + { "tt27","tt27x",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT27_X_OFFSET }, + { "tt27","tt27y",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT27_Y_OFFSET }, + { "tt27","tt27z",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT27_Z_OFFSET }, + { "tt27","tt27u",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT27_U_OFFSET }, + { "tt27","tt27v",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT27_V_OFFSET }, + { "tt27","tt27w",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT27_W_OFFSET }, + { "tt27","tt27a",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT27_A_OFFSET }, + { "tt27","tt27b",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT27_B_OFFSET }, + { "tt27","tt27c",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT27_C_OFFSET }, + + { "tt28","tt28x",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT28_X_OFFSET }, + { "tt28","tt28y",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT28_Y_OFFSET }, + { "tt28","tt28z",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT28_Z_OFFSET }, + { "tt28","tt28u",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT28_U_OFFSET }, + { "tt28","tt28v",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT28_V_OFFSET }, + { "tt28","tt28w",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT28_W_OFFSET }, + { "tt28","tt28a",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT28_A_OFFSET }, + { "tt28","tt28b",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT28_B_OFFSET }, + { "tt28","tt28c",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT28_C_OFFSET }, + + { "tt29","tt29x",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT29_X_OFFSET }, + { "tt29","tt29y",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT29_Y_OFFSET }, + { "tt29","tt29z",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT29_Z_OFFSET }, + { "tt29","tt29u",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT29_U_OFFSET }, + { "tt29","tt29v",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT29_V_OFFSET }, + { "tt29","tt29w",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT29_W_OFFSET }, + { "tt29","tt29a",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT29_A_OFFSET }, + { "tt29","tt29b",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT29_B_OFFSET }, + { "tt29","tt29c",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT29_C_OFFSET }, + + { "tt30","tt30x",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT30_X_OFFSET }, + { "tt30","tt30y",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT30_Y_OFFSET }, + { "tt30","tt30z",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT30_Z_OFFSET }, + { "tt30","tt30u",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT30_U_OFFSET }, + { "tt30","tt30v",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT30_V_OFFSET }, + { "tt30","tt30w",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT30_W_OFFSET }, + { "tt30","tt30a",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT30_A_OFFSET }, + { "tt30","tt30b",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT30_B_OFFSET }, + { "tt30","tt30c",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT30_C_OFFSET }, + + { "tt31","tt31x",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT31_X_OFFSET }, + { "tt31","tt31y",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT31_Y_OFFSET }, + { "tt31","tt31z",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT31_Z_OFFSET }, + { "tt31","tt31u",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT31_U_OFFSET }, + { "tt31","tt31v",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT31_V_OFFSET }, + { "tt31","tt31w",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT31_W_OFFSET }, + { "tt31","tt31a",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT31_A_OFFSET }, + { "tt31","tt31b",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT31_B_OFFSET }, + { "tt31","tt31c",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT31_C_OFFSET }, + + { "tt32","tt32x",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT32_X_OFFSET }, + { "tt32","tt32y",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT32_Y_OFFSET }, + { "tt32","tt32z",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT32_Z_OFFSET }, + { "tt32","tt32u",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT32_U_OFFSET }, + { "tt32","tt32v",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT32_V_OFFSET }, + { "tt32","tt32w",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT32_W_OFFSET }, + { "tt32","tt32a",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT32_A_OFFSET }, + { "tt32","tt32b",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT32_B_OFFSET }, + { "tt32","tt32c",_fipc, 5, cm_print_cofs, cm_get_tt, cm_set_tt, nullptr, TT32_C_OFFSET }, + // Diagnostic parameters #ifdef __DIAGNOSTIC_PARAMETERS - { "", "clc",_f0, 0, tx_print_nul, st_clc, st_clc, (float *)&cs.null, 0 }, // clear diagnostic step counters - { "", "_dam",_f0, 0, tx_print_nul, cm_dam, cm_dam, (float *)&cs.null, 0 }, // dump active model + { "", "clc",_f0, 0, tx_print_nul, st_clc, st_clc, nullptr, 0 }, // clear diagnostic step counters - { "_te","_tex",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.target[AXIS_X], 0 }, // X target endpoint - { "_te","_tey",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.target[AXIS_Y], 0 }, - { "_te","_tez",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.target[AXIS_Z], 0 }, - { "_te","_tea",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.target[AXIS_A], 0 }, - { "_te","_teb",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.target[AXIS_B], 0 }, - { "_te","_tec",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.target[AXIS_C], 0 }, + { "_te","_tex",_f0, 2, tx_print_flt, get_flt, set_nul, &mr->target[AXIS_X], 0 }, // X target endpoint + { "_te","_tey",_f0, 2, tx_print_flt, get_flt, set_nul, &mr->target[AXIS_Y], 0 }, + { "_te","_tez",_f0, 2, tx_print_flt, get_flt, set_nul, &mr->target[AXIS_Z], 0 }, + { "_te","_tea",_f0, 2, tx_print_flt, get_flt, set_nul, &mr->target[AXIS_A], 0 }, + { "_te","_teb",_f0, 2, tx_print_flt, get_flt, set_nul, &mr->target[AXIS_B], 0 }, + { "_te","_tec",_f0, 2, tx_print_flt, get_flt, set_nul, &mr->target[AXIS_C], 0 }, - { "_tr","_trx",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.gm.target[AXIS_X], 0 }, // X target runtime - { "_tr","_try",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.gm.target[AXIS_Y], 0 }, - { "_tr","_trz",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.gm.target[AXIS_Z], 0 }, - { "_tr","_tra",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.gm.target[AXIS_A], 0 }, - { "_tr","_trb",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.gm.target[AXIS_B], 0 }, - { "_tr","_trc",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.gm.target[AXIS_C], 0 }, + { "_tr","_trx",_f0, 2, tx_print_flt, get_flt, set_nul, &mr->gm.target[AXIS_X], 0 }, // X target runtime + { "_tr","_try",_f0, 2, tx_print_flt, get_flt, set_nul, &mr->gm.target[AXIS_Y], 0 }, + { "_tr","_trz",_f0, 2, tx_print_flt, get_flt, set_nul, &mr->gm.target[AXIS_Z], 0 }, + { "_tr","_tra",_f0, 2, tx_print_flt, get_flt, set_nul, &mr->gm.target[AXIS_A], 0 }, + { "_tr","_trb",_f0, 2, tx_print_flt, get_flt, set_nul, &mr->gm.target[AXIS_B], 0 }, + { "_tr","_trc",_f0, 2, tx_print_flt, get_flt, set_nul, &mr->gm.target[AXIS_C], 0 }, #if (MOTORS >= 1) - { "_ts","_ts1",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.target_steps[MOTOR_1], 0 }, // Motor 1 target steps - { "_ps","_ps1",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.position_steps[MOTOR_1], 0 }, // Motor 1 position steps - { "_cs","_cs1",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.commanded_steps[MOTOR_1], 0 }, // Motor 1 commanded steps (delayed steps) - { "_es","_es1",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.encoder_steps[MOTOR_1], 0 }, // Motor 1 encoder steps - { "_xs","_xs1",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&st_pre.mot[MOTOR_1].corrected_steps, 0 }, // Motor 1 correction steps applied - { "_fe","_fe1",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.following_error[MOTOR_1], 0 }, // Motor 1 following error in steps + { "_ts","_ts1",_f0, 2, tx_print_flt, get_flt, set_nul, &mr->target_steps[MOTOR_1], 0 }, // Motor 1 target steps + { "_ps","_ps1",_f0, 2, tx_print_flt, get_flt, set_nul, &mr->position_steps[MOTOR_1], 0 }, // Motor 1 position steps + { "_cs","_cs1",_f0, 2, tx_print_flt, get_flt, set_nul, &mr->commanded_steps[MOTOR_1], 0 }, // Motor 1 commanded steps (delayed steps) + { "_es","_es1",_f0, 2, tx_print_flt, get_flt, set_nul, &mr->encoder_steps[MOTOR_1], 0 }, // Motor 1 encoder steps + { "_xs","_xs1",_f0, 2, tx_print_flt, get_flt, set_nul, &st_pre.mot[MOTOR_1].corrected_steps, 0 }, // Motor 1 correction steps applied + { "_fe","_fe1",_f0, 2, tx_print_flt, get_flt, set_nul, &mr->following_error[MOTOR_1], 0 }, // Motor 1 following error in steps #endif #if (MOTORS >= 2) - { "_ts","_ts2",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.target_steps[MOTOR_2], 0 }, - { "_ps","_ps2",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.position_steps[MOTOR_2], 0 }, - { "_cs","_cs2",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.commanded_steps[MOTOR_2], 0 }, - { "_es","_es2",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.encoder_steps[MOTOR_2], 0 }, - { "_xs","_xs2",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&st_pre.mot[MOTOR_2].corrected_steps, 0 }, - { "_fe","_fe2",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.following_error[MOTOR_2], 0 }, + { "_ts","_ts2",_f0, 2, tx_print_flt, get_flt, set_nul, &mr->target_steps[MOTOR_2], 0 }, + { "_ps","_ps2",_f0, 2, tx_print_flt, get_flt, set_nul, &mr->position_steps[MOTOR_2], 0 }, + { "_cs","_cs2",_f0, 2, tx_print_flt, get_flt, set_nul, &mr->commanded_steps[MOTOR_2], 0 }, + { "_es","_es2",_f0, 2, tx_print_flt, get_flt, set_nul, &mr->encoder_steps[MOTOR_2], 0 }, + { "_xs","_xs2",_f0, 2, tx_print_flt, get_flt, set_nul, &st_pre.mot[MOTOR_2].corrected_steps, 0 }, + { "_fe","_fe2",_f0, 2, tx_print_flt, get_flt, set_nul, &mr->following_error[MOTOR_2], 0 }, #endif #if (MOTORS >= 3) - { "_ts","_ts3",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.target_steps[MOTOR_3], 0 }, - { "_ps","_ps3",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.position_steps[MOTOR_3], 0 }, - { "_cs","_cs3",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.commanded_steps[MOTOR_3], 0 }, - { "_es","_es3",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.encoder_steps[MOTOR_3], 0 }, - { "_xs","_xs3",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&st_pre.mot[MOTOR_3].corrected_steps, 0 }, - { "_fe","_fe3",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.following_error[MOTOR_3], 0 }, + { "_ts","_ts3",_f0, 2, tx_print_flt, get_flt, set_nul, &mr->target_steps[MOTOR_3], 0 }, + { "_ps","_ps3",_f0, 2, tx_print_flt, get_flt, set_nul, &mr->position_steps[MOTOR_3], 0 }, + { "_cs","_cs3",_f0, 2, tx_print_flt, get_flt, set_nul, &mr->commanded_steps[MOTOR_3], 0 }, + { "_es","_es3",_f0, 2, tx_print_flt, get_flt, set_nul, &mr->encoder_steps[MOTOR_3], 0 }, + { "_xs","_xs3",_f0, 2, tx_print_flt, get_flt, set_nul, &st_pre.mot[MOTOR_3].corrected_steps, 0 }, + { "_fe","_fe3",_f0, 2, tx_print_flt, get_flt, set_nul, &mr->following_error[MOTOR_3], 0 }, #endif #if (MOTORS >= 4) - { "_ts","_ts4",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.target_steps[MOTOR_4], 0 }, - { "_ps","_ps4",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.position_steps[MOTOR_4], 0 }, - { "_cs","_cs4",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.commanded_steps[MOTOR_4], 0 }, - { "_es","_es4",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.encoder_steps[MOTOR_4], 0 }, - { "_xs","_xs4",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&st_pre.mot[MOTOR_4].corrected_steps, 0 }, - { "_fe","_fe4",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.following_error[MOTOR_4], 0 }, + { "_ts","_ts4",_f0, 2, tx_print_flt, get_flt, set_nul, &mr->target_steps[MOTOR_4], 0 }, + { "_ps","_ps4",_f0, 2, tx_print_flt, get_flt, set_nul, &mr->position_steps[MOTOR_4], 0 }, + { "_cs","_cs4",_f0, 2, tx_print_flt, get_flt, set_nul, &mr->commanded_steps[MOTOR_4], 0 }, + { "_es","_es4",_f0, 2, tx_print_flt, get_flt, set_nul, &mr->encoder_steps[MOTOR_4], 0 }, + { "_xs","_xs4",_f0, 2, tx_print_flt, get_flt, set_nul, &st_pre.mot[MOTOR_4].corrected_steps, 0 }, + { "_fe","_fe4",_f0, 2, tx_print_flt, get_flt, set_nul, &mr->following_error[MOTOR_4], 0 }, #endif #if (MOTORS >= 5) - { "_ts","_ts5",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.target_steps[MOTOR_5], 0 }, - { "_ps","_ps5",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.position_steps[MOTOR_5], 0 }, - { "_cs","_cs5",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.commanded_steps[MOTOR_5], 0 }, - { "_es","_es5",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.encoder_steps[MOTOR_5], 0 }, - { "_xs","_xs6",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&st_pre.mot[MOTOR_5].corrected_steps, 0 }, - { "_fe","_fe5",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.following_error[MOTOR_5], 0 }, + { "_ts","_ts5",_f0, 2, tx_print_flt, get_flt, set_nul, &mr->target_steps[MOTOR_5], 0 }, + { "_ps","_ps5",_f0, 2, tx_print_flt, get_flt, set_nul, &mr->position_steps[MOTOR_5], 0 }, + { "_cs","_cs5",_f0, 2, tx_print_flt, get_flt, set_nul, &mr->commanded_steps[MOTOR_5], 0 }, + { "_es","_es5",_f0, 2, tx_print_flt, get_flt, set_nul, &mr->encoder_steps[MOTOR_5], 0 }, + { "_xs","_xs6",_f0, 2, tx_print_flt, get_flt, set_nul, &st_pre.mot[MOTOR_5].corrected_steps, 0 }, + { "_fe","_fe5",_f0, 2, tx_print_flt, get_flt, set_nul, &mr->following_error[MOTOR_5], 0 }, #endif #if (MOTORS >= 6) - { "_ts","_ts6",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.target_steps[MOTOR_6], 0 }, - { "_ps","_ps6",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.position_steps[MOTOR_6], 0 }, - { "_cs","_cs6",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.commanded_steps[MOTOR_6], 0 }, - { "_es","_es6",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.encoder_steps[MOTOR_6], 0 }, - { "_xs","_xs5",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&st_pre.mot[MOTOR_6].corrected_steps, 0 }, - { "_fe","_fe6",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.following_error[MOTOR_6], 0 }, + { "_ts","_ts6",_f0, 2, tx_print_flt, get_flt, set_nul, &mr->target_steps[MOTOR_6], 0 }, + { "_ps","_ps6",_f0, 2, tx_print_flt, get_flt, set_nul, &mr->position_steps[MOTOR_6], 0 }, + { "_cs","_cs6",_f0, 2, tx_print_flt, get_flt, set_nul, &mr->commanded_steps[MOTOR_6], 0 }, + { "_es","_es6",_f0, 2, tx_print_flt, get_flt, set_nul, &mr->encoder_steps[MOTOR_6], 0 }, + { "_xs","_xs5",_f0, 2, tx_print_flt, get_flt, set_nul, &st_pre.mot[MOTOR_6].corrected_steps, 0 }, + { "_fe","_fe6",_f0, 2, tx_print_flt, get_flt, set_nul, &mr->following_error[MOTOR_6], 0 }, #endif + #endif // __DIAGNOSTIC_PARAMETERS // Persistence for status report - must be in sequence // *** Count must agree with NV_STATUS_REPORT_LEN in report.h *** - { "","se00",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[0],0 }, - { "","se01",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[1],0 }, - { "","se02",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[2],0 }, - { "","se03",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[3],0 }, - { "","se04",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[4],0 }, - { "","se05",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[5],0 }, - { "","se06",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[6],0 }, - { "","se07",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[7],0 }, - { "","se08",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[8],0 }, - { "","se09",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[9],0 }, - { "","se10",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[10],0 }, - { "","se11",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[11],0 }, - { "","se12",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[12],0 }, - { "","se13",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[13],0 }, - { "","se14",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[14],0 }, - { "","se15",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[15],0 }, - { "","se16",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[16],0 }, - { "","se17",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[17],0 }, - { "","se18",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[18],0 }, - { "","se19",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[19],0 }, - { "","se20",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[20],0 }, - { "","se21",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[21],0 }, - { "","se22",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[22],0 }, - { "","se23",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[23],0 }, - { "","se24",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[24],0 }, - { "","se25",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[25],0 }, - { "","se26",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[26],0 }, - { "","se27",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[27],0 }, - { "","se28",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[28],0 }, - { "","se29",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[29],0 }, - { "","se30",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[30],0 }, - { "","se31",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[31],0 }, - { "","se32",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[32],0 }, - { "","se33",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[33],0 }, - { "","se34",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[34],0 }, - { "","se35",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[35],0 }, - { "","se36",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[36],0 }, - { "","se37",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[37],0 }, - { "","se38",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[38],0 }, - { "","se39",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[39],0 }, + { "","se00",_fp, 0, tx_print_nul, get_int32, set_int32, &sr.status_report_list[0],0 }, + { "","se01",_fp, 0, tx_print_nul, get_int32, set_int32, &sr.status_report_list[1],0 }, + { "","se02",_fp, 0, tx_print_nul, get_int32, set_int32, &sr.status_report_list[2],0 }, + { "","se03",_fp, 0, tx_print_nul, get_int32, set_int32, &sr.status_report_list[3],0 }, + { "","se04",_fp, 0, tx_print_nul, get_int32, set_int32, &sr.status_report_list[4],0 }, + { "","se05",_fp, 0, tx_print_nul, get_int32, set_int32, &sr.status_report_list[5],0 }, + { "","se06",_fp, 0, tx_print_nul, get_int32, set_int32, &sr.status_report_list[6],0 }, + { "","se07",_fp, 0, tx_print_nul, get_int32, set_int32, &sr.status_report_list[7],0 }, + { "","se08",_fp, 0, tx_print_nul, get_int32, set_int32, &sr.status_report_list[8],0 }, + { "","se09",_fp, 0, tx_print_nul, get_int32, set_int32, &sr.status_report_list[9],0 }, + { "","se10",_fp, 0, tx_print_nul, get_int32, set_int32, &sr.status_report_list[10],0 }, + { "","se11",_fp, 0, tx_print_nul, get_int32, set_int32, &sr.status_report_list[11],0 }, + { "","se12",_fp, 0, tx_print_nul, get_int32, set_int32, &sr.status_report_list[12],0 }, + { "","se13",_fp, 0, tx_print_nul, get_int32, set_int32, &sr.status_report_list[13],0 }, + { "","se14",_fp, 0, tx_print_nul, get_int32, set_int32, &sr.status_report_list[14],0 }, + { "","se15",_fp, 0, tx_print_nul, get_int32, set_int32, &sr.status_report_list[15],0 }, + { "","se16",_fp, 0, tx_print_nul, get_int32, set_int32, &sr.status_report_list[16],0 }, + { "","se17",_fp, 0, tx_print_nul, get_int32, set_int32, &sr.status_report_list[17],0 }, + { "","se18",_fp, 0, tx_print_nul, get_int32, set_int32, &sr.status_report_list[18],0 }, + { "","se19",_fp, 0, tx_print_nul, get_int32, set_int32, &sr.status_report_list[19],0 }, + { "","se20",_fp, 0, tx_print_nul, get_int32, set_int32, &sr.status_report_list[20],0 }, + { "","se21",_fp, 0, tx_print_nul, get_int32, set_int32, &sr.status_report_list[21],0 }, + { "","se22",_fp, 0, tx_print_nul, get_int32, set_int32, &sr.status_report_list[22],0 }, + { "","se23",_fp, 0, tx_print_nul, get_int32, set_int32, &sr.status_report_list[23],0 }, + { "","se24",_fp, 0, tx_print_nul, get_int32, set_int32, &sr.status_report_list[24],0 }, + { "","se25",_fp, 0, tx_print_nul, get_int32, set_int32, &sr.status_report_list[25],0 }, + { "","se26",_fp, 0, tx_print_nul, get_int32, set_int32, &sr.status_report_list[26],0 }, + { "","se27",_fp, 0, tx_print_nul, get_int32, set_int32, &sr.status_report_list[27],0 }, + { "","se28",_fp, 0, tx_print_nul, get_int32, set_int32, &sr.status_report_list[28],0 }, + { "","se29",_fp, 0, tx_print_nul, get_int32, set_int32, &sr.status_report_list[29],0 }, + { "","se30",_fp, 0, tx_print_nul, get_int32, set_int32, &sr.status_report_list[30],0 }, + { "","se31",_fp, 0, tx_print_nul, get_int32, set_int32, &sr.status_report_list[31],0 }, + { "","se32",_fp, 0, tx_print_nul, get_int32, set_int32, &sr.status_report_list[32],0 }, + { "","se33",_fp, 0, tx_print_nul, get_int32, set_int32, &sr.status_report_list[33],0 }, + { "","se34",_fp, 0, tx_print_nul, get_int32, set_int32, &sr.status_report_list[34],0 }, + { "","se35",_fp, 0, tx_print_nul, get_int32, set_int32, &sr.status_report_list[35],0 }, + { "","se36",_fp, 0, tx_print_nul, get_int32, set_int32, &sr.status_report_list[36],0 }, + { "","se37",_fp, 0, tx_print_nul, get_int32, set_int32, &sr.status_report_list[37],0 }, + { "","se38",_fp, 0, tx_print_nul, get_int32, set_int32, &sr.status_report_list[38],0 }, + { "","se39",_fp, 0, tx_print_nul, get_int32, set_int32, &sr.status_report_list[39],0 }, // Count is 40, since se00 counts as one. + // Group lookups - must follow the single-valued entries for proper sub-string matching // *** Must agree with NV_COUNT_GROUPS below *** - // *** START COUNTING FROM HERE *** - // *** Do not count: - // - Optional motors (5 and 6) - // - Optional USER_DATA - // - Optional DIAGNOSTIC_PARAMETERS - // - Uber groups (count these separately) + // *** If you adjust the number of entries in a group you must also adjust the count for that group *** + // *** COUNT STARTS FROM HERE *** - { "","sys",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // system group - { "","p1", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // PWM 1 group - // 2 - { "","1", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // motor groups - { "","2", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, - { "","3", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, - { "","4", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, +#define FIXED_GROUPS 4 + { "","sys",_f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, // system group + { "","p1", _f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, // PWM 1 group + { "","sp", _f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, // Spindle group + { "","co", _f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, // Coolant group + +#define AXIS_GROUPS AXES + { "","x", _f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, // axis groups + { "","y", _f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, + { "","z", _f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, + { "","u", _f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, + { "","v", _f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, + { "","w", _f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, + { "","a", _f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, + { "","b", _f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, + { "","c", _f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, + +#define MOTOR_GROUPS MOTORS + { "","1", _f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, // motor groups +#if (MOTORS >= 2) + { "","2", _f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, +#endif +#if (MOTORS >= 3) + { "","3", _f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, +#endif +#if (MOTORS >= 4) + { "","4", _f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, +#endif #if (MOTORS >= 5) - { "","5", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, + { "","5", _f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, #endif #if (MOTORS >= 6) - { "","6", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, + { "","6", _f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, #endif - // +4 = 6 - { "","x", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // axis groups - { "","y", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, - { "","z", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, - { "","a", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, - { "","b", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, - { "","c", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, - // +6 = 12 - { "","in", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // input state - { "","di1", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // input configs - { "","di2", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, - { "","di3", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, - { "","di4", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, - { "","di5", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, - { "","di6", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, - { "","di7", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, - { "","di8", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, - { "","di9", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, - // +10 = 22 - { "","out", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // output state - { "","do1", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // output configs - { "","do2", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, - { "","do3", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, - { "","do4", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, - { "","do5", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, - { "","do6", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, - { "","do7", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, - { "","do8", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, - { "","do9", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, - { "","do10", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, - { "","do11", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, - { "","do12", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, - { "","do13", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, - // +14 = 36 - { "","ai1", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // analog input configs - { "","ai2", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, - { "","ai3", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, - { "","ai4", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, - { "","ain1", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // analog input configs - { "","ain2", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, - { "","ain3", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, - { "","ain4", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, - // +8 = 44 - { "","g54",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // coord offset groups - { "","g55",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, - { "","g56",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, - { "","g57",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, - { "","g58",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, - { "","g59",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, - { "","g92",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // origin offsets - { "","g28",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // g28 home position - { "","g30",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // g30 home position - // +9 = 53 - { "","tof",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // tool offsets - { "","tt1",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // tt offsets - { "","tt2",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // tt offsets - { "","tt3",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // tt offsets - { "","tt4",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // tt offsets - { "","tt5",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // tt offsets - { "","tt6",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // tt offsets - { "","tt7",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // tt offsets - { "","tt8",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // tt offsets - { "","tt9",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // tt offsets - { "","tt10",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // tt offsets - { "","tt11",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // tt offsets - { "","tt12",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // tt offsets - { "","tt13",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // tt offsets - { "","tt14",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // tt offsets - { "","tt15",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // tt offsets - { "","tt16",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // tt offsets - // +17 = 70 - { "","mpo",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // machine position group - { "","pos",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // work position group - { "","ofs",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // work offset group - { "","hom",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // axis homing state group - { "","prb",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // probing state group - { "","pwr",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // motor power enagled group - { "","jog",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // axis jogging state group - { "","jid",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // job ID group - // +8 = 78 - { "","he1", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // heater 1 group - { "","he2", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // heater 2 group - { "","he3", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // heater 3 group - { "","pid1",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // PID 1 group - { "","pid2",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // PID 2 group - { "","pid3",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // PID 3 group - // +6 = 84 + +#define DIGITAL_IN_GROUPS 10 + { "","in", _f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, // input state + { "","di1", _f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, // input configs + { "","di2", _f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, + { "","di3", _f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, + { "","di4", _f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, + { "","di5", _f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, + { "","di6", _f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, + { "","di7", _f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, + { "","di8", _f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, + { "","di9", _f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, + +#define DIGITAL_OUT_GROUPS 14 + { "","out", _f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, // output state + { "","do1", _f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, // output configs + { "","do2", _f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, + { "","do3", _f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, + { "","do4", _f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, + { "","do5", _f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, + { "","do6", _f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, + { "","do7", _f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, + { "","do8", _f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, + { "","do9", _f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, + { "","do10", _f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, + { "","do11", _f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, + { "","do12", _f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, + { "","do13", _f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, + +#define ANALOG_IN_GROUPS 12 + { "","ai1", _f0, 0, tx_print_nul, get_grp, set_grp, nullptr,0 }, // analog input configs + { "","ai2", _f0, 0, tx_print_nul, get_grp, set_grp,nullptr,0 }, + { "","ai3", _f0, 0, tx_print_nul, get_grp, set_grp,nullptr,0 }, + { "","ai4", _f0, 0, tx_print_nul, get_grp, set_grp,nullptr,0 }, + { "","ain1", _f0, 0, tx_print_nul, get_grp, set_grp,nullptr,0 }, // analog input configs + { "","ain2", _f0, 0, tx_print_nul, get_grp, set_grp,nullptr,0 }, + { "","ain3", _f0, 0, tx_print_nul, get_grp, set_grp,nullptr,0 }, + { "","ain4", _f0, 0, tx_print_nul, get_grp, set_grp,nullptr,0 }, + { "","ain5", _f0, 0, tx_print_nul, get_grp, set_grp,nullptr,0 }, // analog input configs + { "","ain6", _f0, 0, tx_print_nul, get_grp, set_grp,nullptr,0 }, + { "","ain7", _f0, 0, tx_print_nul, get_grp, set_grp,nullptr,0 }, + { "","ain8", _f0, 0, tx_print_nul, get_grp, set_grp,nullptr,0 }, + +#define COORDINATE_OFFSET_GROUPS 9 + { "","g54",_f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, // coord offset groups + { "","g55",_f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, + { "","g56",_f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, + { "","g57",_f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, + { "","g58",_f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, + { "","g59",_f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, + { "","g92",_f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, // origin offsets + { "","g28",_f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, // g28 home position + { "","g30",_f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, // g30 home position + +#define TOOL_OFFSET_GROUPS 33 + { "","tof",_f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, // current tool offsets + { "","tt1",_f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, // tt offsets + { "","tt2",_f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, // tt offsets + { "","tt3",_f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, // tt offsets + { "","tt4",_f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, // tt offsets + { "","tt5",_f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, // tt offsets + { "","tt6",_f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, // tt offsets + { "","tt7",_f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, // tt offsets + { "","tt8",_f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, // tt offsets + { "","tt9",_f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, // tt offsets + { "","tt10",_f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, // tt offsets + { "","tt11",_f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, // tt offsets + { "","tt12",_f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, // tt offsets + { "","tt13",_f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, // tt offsets + { "","tt14",_f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, // tt offsets + { "","tt15",_f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, // tt offsets + { "","tt16",_f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, // tt offsets + { "","tt17",_f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, // tt offsets + { "","tt18",_f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, // tt offsets + { "","tt19",_f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, // tt offsets + { "","tt20",_f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, // tt offsets + { "","tt21",_f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, // tt offsets + { "","tt22",_f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, // tt offsets + { "","tt23",_f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, // tt offsets + { "","tt24",_f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, // tt offsets + { "","tt25",_f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, // tt offsets + { "","tt26",_f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, // tt offsets + { "","tt27",_f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, // tt offsets + { "","tt28",_f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, // tt offsets + { "","tt29",_f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, // tt offsets + { "","tt30",_f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, // tt offsets + { "","tt31",_f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, // tt offsets + { "","tt32",_f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, // tt offsets + +#define MACHINE_STATE_GROUPS 8 + { "","mpo",_f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, // machine position group + { "","pos",_f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, // work position group + { "","ofs",_f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, // work offset group + { "","hom",_f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, // axis homing state group + { "","prb",_f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, // probing state group + { "","pwr",_f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, // motor power enagled group + { "","jog",_f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, // axis jogging state group + { "","jid",_f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, // job ID group + +#define TEMPERATURE_GROUPS 6 + { "","he1", _f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, // heater 1 group + { "","he2", _f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, // heater 2 group + { "","he3", _f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, // heater 3 group + { "","pid1",_f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, // PID 1 group + { "","pid2",_f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, // PID 2 group + { "","pid3",_f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, // PID 3 group #ifdef __USER_DATA - { "","uda", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // user data group - { "","udb", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // user data group - { "","udc", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // user data group - { "","udd", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // user data group -#endif -#ifdef __DIAGNOSTIC_PARAMETERS - { "","_te",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // target axis endpoint group - { "","_tr",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // target axis runtime group - { "","_ts",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // target motor steps group - { "","_ps",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // position motor steps group - { "","_cs",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // commanded motor steps group - { "","_es",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // encoder steps group - { "","_xs",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // correction steps group - { "","_fe",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // following error group +#define USER_DATA_GROUPS 4 + { "","uda", _f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, // user data group + { "","udb", _f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, // user data group + { "","udc", _f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, // user data group + { "","udd", _f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, // user data group #endif +#ifdef __DIAGNOSTIC_PARAMETERS +#define DIAGNOSTIC_GROUPS 8 + { "","_te",_f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, // target axis endpoint group + { "","_tr",_f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, // target axis runtime group + { "","_ts",_f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, // target motor steps group + { "","_ps",_f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, // position motor steps group + { "","_cs",_f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, // commanded motor steps group + { "","_es",_f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, // encoder steps group + { "","_xs",_f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, // correction steps group + { "","_fe",_f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, // following error group +#endif + +#define NV_COUNT_UBER_GROUPS 6 // Uber-group (groups of groups, for text-mode displays only) // *** Must agree with NV_COUNT_UBER_GROUPS below **** - { "", "m", _f0, 0, tx_print_nul, _do_motors, set_nul,(float *)&cs.null,0 }, - { "", "q", _f0, 0, tx_print_nul, _do_axes, set_nul,(float *)&cs.null,0 }, - { "", "o", _f0, 0, tx_print_nul, _do_offsets,set_nul,(float *)&cs.null,0 }, - { "", "di", _f0, 0, tx_print_nul,_do_inputs, set_nul,(float *)&cs.null,0 }, - { "", "do", _f0, 0, tx_print_nul,_do_outputs,set_nul,(float *)&cs.null,0 }, - { "", "$", _f0, 0, tx_print_nul, _do_all, set_nul,(float *)&cs.null,0 } + { "", "m", _f0, 0, tx_print_nul, _do_motors, set_nul, nullptr, 0 }, + { "", "q", _f0, 0, tx_print_nul, _do_axes, set_nul, nullptr, 0 }, + { "", "o", _f0, 0, tx_print_nul, _do_offsets,set_nul, nullptr, 0 }, + { "", "di", _f0, 0, tx_print_nul,_do_inputs, set_nul, nullptr, 0 }, + { "", "do", _f0, 0, tx_print_nul,_do_outputs,set_nul, nullptr, 0 }, + { "", "$", _f0, 0, tx_print_nul, _do_all, set_nul, nullptr, 0 } }; /***** Make sure these defines line up with any changes in the above table *****/ -#define NV_COUNT_UBER_GROUPS 6 // count of uber-groups, above -#define FIXED_GROUPS 92 // count of fixed groups, excluding optional groups - -#if (MOTORS >= 5) -#define MOTOR_GROUP_5 1 -#else -#define MOTOR_GROUP_5 0 -#endif - -#if (MOTORS >= 6) -#define MOTOR_GROUP_6 1 -#else -#define MOTOR_GROUP_6 0 -#endif - -#ifdef __USER_DATA -#define USER_DATA_GROUPS 4 // count of user data groups only -#else -#define USER_DATA_GROUPS 0 -#endif - -#ifdef __DIAGNOSTIC_PARAMETERS -#define DIAGNOSTIC_GROUPS 8 // count of diagnostic groups only -#else -#define DIAGNOSTIC_GROUPS 0 -#endif - -#define TEMPERATURE_GROUPS 6 -#define NV_COUNT_GROUPS (FIXED_GROUPS + MOTOR_GROUP_5 + MOTOR_GROUP_6 + USER_DATA_GROUPS + DIAGNOSTIC_GROUPS + TEMPERATURE_GROUPS) +#define NV_COUNT_GROUPS (FIXED_GROUPS \ + + AXIS_GROUPS \ + + MOTOR_GROUPS \ + + DIGITAL_IN_GROUPS \ + + DIGITAL_OUT_GROUPS \ + + ANALOG_IN_GROUPS \ + + COORDINATE_OFFSET_GROUPS \ + + TOOL_OFFSET_GROUPS \ + + MACHINE_STATE_GROUPS \ + + TEMPERATURE_GROUPS \ + + USER_DATA_GROUPS \ + + DIAGNOSTIC_GROUPS) /* */ #define NV_INDEX_MAX (sizeof(cfgArray) / sizeof(cfgItem_t)) @@ -1526,79 +1748,41 @@ bool nv_index_is_group(index_t index) { return (((index >= NV_INDEX_START_GROUPS bool nv_index_lt_groups(index_t index) { return ((index <= NV_INDEX_START_GROUPS) ? true : false);} /***** APPLICATION SPECIFIC CONFIGS AND EXTENSIONS TO GENERIC FUNCTIONS *****/ - /* - * set_flu() - set floating point number with G20/G21 units conversion - * set_flup() - set positive floating point number with G20/G21 units conversion - * set_fltp() - set positive floating point number with no units conversion + * convert_incoming_float() - pre-process an incoming floating point number for canonical units + * convert_outgoing_float() - pre-process an outgoing floating point number for units display * - * The number 'setted' will have been delivered in external units (inches or mm). - * It is written to the target memory location in internal canonical units (mm). - * The original nv->value is also changed so persistence works correctly. - * Displays should convert back from internal canonical form to external form. + * Incoming floats are destined for SET operations. + * Outgoing floats are the raw values from GET operations, destined for text or JSON display. * - * !!!! WARNING !!!! set_flu() doesn't care about axes, so make sure you aren't passing it ABC axes + * Apologies in advance for these twisty little functions. These functions are used to + * convert incoming floats into the native, canonical form of a parameter (mm, or whatever) + * and outgoing floats into a display format appropriate to the units mode in effect. + * They use the flags in the config table and other cues to determine what type of conversion + * to perform. + * + * The conversions are complicated by the fact that only linear axes actually convert - + * rotaries do not - unless they are in radius mode. Plus, determining the axis for a motor + * requires unraveling the motor mapping (handled in cm_get_axis_type()). Also, there are + * global SYS group values that are not associated with any axis. Lastly, the + * steps-per-unit value (1su) is actually kept in inverse conversion form, as its native + * form would be units-per-step. */ -stat_t set_flu(nvObj_t *nv) -{ - if (cm_get_units_mode(MODEL) == INCHES) { // if in inches... - nv->value *= MM_PER_INCH; // convert to canonical millimeter units - } - *((float *)GET_TABLE_WORD(target)) = nv->value; // write value as millimeters or degrees - nv->precision = GET_TABLE_WORD(precision); - nv->valuetype = TYPE_FLOAT; - return(STAT_OK); -} - -stat_t set_flup(nvObj_t *nv) -{ - if (nv->value < 0) { - nv->valuetype = TYPE_NULL; - return (STAT_INPUT_LESS_THAN_MIN_VALUE); - } - return (set_flu(nv)); -} - -stat_t set_fltp(nvObj_t *nv) -{ - if (nv->value < 0) { - nv->valuetype = TYPE_NULL; - return (STAT_INPUT_LESS_THAN_MIN_VALUE); - } - return (set_flt(nv)); -} - -/* - * preprocess_float() - pre-process floating point number for units display - * - * Apologies in advance for this twisty little function. This function is used to - * convert the native, canonical form of a parameter (mm, or whatever), into a display - * format appropriate to the units mode in effect. It uses the flags in the config table - * to determine what type of conversion to perform. It's complicated by the fact that - * only linear axes actually convert - rotaries do not. Plus, determining the axis for - * a motor requires unraveling the motor mapping (handled in cm_get_axis_type()). - * Also, there are global SYS group values that are not associated with any axis. - * Lastly, the steps-per-unit value (1su) is actually kept in inverse conversion form, - * as its native form would be units-per-step. - */ - -void preprocess_float(nvObj_t *nv) +static void _convert(nvObj_t *nv, float conversion_factor) { if (nv->valuetype != TYPE_FLOAT) { return; } // can be called non-destructively for any value type - if (isnan((double)nv->value) || isinf((double)nv->value)) { return; } // trap illegal float values + if (isnan((double)nv->value_flt) || isinf((double)nv->value_flt)) { return; } // trap illegal float values ///+++ transform these checks into NaN or INF strings with an error return? - // We may need one of two types of units conversion, but only if in inches mode if (cm_get_units_mode(MODEL) == INCHES) { - cmAxisType type = cm_get_axis_type(nv->index); // linear, rotary or global - if (cfgArray[nv->index].flags & F_CONVERT) { // standard units conversion - if ((type == AXIS_TYPE_LINEAR) || (type == AXIS_TYPE_SYSTEM)) { - nv->value *= INCHES_PER_MM; - } - } else if (cfgArray[nv->index].flags & F_ICONVERT) {// inverse units conversion - if ((type == AXIS_TYPE_LINEAR) || (type == AXIS_TYPE_SYSTEM)) { - nv->value *= MM_PER_INCH; + cmAxisType axis_type = cm_get_axis_type(nv); // linear, rotary, global or error + if ((axis_type == AXIS_TYPE_LINEAR) || (axis_type == AXIS_TYPE_SYSTEM)) { + if (cfgArray[nv->index].flags & F_CONVERT) { // standard units conversion + nv->value_flt *= conversion_factor; + } else + if (cfgArray[nv->index].flags & F_ICONVERT) { // inverse units conversion + nv->value_flt /= conversion_factor; } } } @@ -1606,6 +1790,123 @@ void preprocess_float(nvObj_t *nv) nv->valuetype = TYPE_FLOAT; } +void convert_incoming_float(nvObj_t *nv) { return(_convert (nv, MM_PER_INCH)); } +void convert_outgoing_float(nvObj_t *nv) { return(_convert (nv, INCHES_PER_MM)); } + +/* + * get_float() - boilerplate for retrieving raw floating point value + * set_float() - boilerplate for setting a floating point value with unit conversion + * set_float_range() - set a floating point value with inclusive range check + * + * get_float() loads nv->value with 'value' in internal canonical units (e.g. mm, degrees) + * without units conversion. If conversion is required call convert_outgoing_float() + * afterwards. The text mode and JSON display routines do this, so you generally don't + * have to worry about this. + * + * set_float() is designed to capture incoming float values, so it performs unit conversion. + * set_float_range() perfoems an inclusive range test on the CONVERTED value + */ + +stat_t get_float(nvObj_t *nv, const float value) { + nv->value_flt = value; + nv->valuetype = TYPE_FLOAT; + nv->precision = GET_TABLE_WORD(precision); + return STAT_OK; +} + +stat_t set_float(nvObj_t *nv, float &value) { + convert_incoming_float(nv); + value = nv->value_flt; + return (STAT_OK); +} + +stat_t set_float_range(nvObj_t *nv, float &value, float low, float high) { + + char msg[64]; + + convert_incoming_float(nv); // conditional unit conversion + if (nv->value_flt < low) { + sprintf(msg, "Input is less than minimum value %0.4f", low); + nv_add_conditional_message(msg); + nv->valuetype = TYPE_NULL; + return (STAT_INPUT_LESS_THAN_MIN_VALUE); + } + if (nv->value_flt > high) { + sprintf(msg, "Input is more than maximum value %0.4f", high); + nv_add_conditional_message(msg); + nv->valuetype = TYPE_NULL; + return (STAT_INPUT_EXCEEDS_MAX_VALUE); + } + value = nv->value_flt; + return (STAT_OK); +} + +/* + * get_integer() - boilerplate for retrieving 8 and 32 bit integer values + * set_integer() - boilerplate for setting 8 bit integer value with range checking + * set_int32() - boilerplate for setting 32 bit integer value with range checking + */ + +static stat_t _set_int_tests(nvObj_t *nv, int32_t low, int32_t high) +{ + char msg[64]; + + if (nv->value_int < low) { + sprintf(msg, "Input less than minimum value %d", (int)low); + nv_add_conditional_message(msg); + nv->valuetype = TYPE_NULL; + return (STAT_INPUT_LESS_THAN_MIN_VALUE); + } + if (nv->value_int > high) { + sprintf(msg, "Input more than maximum value %d", (int)high); + nv_add_conditional_message(msg); + nv->valuetype = TYPE_NULL; + return (STAT_INPUT_EXCEEDS_MAX_VALUE); + } + return (STAT_OK); +} + +stat_t get_integer(nvObj_t *nv, const int32_t value) +{ + nv->value_int = value; + nv->valuetype = TYPE_INTEGER; + return STAT_OK; +} + +stat_t set_integer(nvObj_t *nv, uint8_t &value, uint8_t low, uint8_t high) +{ + ritorno(_set_int_tests(nv, low, high)) + value = nv->value_int; + nv->valuetype = TYPE_INTEGER; + return (STAT_OK); +} + +stat_t set_int32(nvObj_t *nv, int32_t &value, int32_t low, int32_t high) +{ + ritorno(_set_int_tests(nv, low, high)) + value = nv->value_int; // note: valuetype = TYPE_INT already set + nv->valuetype = TYPE_INTEGER; + return (STAT_OK); +} + +stat_t set_uint32(nvObj_t *nv, uint32_t &value, int32_t low, int32_t high) +{ + ritorno(_set_int_tests(nv, low, high)) + value = nv->value_int; // note: valuetype = TYPE_INT already set + nv->valuetype = TYPE_INTEGER; + return (STAT_OK); +} + +/* + * get_string() - boilerplate for retrieving a string value + */ + +stat_t get_string(nvObj_t *nv, const char *str) +{ + nv->valuetype = TYPE_STRING; + return (nv_copy_string(nv, str)); +} + /* * nv_group_is_prefixed() - hack * @@ -1672,7 +1973,7 @@ static stat_t _do_motors(nvObj_t *nv) // print parameters for all motor groups static stat_t _do_axes(nvObj_t *nv) // print parameters for all axis groups { - char list[][TOKEN_LEN+1] = {"x","y","z","a","b","c",""}; // must have a terminating element + char list[][TOKEN_LEN+1] = {"x","y","z","u","v","w","a","b","c",""}; // must have a terminating element return (_do_group_list(nv, list)); } @@ -1742,15 +2043,15 @@ static stat_t _do_all(nvObj_t *nv) // print all parameters static stat_t get_rx(nvObj_t *nv) { - nv->value = (float)254; // ARM always says the serial buffer is available (max) - nv->valuetype = TYPE_INT; + nv->value_int = (float)254; // ARM always says the serial buffer is available (max) + nv->valuetype = TYPE_INTEGER; return (STAT_OK); } static stat_t get_tick(nvObj_t *nv) { - nv->value = (float)SysTickTimer.getValue(); - nv->valuetype = TYPE_INT; + nv->value_int = (float)SysTickTimer.getValue(); + nv->valuetype = TYPE_INTEGER; return (STAT_OK); } @@ -1763,14 +2064,8 @@ static stat_t get_tick(nvObj_t *nv) static const char fmt_rx[] = "rx:%d\n"; static const char fmt_ex[] = "[ex] enable flow control%10d [0=off,1=XON/XOFF, 2=RTS/CTS]\n"; -//static const char fmt_ec[] = "[ec] expand LF to CRLF on TX%6d [0=off,1=on]\n"; -//static const char fmt_ee[] = "[ee] enable echo%18d [0=off,1=on]\n"; -//static const char fmt_baud[] = "[baud] USB baud rate%15d [1=9600,2=19200,3=38400,4=57600,5=115200,6=230400]\n"; void cfg_print_rx(nvObj_t *nv) { text_print(nv, fmt_rx);} // TYPE_INT void cfg_print_ex(nvObj_t *nv) { text_print(nv, fmt_ex);} // TYPE_INT -//void cfg_print_ec(nvObj_t *nv) { text_print(nv, fmt_ec);} // TYPE_INT -//void cfg_print_ee(nvObj_t *nv) { text_print(nv, fmt_ee);} // TYPE_INT -//void cfg_print_baud(nvObj_t *nv) { text_print(nv, fmt_baud);} // TYPE_INT #endif // __TEXT_MODE diff --git a/g2core/config_app.h b/g2core/config_app.h old mode 100755 new mode 100644 index 190b339f..bfd3931c --- a/g2core/config_app.h +++ b/g2core/config_app.h @@ -2,8 +2,8 @@ * config_app.h - application-specific part of configuration sub-system * This file is part of the g2core project * - * Copyright (c) 2010 - 2016 Alden S. Hart, Jr. - * Copyright (c) 2013 - 2016 Robert Giseburt + * Copyright (c) 2010 - 2018 Alden S. Hart, Jr. + * Copyright (c) 2013 - 2018 Robert Giseburt * * This file ("the software") is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License, version 2 as published by the diff --git a/g2core/controller.cpp b/g2core/controller.cpp index 14ab7149..714f7292 100644 --- a/g2core/controller.cpp +++ b/g2core/controller.cpp @@ -2,8 +2,8 @@ * controller.cpp - g2core controller and top level parser * This file is part of the g2core project * - * Copyright (c) 2010 - 2016 Alden S. Hart, Jr. - * Copyright (c) 2013 - 2016 Robert Giseburt + * Copyright (c) 2010 - 2019 Alden S. Hart, Jr. + * Copyright (c) 2013 - 2019 Robert Giseburt * * This file ("the software") is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License, version 2 as published by the @@ -31,7 +31,7 @@ #include "controller.h" #include "json_parser.h" #include "text_parser.h" -#include "gcode_parser.h" +#include "gcode.h" #include "canonical_machine.h" #include "plan_arc.h" #include "planner.h" @@ -52,15 +52,15 @@ #include "marlin_compatibility.h" #endif -/*********************************************************************************** - **** STRUCTURE ALLOCATIONS ********************************************************* - ***********************************************************************************/ +/**************************************************************************************** + **** STRUCTURE ALLOCATIONS ************************************************************* + ****************************************************************************************/ controller_t cs; // controller state structure -/*********************************************************************************** - **** STATICS AND LOCALS *********************************************************** - ***********************************************************************************/ +/**************************************************************************************** + **** STATICS AND LOCALS **************************************************************** + ****************************************************************************************/ static void _controller_HSM(void); static stat_t _led_indicator(void); // twiddle the LED indicator @@ -85,7 +85,7 @@ gpioDigitalInputHandler _shutdown_input_handler { [&](const bool state, const inputEdgeFlag edge, const uint8_t triggering_pin_number) { if (edge != INPUT_EDGE_LEADING) { return false; } - cm.shutdown_requested = triggering_pin_number; + cm->shutdown_requested = triggering_pin_number; return false; // allow others to see this notice }, @@ -97,7 +97,7 @@ gpioDigitalInputHandler _limit_input_handler { [&](const bool state, const inputEdgeFlag edge, const uint8_t triggering_pin_number) { if (edge != INPUT_EDGE_LEADING) { return false; } - cm.limit_requested = triggering_pin_number; + cm->limit_requested = triggering_pin_number; return false; // allow others to see this notice }, @@ -108,9 +108,9 @@ gpioDigitalInputHandler _limit_input_handler { gpioDigitalInputHandler _interlock_input_handler { [&](const bool state, const inputEdgeFlag edge, const uint8_t triggering_pin_number) { if (edge == INPUT_EDGE_LEADING) { - cm.safety_interlock_disengaged = triggering_pin_number; + cm->safety_interlock_disengaged = triggering_pin_number; } else { // edge == INPUT_EDGE_TRAILING - cm.safety_interlock_reengaged = triggering_pin_number; + cm->safety_interlock_reengaged = triggering_pin_number; } return false; // allow others to see this notice @@ -137,7 +137,7 @@ void controller_init() cs.comm_mode = comm_mode; // restore parameters cs.fw_build = G2CORE_FIRMWARE_BUILD; // set up identification cs.fw_version = G2CORE_FIRMWARE_VERSION; - cs.hw_platform = G2CORE_HARDWARE_PLATFORM; // NB: HW version is set from EEPROM + cs.controller_state = CONTROLLER_STARTUP; // ready to run startup lines if (xio_connected()) { cs.controller_state = CONTROLLER_CONNECTED; @@ -188,7 +188,9 @@ static void _controller_HSM() // See hardware.h for a list of ISRs and their priorities. // //----- kernel level ISR handlers ----(flags are set in ISRs)------------------------// - // Order is important: + + // Order is important, and line breaks indicate dependency groups + DISPATCH(hardware_periodic()); // give the hardware a chance to do stuff DISPATCH(_led_indicator()); // blink LEDs at the current rate DISPATCH(_shutdown_handler()); // invoke shutdown @@ -205,14 +207,17 @@ static void _controller_HSM() DISPATCH(sr_status_report_callback()); // conditionally send status report DISPATCH(qr_queue_report_callback()); // conditionally send queue report - DISPATCH(cm_feedhold_sequencing_callback());// feedhold state machine runner + // these 3 must be in this exact order: DISPATCH(mp_planner_callback()); // motion planner - DISPATCH(cm_arc_callback()); // arc generation runs as a cycle above lines + DISPATCH(cm_operation_runner_callback()); // operation action runner + DISPATCH(cm_arc_callback(cm)); // arc generation runs as a cycle above lines + DISPATCH(cm_homing_cycle_callback()); // homing cycle operation (G28.2) DISPATCH(cm_probing_cycle_callback()); // probing cycle operation (G38.2) DISPATCH(cm_jogging_cycle_callback()); // jog cycle operation DISPATCH(cm_deferred_write_callback()); // persist G10 changes when not in machining cycle + DISPATCH(cm_feedhold_command_blocker()); // blocks new Gcode from arriving while in feedhold #if MARLIN_COMPAT_ENABLED == true DISPATCH(marlin_callback()); // handle Marlin stuff - may return EAGAIN, must be after planner_callback! #endif @@ -224,7 +229,7 @@ static void _controller_HSM() DISPATCH(_dispatch_command()); // MUST BE LAST - read and execute next command } -/***************************************************************************** +/**************************************************************************************** * command dispatchers * _dispatch_control - entry point for control-only dispatches * _dispatch_command - entry point for control and data dispatches @@ -251,7 +256,7 @@ static stat_t _dispatch_command() { if (cs.controller_state != CONTROLLER_PAUSED) { devflags_t flags = DEV_IS_BOTH | DEV_IS_MUTED; // expressly state we'll handle muted devices - if ((!mp_planner_is_full()) && (cs.bufp = xio_readline(flags, cs.linelen)) != NULL) { + if ((!mp_planner_is_full(mp)) && (cs.bufp = xio_readline(flags, cs.linelen)) != NULL) { _dispatch_kernel(flags); } } @@ -295,10 +300,10 @@ static void _dispatch_kernel(const devflags_t flags) } // trap single character commands - if (*cs.bufp == '!') { cm_request_feedhold(); } + if (*cs.bufp == '!') { cm_request_feedhold(FEEDHOLD_TYPE_ACTIONS, FEEDHOLD_EXIT_CYCLE); } + else if (*cs.bufp == '~') { cm_request_cycle_start(); } else if (*cs.bufp == '%') { cm_request_queue_flush(); xio_flush_to_command(); } - else if (*cs.bufp == '~') { cm_request_end_hold(); } - else if (*cs.bufp == EOT) { cm_alarm(STAT_KILL_JOB, "EOT Received"); } + else if (*cs.bufp == EOT) { cm_request_job_kill(); xio_flush_to_command(); } else if (*cs.bufp == ENQ) { controller_request_enquiry(); } else if (*cs.bufp == CAN) { hw_hard_reset(); } // reset immediately @@ -325,8 +330,8 @@ static void _dispatch_kernel(const devflags_t flags) #endif #if MARLIN_COMPAT_ENABLED == true - else if (js.json_mode == MARLIN_COMM_MODE) { // handle marlin-specific protocol gcode - cs.comm_request_mode = MARLIN_COMM_MODE; // mode of this command + else if (js.json_mode == MARLIN_COMM_MODE) { // handle marlin-specific protocol gcode + cs.comm_request_mode = MARLIN_COMM_MODE; // mode of this command marlin_response(gcode_parser(cs.bufp), cs.saved_buf); } #endif @@ -356,8 +361,7 @@ static void _dispatch_kernel(const devflags_t flags) } } -/**** Local Functions ********************************************************/ - +/**** Local Functions ******************************************************************/ /* * _reset_comms_mode() - reset the communications mode (and other effected settings) after connection or disconnection @@ -495,20 +499,18 @@ static stat_t _sync_to_tx_buffer() static stat_t _sync_to_planner() { - if (mp_planner_is_full()) { // allow up to N planner buffers for this line + if (mp_planner_is_full(mp)) { // allow up to N planner buffers for this line return (STAT_EAGAIN); } return (STAT_OK); } -/* ALARM STATE HANDLERS +/**************************************************************************************** + * ALARM STATE HANDLERS * * _shutdown_handler() - put system into shutdown state - * _shutdown_input_handler - a gpioDigitalInputHandler to capture pin change events * _limit_switch_handler() - shut down system if limit switch fired - * _limit_input_handler - a gpioDigitalInputHandler to capture pin change events - * _interlock_input_handler() - feedhold and resume depending on edge - * _interlock_handler - a gpioDigitalInputHandler to capture pin change events + * _interlock_handler() - feedhold and resume depending on edge * * Some handlers return EAGAIN causing the control loop to never advance beyond that point. * @@ -517,13 +519,12 @@ static stat_t _sync_to_planner() * - safety_interlock_requested == INPUT_EDGE_LEADING is interlock onset * - safety_interlock_requested == INPUT_EDGE_TRAILING is interlock offset */ - static stat_t _shutdown_handler(void) { - if (cm.shutdown_requested != 0) { // request may contain the (non-zero) input number + if (cm->shutdown_requested != 0) { // request may contain the (non-zero) input number char msg[10]; - sprintf(msg, "input %d", (int)cm.shutdown_requested); - cm.shutdown_requested = false; // clear limit request used here ^ + sprintf(msg, "input %d", (int)cm->shutdown_requested); + cm->shutdown_requested = false; // clear limit request used here ^ cm_shutdown(STAT_SHUTDOWN, msg); } return(STAT_OK); @@ -532,16 +533,15 @@ static stat_t _shutdown_handler(void) static stat_t _limit_switch_handler(void) { auto machine_state = cm_get_machine_state(); - if ((machine_state != MACHINE_ALARM) && (machine_state != MACHINE_PANIC) && (machine_state != MACHINE_SHUTDOWN)) { + if ((machine_state != MACHINE_ALARM) && + (machine_state != MACHINE_PANIC) && + (machine_state != MACHINE_SHUTDOWN)) { safe_pin.toggle(); } - -// safe_pin = 1; - - if ((cm.limit_enable == true) && (cm.limit_requested != 0)) { + if ((cm->limit_enable == true) && (cm->limit_requested != 0)) { char msg[10]; - sprintf(msg, "input %d", (int)cm.limit_requested); - cm.limit_requested = false; // clear limit request used here ^ + sprintf(msg, "input %d", (int)cm->limit_requested); + cm->limit_requested = false; // clear limit request used here ^ cm_alarm(STAT_LIMIT_SWITCH_HIT, msg); } return (STAT_OK); @@ -549,30 +549,29 @@ static stat_t _limit_switch_handler(void) static stat_t _interlock_handler(void) { - if (cm.safety_interlock_enable) { + if (cm->safety_interlock_enable) { // interlock broken - if (cm.safety_interlock_disengaged != 0) { - cm.safety_interlock_disengaged = 0; - cm.safety_interlock_state = SAFETY_INTERLOCK_DISENGAGED; - cm_request_feedhold(); // may have already requested STOP as INPUT_ACTION + if (cm->safety_interlock_disengaged != 0) { + cm->safety_interlock_disengaged = 0; + cm->safety_interlock_state = SAFETY_INTERLOCK_DISENGAGED; + cm_request_feedhold(FEEDHOLD_TYPE_ACTIONS, FEEDHOLD_EXIT_INTERLOCK); // may have already requested STOP as INPUT_ACTION // feedhold was initiated by input action in gpio // pause spindle // pause coolant } // interlock restored - if ((cm.safety_interlock_reengaged != 0) && (mp_runtime_is_idle())) { - cm.safety_interlock_reengaged = 0; - cm.safety_interlock_state = SAFETY_INTERLOCK_ENGAGED; // interlock restored - // restart spindle with dwell - cm_request_end_hold(); // use cm_request_end_hold() instead of just ending - // restart coolant + if ((cm->safety_interlock_reengaged != 0) && (mp_runtime_is_idle())) { + cm->safety_interlock_reengaged = 0; + cm->safety_interlock_state = SAFETY_INTERLOCK_ENGAGED; // interlock restored +// cm_request_exit_hold(); // use cm_request_exit_hold() instead of just ending +++++ + cm_request_cycle_start(); // proper way to restart the cycle } } return(STAT_OK); } -/* +/**************************************************************************************** * _init_assertions() - initialize controller memory integrity assertions * _test_assertions() - check controller memory integrity assertions * _test_system_assertions() - check assertions for entire system @@ -595,10 +594,12 @@ static stat_t _test_assertions() stat_t _test_system_assertions() { // these functions will panic if an assertion fails - _test_assertions(); // controller assertions (local) + _test_assertions(); // controller assertions (local) config_test_assertions(); - canonical_machine_test_assertions(); - planner_test_assertions(); + canonical_machine_test_assertions(&cm1); + canonical_machine_test_assertions(&cm2); + planner_assert(&mp1); + planner_assert(&mp2); stepper_test_assertions(); encoder_test_assertions(); xio_test_assertions(); diff --git a/g2core/controller.h b/g2core/controller.h old mode 100755 new mode 100644 index 7c6e3078..69e35f36 --- a/g2core/controller.h +++ b/g2core/controller.h @@ -2,8 +2,8 @@ * controller.h - g2core controller and main dispatch loop * This file is part of the g2core project * - * Copyright (c) 2010 - 2016 Alden S. Hart, Jr. - * Copyright (c) 2013 - 2016 Robert Giseburt + * Copyright (c) 2010 - 2018 Alden S. Hart, Jr. + * Copyright (c) 2013 - 2018 Robert Giseburt * * This file ("the software") is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License, version 2 as published by the @@ -55,8 +55,6 @@ typedef struct controllerSingleton { // main TG controller struct // system identification values float fw_build; // firmware build number float fw_version; // firmware version number - float hw_platform; // hardware compatibility - platform type - float hw_version; // hardware compatibility - platform revision // system state variables csControllerState controller_state; @@ -64,11 +62,11 @@ typedef struct controllerSingleton { // main TG controller struct uint32_t led_blink_rate; // used to flash indicator LED // communications state variables - // cs.comm_mode is the setting for the communications mode - // js.json_mode is the actual current mode (see also js.json_now) + // useful to know: + // cs.comm_mode is the setting for the communications mode + // js.json_mode is the actual current mode (see also js.json_now) commMode comm_mode; // ej: 0=text mode sticky, 1=JSON mode sticky, 2=auto mode - commMode comm_request_mode; // mode of request (may be different thatn the setting) - + commMode comm_request_mode; // mode of request (may be different than the setting) bool responses_suppressed; // if true, responses are to be suppressed (for internal-file delivery) // controller serial buffers @@ -77,6 +75,9 @@ typedef struct controllerSingleton { // main TG controller struct char out_buf[OUTPUT_BUFFER_LEN]; // output buffer char saved_buf[SAVED_BUFFER_LEN]; // save the input buffer + // Exceptions - some exceptions cannot be notified by an ER because they are in interrupts + bool exec_aline_assertion_failure; // record an exception deep inside mp_exec_aline() + magic_t magic_end; } controller_t; diff --git a/g2core/coolant.cpp b/g2core/coolant.cpp old mode 100755 new mode 100644 index c553c6a6..77fe4e5e --- a/g2core/coolant.cpp +++ b/g2core/coolant.cpp @@ -2,7 +2,7 @@ * coolant.cpp - canonical machine coolant driver * This file is part of the g2core project * - * Copyright (c) 2015 - 2016 Alden S. Hart, Jr. + * Copyright (c) 2015 - 2019 Alden S. Hart, Jr. * * This file ("the software") is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License, version 2 as published by the @@ -38,10 +38,10 @@ /**** Allocate structures ****/ -cmCoolantSingleton_t coolant; +coCoolant_t coolant; -gpioDigitalOutput *mist_enable_output = nullptr; -gpioDigitalOutput *flood_enable_output = nullptr; +// gpioDigitalOutput *mist_enable_output = nullptr; +// gpioDigitalOutput *flood_enable_output = nullptr; #ifndef MIST_ENABLE_OUTPUT_NUMBER #define MIST_ENABLE_OUTPUT_NUMBER 6 @@ -53,138 +53,151 @@ gpioDigitalOutput *flood_enable_output = nullptr; /**** Static functions ****/ -static void _exec_coolant_control(float* value, bool* flags); +static void _exec_coolant_control(float* value, bool* flag); -/* +/**************************************************************************************** * coolant_init() * coolant_reset() */ void coolant_init() { if (MIST_ENABLE_OUTPUT_NUMBER > 0) { - mist_enable_output = d_out[MIST_ENABLE_OUTPUT_NUMBER-1]; - mist_enable_output->setEnabled(IO_ENABLED); - mist_enable_output->setPolarity((ioPolarity)COOLANT_MIST_POLARITY); + coolant.mist.output = d_out[MIST_ENABLE_OUTPUT_NUMBER-1]; + coolant.mist.output->setEnabled(IO_ENABLED); + coolant.mist.output->setPolarity((ioPolarity)COOLANT_MIST_POLARITY); } if (FLOOD_ENABLE_OUTPUT_NUMBER > 0) { - flood_enable_output = d_out[FLOOD_ENABLE_OUTPUT_NUMBER-1]; - flood_enable_output->setEnabled(IO_ENABLED); - flood_enable_output->setPolarity((ioPolarity)COOLANT_FLOOD_POLARITY); + coolant.flood.output = d_out[FLOOD_ENABLE_OUTPUT_NUMBER-1]; + coolant.flood.output->setEnabled(IO_ENABLED); + coolant.flood.output->setPolarity((ioPolarity)COOLANT_FLOOD_POLARITY); } - coolant.mist_enable = COOLANT_OFF; - coolant.flood_enable = COOLANT_OFF; + coolant.mist.state = COOLANT_OFF; + coolant.flood.state = COOLANT_OFF; } void coolant_reset() { coolant_init(); - cm_coolant_off_immediate(); + coolant_control_immediate(COOLANT_OFF, COOLANT_BOTH); } -/* - * cm_coolant_off_immediate() - turn off all coolant - * cm_coolant_optional_pause() - pause coolants if option is true - * cm_coolant_resume() - restart paused coolants +/**************************************************************************************** + * coolant_control_immediate() - execute coolant control immediately + * coolant_control_sync() - queue a coolant control to the planner buffer + * _exec_coolant_control() - actually execute the coolant command */ -void cm_coolant_off_immediate() { - float value[] = {0, 0, 0, 0, 0, 0}; - bool flags[] = {1, 1, 0, 0, 0, 0}; +stat_t coolant_control_immediate(coControl control, coSelect select) +{ + float value[] = { (float)control }; + bool flags[] = { (select & COOLANT_MIST), (select & COOLANT_FLOOD) }; _exec_coolant_control(value, flags); + return(STAT_OK); } -void cm_coolant_optional_pause(bool option) { - if (option) { - float value[] = {0, 0, 0, 0, 0, 0}; - bool flags[] = {0, 0, 0, 0, 0, 0}; - - if (coolant.flood_enable == COOLANT_ON) { - coolant.flood_enable = COOLANT_PAUSE; // mark as paused - flags[COOLANT_FLOOD] = 1; // set flag so it will turn off - } - if (coolant.mist_enable == COOLANT_ON) { - coolant.mist_enable = COOLANT_PAUSE; // mark as paused - flags[COOLANT_MIST] = 1; - } - _exec_coolant_control(value, flags); // execute (w/o changing local state) +stat_t coolant_control_sync(coControl control, coSelect select) +{ + // Skip the PAUSE operation if pause_enable is not enabled (pause-on-hold) + // The pause setting in mist coolant is treated as the master even though it's replicated in flood + if ((control == COOLANT_PAUSE) && (!coolant.mist.pause_enable)) { + return (STAT_OK); } -} -void cm_coolant_resume() { -// float value[] = { 1,1,0,0,0,0 }; // ++++ Will this work? No need to set 'value' below - float value[] = {0, 0, 0, 0, 0, 0}; - bool flags[] = {0, 0, 0, 0, 0, 0}; - - if (coolant.flood_enable == COOLANT_PAUSE) { - coolant.flood_enable = COOLANT_ON; - value[COOLANT_FLOOD] = 1; - flags[COOLANT_FLOOD] = true; - } - if (coolant.mist_enable == COOLANT_PAUSE) { - coolant.mist_enable = COOLANT_ON; - value[COOLANT_MIST] = 1; - flags[COOLANT_MIST] = true; - } - _exec_coolant_control(value, flags); -} - -/* - * cm_mist_coolant_control() - access points from Gcode parser - * cm_flood_coolant_control() - access points from Gcode parser - * _exec_coolant_control() - combined flood and mist coolant control - * - * - value[0] is flood state - * - value[1] is mist state - * - uses flags to determine which to run - */ - -stat_t cm_flood_coolant_control(uint8_t flood_enable) { - float value[] = {(float)flood_enable, 0, 0, 0, 0, 0}; - bool flags[] = {1, 0, 0, 0, 0, 0}; + // queue the coolant control + float value[] = { (float)control }; + bool flags[] = { (select & COOLANT_MIST), (select & COOLANT_FLOOD) }; mp_queue_command(_exec_coolant_control, value, flags); - return (STAT_OK); + return(STAT_OK); } -stat_t cm_mist_coolant_control(uint8_t mist_enable) { - float value[] = {0, (float)mist_enable, 0, 0, 0, 0}; - bool flags[] = {0, 1, 0, 0, 0, 0}; - mp_queue_command(_exec_coolant_control, value, flags); - return (STAT_OK); -} +static void _exec_coolant_helper(coCoolantChannel_t &co, coControl control) { -// NOTE: flood and mist coolants are mapped to the same pin - see hardware.h -//#define _set_flood_enable_bit_hi() flood_enable_pin.set() -//#define _set_flood_enable_bit_lo() flood_enable_pin.clear() -//#define _set_mist_enable_bit_hi() mist_enable_pin.set() -//#define _set_mist_enable_bit_lo() mist_enable_pin.clear() - -static void _exec_coolant_control(float* value, bool* flags) { - if (flags[COOLANT_FLOOD]) { - coolant.flood_enable = (cmCoolantEnable)value[COOLANT_FLOOD]; -// if (!((coolant.flood_enable & 0x01) ^ coolant.flood_polarity)) { // inverted XOR -// _set_flood_enable_bit_hi(); -// } else { -// _set_flood_enable_bit_lo(); -// } - if (flood_enable_output != nullptr) { - flood_enable_output->setValue(coolant.flood_enable); + bool action = true; + int8_t enable_bit = 0; + switch (control) { + case COOLANT_OFF: { + co.state = COOLANT_OFF; + break; + } + case COOLANT_ON: { + co.state = COOLANT_ON; + enable_bit = 1; + break; + } + case COOLANT_PAUSE: { + if (co.state == COOLANT_ON) { + co.state = COOLANT_PAUSE; + } else { + action = false; + } + break; + } + case COOLANT_RESUME: { + if (co.state == COOLANT_PAUSE) { + co.state = COOLANT_ON; + enable_bit = 1; + } else { + action = false; + } + break; } } - if (flags[COOLANT_MIST]) { - coolant.mist_enable = (cmCoolantEnable)value[COOLANT_MIST]; -// if (!((coolant.mist_enable & 0x01) ^ coolant.mist_polarity)) { -// _set_mist_enable_bit_hi(); -// } else { -// _set_mist_enable_bit_lo(); -// } - if (mist_enable_output != nullptr) { - mist_enable_output->setValue(coolant.mist_enable); + if (action) { + if (co.output != nullptr) { + co.output->setValue(enable_bit); } } } -/*********************************************************************************** +static void _exec_coolant_control(float* value, bool* flag) { + + coControl control = (coControl)value[0]; + if (control > COOLANT_ACTION_MAX) { + return; + } + if (flag[0]) { // Mist, M7 + _exec_coolant_helper(coolant.mist, control); + } + if (flag[1]) { // Flood, M8 + _exec_coolant_helper(coolant.flood, control); + } +} + +/**************************************************************************************** + **** Coolant Settings ****************************************************************** + ****************************************************************************************/ + +stat_t co_get_com(nvObj_t *nv) { return(get_integer(nv, coolant.mist.state)); } +stat_t co_set_com(nvObj_t *nv) { return(coolant_control_immediate((coControl)nv->value_int, COOLANT_MIST)); } +stat_t co_get_cof(nvObj_t *nv) { return(get_integer(nv, coolant.flood.state)); } +stat_t co_set_cof(nvObj_t *nv) { return(coolant_control_immediate((coControl)nv->value_int, COOLANT_FLOOD)); } + +stat_t co_get_coph(nvObj_t *nv) { return(get_integer(nv, coolant.mist.pause_enable)); } +stat_t co_set_coph(nvObj_t *nv) +{ + ritorno(set_integer(nv, (uint8_t &)coolant.mist.pause_enable, 0, 1)); + return (set_integer(nv, (uint8_t &)coolant.flood.pause_enable, 0, 1)); +} + +stat_t co_get_comp(nvObj_t *nv) { return(get_integer(nv, coolant.mist.polarity)); } +stat_t co_set_comp(nvObj_t *nv) { + stat_t ret = set_integer(nv, (uint8_t &)coolant.mist.polarity, 0, 1); + if (ret == STAT_OK) { + coolant.mist.output->setPolarity((ioPolarity) coolant.mist.polarity); + } + return ret; +} +stat_t co_get_cofp(nvObj_t *nv) { return(get_integer(nv, coolant.flood.polarity)); } +stat_t co_set_cofp(nvObj_t *nv) { + stat_t ret = set_integer(nv, (uint8_t &)coolant.flood.polarity, 0, 1); + if (ret == STAT_OK) { + coolant.flood.output->setPolarity((ioPolarity) coolant.flood.polarity); + } + return ret; +} + +/**************************************************************************************** * TEXT MODE SUPPORT - * Functions to print variables from the cfgArray table + * Functions to print variables from the cfgArray table***** ***********************************************************************************/ #ifdef __TEXT_MODE @@ -192,13 +205,13 @@ static void _exec_coolant_control(float* value, bool* flags) { const char fmt_coph[] = "[coph] coolant pause on hold%7d [0=no,1=pause_on_hold]\n"; const char fmt_comp[] = "[comp] coolant mist polarity%7d [0=low is ON,1=high is ON]\n"; const char fmt_cofp[] = "[cofp] coolant flood polarity%6d [0=low is ON,1=high is ON]\n"; -const char fmt_com[] = "Mist coolant:%6d [0=OFF,1=ON]\n"; -const char fmt_cof[] = "Flood coolant:%5d [0=OFF,1=ON]\n"; +const char fmt_com[] = "[com] coolant mist%16d [0=OFF,1=ON]\n"; +const char fmt_cof[] = "[cof] coolant flood%15d [0=OFF,1=ON]\n"; -void cm_print_coph(nvObj_t* nv) { text_print(nv, fmt_coph); } // TYPE_INT -void cm_print_comp(nvObj_t* nv) { text_print(nv, fmt_comp); } // TYPE_INT -void cm_print_cofp(nvObj_t* nv) { text_print(nv, fmt_cofp); } // TYPE_INT -void cm_print_com(nvObj_t* nv) { text_print(nv, fmt_com); } // TYPE_INT -void cm_print_cof(nvObj_t* nv) { text_print(nv, fmt_cof); } // TYPE_INT +void co_print_coph(nvObj_t* nv) { text_print(nv, fmt_coph); } // TYPE_INT +void co_print_comp(nvObj_t* nv) { text_print(nv, fmt_comp); } // TYPE_INT +void co_print_cofp(nvObj_t* nv) { text_print(nv, fmt_cofp); } // TYPE_INT +void co_print_com(nvObj_t* nv) { text_print(nv, fmt_com); } // TYPE_INT +void co_print_cof(nvObj_t* nv) { text_print(nv, fmt_cof); } // TYPE_INT #endif // __TEXT_MODE diff --git a/g2core/coolant.h b/g2core/coolant.h old mode 100755 new mode 100644 index b30df2a4..c8e7f375 --- a/g2core/coolant.h +++ b/g2core/coolant.h @@ -2,7 +2,7 @@ * coolant.h - coolant driver * This file is part of the g2core project * - * Copyright (c) 2015 - 2016 Alden S. Hart, Jr. + * Copyright (c) 2015 - 2018 Alden S. Hart, Jr. * * This file ("the software") is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License, version 2 as published by the @@ -28,34 +28,49 @@ #ifndef COOLANT_H_ONCE #define COOLANT_H_ONCE +#include "gpio.h" + typedef enum { - COOLANT_OFF = 0, // don't change the order. It gets masked. - COOLANT_ON, - COOLANT_PAUSE -} cmCoolantEnable; + COOLANT_OFF = 0, // don't change the order. 0/1 get masked. + COOLANT_ON = 1, +// COOLANT_RESERVED = 2, // not used. ALigns pause and resume with spindle enum + COOLANT_PAUSE = 3, + COOLANT_RESUME = 4 +} coControl; +#define COOLANT_ACTION_MAX COOLANT_RESUME -typedef enum { COOLANT_ACTIVE_LOW = 0, COOLANT_ACTIVE_HIGH } cmCoolantPolarity; +// *** NOTE: The coolant polarity active hi/low values currently agree with ioMode in gpio.h +// These will all need to be changed to ACTIVE_HIGH = 0, ACTIVE_LOW = 1 +// See: https://github.com/synthetos/g2_private/wiki/GPIO-Design-Discussion#settings-common-to-all-io-types -typedef enum { // used to index the value and flag vectors for coolant execs - COOLANT_FLOOD = 0, - COOLANT_MIST -} cmCoolantIndex; +typedef enum { + COOLANT_ACTIVE_LOW = 0, + COOLANT_ACTIVE_HIGH = 1 +} coPolarity; + +typedef enum { // don't change the order. used as a bitmask + COOLANT_NONE = 0, // none selected + COOLANT_MIST = 1, // mist coolant + COOLANT_FLOOD = 2, // flood coolant + COOLANT_BOTH = 3 // both types ('or' of both coolants) +} coSelect; /* - * Coolant control structure + * Coolant control structures */ -typedef struct cmCoolantSingleton { - bool pause_on_hold; // true to pause coolant on feedhold +typedef struct coCoolantChannel { + bool pause_enable; // {coph:} pause on feedhold + coControl state; // coolant state: coControl + coPolarity polarity; // 0=active low, 1=active high + gpioDigitalOutput *output; // gpio output pointer (may be NULL) +} coCoolantChannel_t; - cmCoolantEnable flood_enable; // COOLANT_ON = flood on (M8), COOLANT_OFF = off (M9) - cmCoolantPolarity flood_polarity; // 0=active low, 1=active high - - cmCoolantEnable mist_enable; // COOLANT_ON = mist on (M7), COOLANT_OFF = off (M9) - cmCoolantPolarity mist_polarity; // 0=active low, 1=active high - -} cmCoolantSingleton_t; -extern cmCoolantSingleton_t coolant; +typedef struct coCoolant { + coCoolantChannel_t mist; // M7 - treated as the "master" for reading pause setting + coCoolantChannel_t flood; // M8 +} coCoolant_t; +extern coCoolant_t coolant; /* * Global Scope Functions @@ -64,29 +79,39 @@ extern cmCoolantSingleton_t coolant; void coolant_init(); void coolant_reset(); -stat_t cm_mist_coolant_control(uint8_t mist_enable); // M7 -stat_t cm_flood_coolant_control(uint8_t flood_enable); // M8, M9 -void cm_coolant_off_immediate(void); -void cm_coolant_optional_pause(bool option); -void cm_coolant_resume(void); +stat_t coolant_control_immediate(coControl control, coSelect select); +stat_t coolant_control_sync(coControl control, coSelect select); + +stat_t co_get_coph(nvObj_t *nv); +stat_t co_set_coph(nvObj_t *nv); + +stat_t co_get_comp(nvObj_t *nv); +stat_t co_set_comp(nvObj_t *nv); +stat_t co_get_cofp(nvObj_t *nv); +stat_t co_set_cofp(nvObj_t *nv); + +stat_t co_get_com(nvObj_t *nv); +stat_t co_set_com(nvObj_t *nv); +stat_t co_get_cof(nvObj_t *nv); +stat_t co_set_cof(nvObj_t *nv); /*--- text_mode support functions ---*/ #ifdef __TEXT_MODE -void cm_print_coph(nvObj_t* nv); // coolant pause on hold -void cm_print_comp(nvObj_t* nv); // coolant polarity mist -void cm_print_cofp(nvObj_t* nv); // coolant polarity flood -void cm_print_com(nvObj_t* nv); // report mist coolant state -void cm_print_cof(nvObj_t* nv); // report flood coolant state +void co_print_coph(nvObj_t* nv); // coolant pause on hold +void co_print_comp(nvObj_t* nv); // coolant polarity mist +void co_print_cofp(nvObj_t* nv); // coolant polarity flood +void co_print_com(nvObj_t* nv); // report mist coolant state +void co_print_cof(nvObj_t* nv); // report flood coolant state #else -#define cm_print_coph tx_print_stub -#define cm_print_comp tx_print_stub -#define cm_print_cofp tx_print_stub -#define cm_print_com tx_print_stub -#define cm_print_cof tx_print_stub +#define co_print_coph tx_print_stub +#define co_print_comp tx_print_stub +#define co_print_cofp tx_print_stub +#define co_print_com tx_print_stub +#define co_print_cof tx_print_stub #endif // __TEXT_MODE diff --git a/g2core/cycle_feedhold.cpp b/g2core/cycle_feedhold.cpp new file mode 100644 index 00000000..f33366c5 --- /dev/null +++ b/g2core/cycle_feedhold.cpp @@ -0,0 +1,809 @@ +/* + * cycle_feedhold.cpp - canonical machine feedhold processing + * This file is part of the g2core project + * + * Copyright (c) 2010 - 2018 Alden S Hart, Jr. + * Copyright (c) 2014 - 2018 Robert Giseburt + * + * This file ("the software") is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License, version 2 as published by the + * Free Software Foundation. You should have received a copy of the GNU General Public + * License, version 2 along with the software. If not, see . + * + * As a special exception, you may use this file as part of a software library without + * restriction. Specifically, if other files instantiate templates or use macros or + * inline functions from this file, or you compile this file and link it with other + * files to produce an executable, this file does not by itself cause the resulting + * executable to be covered by the GNU General Public License. This exception does not + * however invalidate any other reasons why the executable file might be covered by the + * GNU General Public License. + * + * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT WITHOUT ANY + * WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES + * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT + * SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF + * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + */ + +#include "g2core.h" // #1 +#include "config.h" // #2 +#include "gcode.h" // #3 +#include "canonical_machine.h" +#include "planner.h" +#include "plan_arc.h" +#include "stepper.h" +#include "spindle.h" +#include "coolant.h" +#include "util.h" +//#include "xio.h" // DIAGNOSTIC + +//static void _start_feedhold(void); +static void _start_cycle_restart(void); +static void _start_queue_flush(void); +static void _start_job_kill(void); + +// Feedhold actions +static stat_t _feedhold_skip(void); +static stat_t _feedhold_no_actions(void); +static stat_t _feedhold_with_actions(void); +static stat_t _feedhold_restart_with_actions(void); +static stat_t _feedhold_restart_no_actions(void); + +// Feedhold exits (finalization) +static stat_t _run_restart_cycle(void); +static stat_t _run_queue_flush(void); +static stat_t _run_program_stop(void); +static stat_t _run_program_end(void); +static stat_t _run_alarm(void); +static stat_t _run_shutdown(void); +static stat_t _run_interlock(void); +static stat_t _run_reset_position(void); + +/**************************************************************************************** + * OPERATIONS AND ACTIONS + * + * Operations provide a way to assemble a multi-step function from underlying actions, + * then execute the actions in sequence until the operation either completes or returns + * an error. It handles actions that complete immediately (synchronous) as well as long + * running asynchronous operations such as a series of multiple moves. + * + * It works by assembling an operation using a series of add_action() calls, then running + * the operation by one or more run_operation() calls. The cm_operation_sequencing_callback() + * both runs long-running operations, as well as queues operation requests that must + * run sequentially of have other conditions. + * + * Actions are coded to return: + * STAT_OK - successful completion of the action + * STAT_EAGAIN - ran to continuation - the action needs to be called again to complete + * STAT_XXXXX - any other status is an error that should quit the operation + * + * run_operation() returns: + * STAT_NOOP - no operation is set up, but it's OK to call the operation runner + * STAT_OK - operation has completed successfully + * STAT_EAGAIN - operation needs to be re-entered to complete (via operation callback) + * STAT_XXXXX - any other status is an error that quits the operation + * + * Current constraints to keep this simple (at least for now): + * - Operations run to completion. They cannot be canceled, or preempted by other operations + * - Actions cannot be added to an operation once it is being run + * - Actions do not have parameters. Use the CM context if needed (e.g. hold_type) + */ + +/*** Object Definitions ***/ + +#define ACTION_MAX 6 // maximum actions that can be queued for an operation +typedef stat_t (*action_exec_t)(); // callback to action execution function + +typedef struct cmAction { // struct to manage execution of operations + uint8_t number; // DIAGNOSTIC for easier debugging. Not used functionally. + struct cmAction *nx; // static pointer to next buffer + action_exec_t func; // callback to operation action function. NULL == disabled + + void reset() { // clears function pointer + func = NULL; + }; +} cmAction_t; + +typedef struct cmOperation { // operation runner object + + cmAction action[ACTION_MAX]; // singly linked list of action structures + cmAction *add; // pointer to next action to be added + cmAction *run; // pointer to action being executed + bool in_operation; // set true when an operation is running + + void reset(void) { + for (uint8_t i=0; i < ACTION_MAX; i++) { + action[i].reset(); // reset the action controller object + action[i].number = i; // DIAGNOSTIC only. Otherwise not used + action[i].nx = &action[i+1]; // link to the next action + } + action[ACTION_MAX-1].nx = NULL; // set last action (end of list) + add = action; // initialize pointers to first action struct + run = action; + in_operation = false; + }; + + stat_t add_action(stat_t(*action_exec)()) { + if (in_operation) { return (STAT_COMMAND_NOT_ACCEPTED); } // can't add + if (add == NULL) { return (STAT_INPUT_EXCEEDS_MAX_LENGTH); } // no more room + add->func = action_exec; + add = add->nx; + return (STAT_OK); + }; + + stat_t run_operation(void) { + if (run->func == NULL) { return (STAT_NOOP); } // not an error. This is normal. + in_operation = true; // disable add_action during operations + + stat_t status; + while ((status = run->func()) == STAT_OK) { + run = run->nx; + if (run->func == NULL) { // operation has completed + reset(); // setup for next operation + return (STAT_OK); + } + } + if (status == STAT_EAGAIN) { return (STAT_EAGAIN); } + reset(); // reset operation if action threw an error + return (status); // return error code + }; + +} cmOperation_t; + +cmOperation_t op; // operations runner object + +/**************************************************************************************** + * cm_operation_init() + */ + +void cm_operation_init() +{ + op.reset(); +} + +/**************************************************************************************** + **** Feedhold and Related Functions **************************************************** + ****************************************************************************************/ +/* + * Feedholds, queue flushes and the various feedhold exits are all related. + * These are performed in this file and in plan_exec.cpp. Feedholds are implemented + * as a state machine (cmFeedholdState) that runs in these files using Operations. + * + * BACKGROUND: There are 2 planners: p1 (primary planner) and p2 (secondary planner). + * + * A feedhold (!) received while in p1 stops motion in p1 and optionally transitions to p2, + * where feedhold entry actions such as Z lift, parking moves, spindle and coolant pause + * are run. While in p2 (almost) all machine operations are available. There are different + * Types of feedholds; Feedhold with action and feedholds with no action transition to p2, + * but others do not (e.g. feedhold with sync). + * + * A cycle_start (~) returns to p1 and exits the feedhold, performing exit actions if entry + * actions were performed. Motion resumes in p1 from the held point. + * + * A queue_flush (~) returns to p1 and exits the feedhold, performing exit actions if entry + * actions were performed. The p1 planner is flushed, and motion does not resume. The machine + * executes a program_stop, and ends in the STOP state. + * + * A feedhold (!) received while in p2 (a feedhold within a feedhold - very Inception) + * stops motion in p2 and flushes the p2 planner. Control remains in p2. + * + * Other variants of feedhold and exit exist, but these are invoked internally only to put + * the machine in END, ALARM, SHUTDOWN, INTERLOCK and other states. + */ +/* + * Feedhold State Machine Processing + * + * Feedhold is run as a state machine using the following states: + * FEEDHOLD_OFF - Not in a feedhold. May be in a cycle or not running + * FEEDHOLD_HOLD - Feedhold stable state. Achieved when machine has stopped in the hold. + * FEEDHOLD_XXXX - Any other feedhold state is transient; the machine is headed towards + * FEEDHOLD_HOLD or FEEDHOLD_OFF. + * + * For internal purposes, any state other than FEEDHOLD_OFF is considered to be in a hold. + * + * Feedhold processing performs the following (in rough sequence order): + * + * (0) - Feedhold is request by calling cm_feedhold_request() + * + * Control transfers to plan_exec.cpp feedhold functions: + * + * (1) - Feedhold arrives while we are in the middle executing of a block + * (1a) - The block is currently accelerating - wait for the end of acceleration + * (1b) - The block is in a head, but has not started execution yet - start deceleration + * (1b1) - The deceleration fits into the current block + * (1b2) - The deceleration does not fit and needs to continue in the next block + * (1c) - The block is in a body - start deceleration + * (1c1) - The deceleration fits into the current block + * (1c2) - The deceleration does not fit and needs to continue in the next block + * (1d) - The block is currently in the tail - wait until the end of the block + * (1e) - We have a new block and a new feedhold request that arrived at EXACTLY the same time + * (unlikely, but handled as 1b). + * + * (2) - The block has decelerated to some velocity > zero, so needs continuation into next block + * (3) - The end of deceleration is detected inline in mp_exec_aline() + * (4) - Finished all runtime work, now wait for motion to stop at HOLD point. When it does: + * (4a) - It's a homing or probing feedhold - ditch the remaining buffer & go directly to OFF + * (4b) - It's a p2 feedhold - ditch the remaining buffer & signal we want a p2 queue flush + * (4c) - It's a normal feedhold - signal we want the p2 entry actions to execute + * + * Control transfers back to cycle_feedhold.cpp feedhold functions: + * + * (5) - Run the P2 entry actions and transition to HOLD state when complete + * (6) - Remove the hold state / there is queued motion - see cycle_feedhold.cpp + * (7) - Remove the hold state / there is no queued motion - see cycle_feedhold.cpp + */ + +/**************************************************************************************** + * cm_operation_runner_callback() - run feedhold operations and sequence queued requests + * + * Operations are requested by calling their repective request function, e.g. cm_request_feedhold(). + * The operation callback runs the current operation, and sequences requests that must be queued. + * Expected behaviors: (no-hold means machine is not in hold, etc) + * + * (no-cycle) ! No action. Feedhold is not run (nothing to hold!) + * (no-hold) ~ No action. Cannot exit a feedhold that does not exist + * (no-hold) % No action. Queue flush is not honored except during a feedhold + * (in-cycle) ! Start a hold to motion in the p1 planner + * (in-hold) ~ Wait for feedhold actions to complete, exit feedhold, resume motion + * (in-hold) % Wait for feedhold actions to complete, exit feedhold, do not resume motion + * (in-p2) ! If moving in p2 during a p1 hold ! will perform a SYNC type hold in p2 + * (in-cycle) !~ Start a feedhold, do enter and exit actions, exit feedhold, resume motion + * (in-cycle) !% Start a feedhold, do enter and exit actions, exit feedhold, do not resume motion + * (in-cycle) !%~ Same as above + * (in-cycle) !~% Same as above (this one's an anomaly, but the intent would be to Q flush) + * + * The requests are arranged in priority order, highest priority first. + * Note that feedholds from p1 are initiated immediately from cm_request_feedhld(), + * and are not triggered here. Only queued p2 feedholds (feedhold in feedhold) are + * handled in the sequencer. + */ + +stat_t cm_operation_runner_callback() +{ + if (cm1.job_kill_state == JOB_KILL_REQUESTED) { // job kill must wait for any active hold to complete + _start_job_kill(); + } +// if (cm1.hold_state == FEEDHOLD_REQUESTED) { // look for a queued p2 feedhold +// _start_feedhold(); +// } + if (cm1.queue_flush_state == QUEUE_FLUSH_REQUESTED) { // look for a queued flush request + _start_queue_flush(); + } + if (cm1.cycle_start_state == CYCLE_START_REQUESTED) { // look for a queued cycle start ot restart + _start_cycle_restart(); + } + + // run the operation or operation continuation (callback) + return (op.run_operation()); +} + +/* + * cm_has_hold() - return true if a hold condition exists (or a pending hold request) + */ + +bool cm_has_hold() +{ + return (cm1.hold_state != FEEDHOLD_OFF); +} + +/* + * cm_feedhold_command_blocker() - prevents new Gcode commands from reaching the parser while feedhold is in effect + */ + +stat_t cm_feedhold_command_blocker() +{ + if (cm1.hold_state != FEEDHOLD_OFF) { + return (STAT_EAGAIN); + } + return (STAT_OK); +} + +/* + * end state functions and helpers + */ + +static stat_t _run_program_stop() +{ + cm_cycle_end(); // end cycle and run program stop + return (STAT_OK); +} + +static stat_t _run_program_end() +{ + cm_program_end(); + return (STAT_OK); +} + +static stat_t _run_reset_position() +{ + cm_reset_position_to_absolute_position(cm); + return (STAT_OK); +} + +static stat_t _run_alarm() { return (STAT_OK); } +static stat_t _run_shutdown() { return (STAT_OK); } +static stat_t _run_interlock() { return (STAT_OK); } + +/**************************************************************************************** + * cm_request_cycle_start() - set request enum only + * _start_cycle_start() - run the cycle start + */ + +void cm_request_cycle_start() +{ + if (cm1.hold_state != FEEDHOLD_OFF) { // restart from a feedhold + if (cm1.queue_flush_state == QUEUE_FLUSH_REQUESTED) { // possible race condition. Flush wins + cm1.cycle_start_state = CYCLE_START_OFF; + } else { + cm1.cycle_start_state = CYCLE_START_REQUESTED; + } + } else { // execute cycle start directly + if (mp_has_runnable_buffer(&mp1)) { + cm_cycle_start(); + st_request_exec_move(); + } + cm1.cycle_start_state = CYCLE_START_OFF; + } +} + +static void _start_cycle_restart() +{ + // Feedhold cycle restart builds an operation to complete multiple actions + if (cm1.hold_state == FEEDHOLD_HOLD) { + cm1.cycle_start_state = CYCLE_START_OFF; + switch (cm1.hold_type) { + case FEEDHOLD_TYPE_HOLD: { op.add_action(_feedhold_restart_no_actions); break; } + case FEEDHOLD_TYPE_ACTIONS: { op.add_action(_feedhold_restart_with_actions); break; } + default: {} + } + switch (cm1.hold_exit) { + case FEEDHOLD_EXIT_CYCLE: { op.add_action(_run_restart_cycle); break; } + case FEEDHOLD_EXIT_FLUSH: { op.add_action(_run_queue_flush); } // no break + case FEEDHOLD_EXIT_STOP: { op.add_action(_run_program_stop); break; } + case FEEDHOLD_EXIT_END: { op.add_action(_run_program_end); break; } + case FEEDHOLD_EXIT_ALARM: { op.add_action(_run_alarm); break; } + case FEEDHOLD_EXIT_SHUTDOWN: { op.add_action(_run_shutdown); break; } + case FEEDHOLD_EXIT_INTERLOCK: { op.add_action(_run_interlock); break; } + default: {} + } + } +} + +/**************************************************************************************** + * cm_request_queue_flush() - set request enum only + * _start_queue_flush() - run a queue flush from a % + * _run_queue_flush() - run a queue flush from an action + * + * cm_request_queue_flush() should be called concurrently with xio_flush_to_command(), like this: + * { cm_request_queue_flush(); xio_flush_to_command(); } + */ + +void cm_request_queue_flush() +{ + // Can only initiate a queue flush if in a feedhold + if (cm1.hold_state != FEEDHOLD_OFF) { + cm1.queue_flush_state = QUEUE_FLUSH_REQUESTED; + } else { + cm1.queue_flush_state = QUEUE_FLUSH_OFF; + } +} + +static void _start_queue_flush() +{ + // Don't initiate the queue until in HOLD state (this also means that runtime is idle) + if ((cm1.queue_flush_state == QUEUE_FLUSH_REQUESTED) && (cm1.hold_state == FEEDHOLD_HOLD)) { + if (cm1.hold_type == FEEDHOLD_TYPE_ACTIONS) { + op.add_action(_feedhold_restart_with_actions); + } else { + op.add_action(_feedhold_restart_no_actions); + } + op.add_action(_run_queue_flush); + op.add_action(_run_program_stop); + } +} + +// _run_queue_flush() should not be called until motion has stopped. +// It is completely synchronous so it can be called directly; +// it does not need to be part of an operation(). + +static stat_t _run_queue_flush() // typically runs from cm1 planner +{ + cm_abort_arc(cm); // kill arcs so they don't just create more alines + planner_reset((mpPlanner_t *)cm->mp); // reset primary planner. also resets the mr under the planner + cm_reset_position_to_absolute_position(cm); + cm1.queue_flush_state = QUEUE_FLUSH_OFF; + qr_request_queue_report(0); // request a queue report, since we've changed the number of buffers available + return (STAT_OK); +} + +/**************************************************************************************** + * cm_request_job_kill() - Control-D handler - set request flag only by ^d + * _run_job_kill() - perform the job kill. queue flush, program_end + * _start_job_kill() - invoke the job kill function, which may start from various states + * + * cm_request_job_kill() should be called concurrently with xio_flush_to_command(), like this: + * { cm_request_job_kill(); xio_flush_to_command(); } + * + * Job kill cases: Actions: + * (0) job kill from ALARM, SHUTDOWN, PANIC no action, end request + * (1) job kill from READY, STOP, END perform PROGRAM_END + * (2a) Job kill from machining cycle hold, flush, perform PROGRAM_END + * (2b) Job kill from pending hold wait for hold to complete + * (2c) Job kill from finished hold flush, perform PROGRAM_END + * (3) Job kill from PROBE flush, perform PROGRAM_END + * (4) Job kill from HOMING flush, perform PROGRAM_END + * (5) Job kill from JOGGING flush, perform PROGRAM_END + * (6) job kill from INTERLOCK perform PROGRAM_END + */ + +void cm_request_job_kill() +{ + cm1.job_kill_state = JOB_KILL_REQUESTED; +} + +// _run_job_kill() should not be called until motion has stopped. +// It is completely synchronous so it can be called directly; +// it does not need to be part of an operation(). + +static stat_t _run_job_kill() +{ + // if in p2 switch to p1 and copy actual position back to p1 + if (cm == &cm2) { + cm = &cm1; // return to primary planner (p1) + mp = (mpPlanner_t *)cm->mp; // cm->mp is a void pointer + mr = mp->mr; + + copy_vector(cm1.gmx.position, mr2.position); // transfer actual position back to p1 + copy_vector(cm1.gm.target, mr2.position); + copy_vector(mp1.position, mr2.position); + copy_vector(mr1.position, mr2.position); + } + + _run_queue_flush(); + + coolant_control_immediate(COOLANT_OFF, COOLANT_BOTH); // stop coolant + spindle_control_immediate(SPINDLE_OFF); // stop spindle + + cm_set_motion_state(MOTION_STOP); // set to stop and set the active model + cm->hold_state = FEEDHOLD_OFF; + cm_program_end(); + + rpt_exception(STAT_KILL_JOB, "Job killed by ^d"); + sr_request_status_report(SR_REQUEST_IMMEDIATE); + cm->job_kill_state = JOB_KILL_OFF; + return (STAT_OK); +} + +// _start_job_kill() will be entered multiple times until the REQUEST is reset to OFF + +static void _start_job_kill() +{ + switch (cm1.machine_state) { + case MACHINE_ALARM: // Case 0's - nothing to do. turn off the request + case MACHINE_SHUTDOWN: + case MACHINE_PANIC: { + cm1.job_kill_state = JOB_KILL_OFF; + return; + } + case MACHINE_CYCLE: { // Case 2's + if (cm1.hold_state == FEEDHOLD_OFF) { // Case 2a - in cycle and not in a hold + op.add_action(_feedhold_no_actions); +// op.add_action(_run_job_kill); + } + if (cm1.hold_state == FEEDHOLD_HOLD) { // Case 2c - in a finished hold + _run_job_kill(); + } + return; // Case 2b - hold is in progress. Wait for hold to reach HOLD + } + default: { _run_job_kill(); } // Cases 1,3,4,5,6 + } +} + +/**************************************************************************************** + * cm_request_feedhold() - request a feedhold - do not run it yet + * _feedhold_skip() - run feedhold that will skip remaining unused buffer length + * _feedhold_no_actions() - run feedhold with no entry actions + * _feedhold_with_actions() - run feedhold entry actions + * _feedhold_actions_done_callback() - planner callback to reach sync point + * + * Input arguments + * - See cmFeedholdType - how the feedhold will execute + * - See cmFeedholdFinal - the final state when the feedhold is exited + */ + +void cm_request_feedhold(cmFeedholdType type, cmFeedholdExit exit) +{ + // Can only initiate a feedhold if you are in a machining cycle, running, and not already in a feedhold + + // +++++ This needs to be extended to allow HOLDs to be requested when motion has stopped + if ((cm1.hold_state == FEEDHOLD_OFF) && + (cm1.machine_state == MACHINE_CYCLE) && (cm1.motion_state == MOTION_RUN)) { + + cm1.hold_type = type; + cm1.hold_exit = exit; + cm1.hold_profile = ((type == FEEDHOLD_TYPE_ACTIONS) || (type == FEEDHOLD_TYPE_HOLD)) ? + PROFILE_NORMAL : PROFILE_FAST; + + switch (cm1.hold_type) { + case FEEDHOLD_TYPE_HOLD: { op.add_action(_feedhold_no_actions); break; } + case FEEDHOLD_TYPE_ACTIONS: { op.add_action(_feedhold_with_actions); break; } + case FEEDHOLD_TYPE_SKIP: { op.add_action(_feedhold_skip); break; } + default: {} + } + switch (cm1.hold_exit) { + case FEEDHOLD_EXIT_STOP: { op.add_action(_run_program_stop); break; } + case FEEDHOLD_EXIT_END: { op.add_action(_run_program_end); break; } + case FEEDHOLD_EXIT_ALARM: { op.add_action(_run_alarm); break; } + case FEEDHOLD_EXIT_SHUTDOWN: { op.add_action(_run_shutdown); break; } + case FEEDHOLD_EXIT_INTERLOCK: { op.add_action(_run_interlock); break; } + case FEEDHOLD_EXIT_RESET_POSITION: { op.add_action(_run_reset_position); break; } + default: {} + } + return; + } + + // Look for p2 feedhold (feedhold in a feedhold) + if ((cm1.hold_state == FEEDHOLD_HOLD) && + (cm2.hold_state == FEEDHOLD_OFF) && (cm2.machine_state == MACHINE_CYCLE)) { + cm2.hold_state = FEEDHOLD_REQUESTED; + return; + } + + // Reset the request if it's invalid + if ((cm1.machine_state != MACHINE_CYCLE) || (cm1.motion_state == MOTION_STOP)) { + cm->hold_state = FEEDHOLD_OFF; // cannot honor the feedhold request. reset it + } + +} +/* +static void _start_p2_feedhold() +{ + // P2 feedholds only allow skip types + if ((cm2.hold_state == FEEDHOLD_REQUESTED) && (cm2.motion_state == MOTION_RUN)) { + op.add_action(_feedhold_skip); + cm2.hold_state = FEEDHOLD_SYNC; + } +} +*/ + +/* + * _enter_p2() - enter p2 planner with proper state transfer from p1 + * _exit_p2() - reenter p1 planner with proper state transfer from p2 + * + * Encapsulate entering and exiting p2, as this is tricky and must be done exactly right + */ + +static void _enter_p2() +{ + // Copy the primary canonical machine to the secondary. Here it's OK to co a memcpy. + // Set parameters in cm, gm and gmx so you can actually use it + memcpy(&cm2, &cm1, sizeof(cmMachine_t)); + cm2.hold_state = FEEDHOLD_OFF; + cm2.gm.motion_mode = MOTION_MODE_CANCEL_MOTION_MODE; + cm2.gm.absolute_override = ABSOLUTE_OVERRIDE_OFF; + cm2.queue_flush_state = QUEUE_FLUSH_OFF; + cm2.gm.feed_rate = 0; + cm2.arc.run_state = BLOCK_INACTIVE; // Stop a running p1 arc from continuing to execute in p2 + + // Set mp planner to p2 and reset it + cm2.mp = &mp2; + planner_reset((mpPlanner_t *)cm2.mp); // mp is a void pointer + + // Clear the target and set the positions to the current hold position + memset(&(cm2.return_flags), 0, sizeof(cm2.return_flags)); + memset(&(cm2.gm.target), 0, sizeof(cm2.gm.target)); + memset(&(cm2.gm.target_comp), 0, sizeof(cm2.gm.target_comp)); // zero Kahan compensation + + copy_vector(cm2.gmx.position, mr1.position); + copy_vector(mp2.position, mr1.position); + copy_vector(mr2.position, mr1.position); + + // Copy MR position and encoder terms - needed for following error correction state + copy_vector(mr2.target_steps, mr1.target_steps); + copy_vector(mr2.position_steps, mr1.position_steps); + copy_vector(mr2.commanded_steps, mr1.commanded_steps); + copy_vector(mr2.encoder_steps, mr1.encoder_steps); // NB: following error is re-computed in p2 + + // Reassign the globals to the secondary CM + cm = &cm2; + mp = (mpPlanner_t *)cm2.mp; // mp is a void pointer + mr = mp2.mr; +} + +static void _exit_p2() +{ + cm = &cm1; // return to primary planner (p1) + mp = (mpPlanner_t *)cm1.mp; // cm->mp is a void pointer + mr = mp1.mr; +} + +static void _check_motion_stopped() +{ + if (mp_runtime_is_idle()) { // wait for steppers to actually finish + + mpBuf_t *bf = mp_get_r(); + + // Motion has stopped, so we can rely on positions and other values to be stable + // If SKIP type, discard the remainder of the block and position to the next block + if (cm->hold_type == FEEDHOLD_TYPE_SKIP) { + copy_vector(mp->position, mr->position); // update planner position to the final runtime position + mp_free_run_buffer(); // advance to next block, discarding the rest of the move + } else { // Otherwise setup the block to complete motion (regardless of how hold will ultimately be exited) + bf->length = get_axis_vector_length(mr->position, mr->target); // update bf w/remaining length in move + bf->block_state = BLOCK_INITIAL_ACTION; // tell _exec to re-use the bf buffer + bf->buffer_state = MP_BUFFER_BACK_PLANNED; // so it can be forward planned again + bf->plannable = true; // needed so block can be re-planned + } + mr->reset(); // reset MR for next use and for forward planning + cm_set_motion_state(MOTION_STOP); + cm->hold_state = FEEDHOLD_MOTION_STOPPED; + sr_request_status_report(SR_REQUEST_IMMEDIATE); + } +} + +static stat_t _feedhold_skip() +{ + if (cm1.hold_state == FEEDHOLD_OFF) { // if entered while OFF start a feedhold + cm1.hold_type = FEEDHOLD_TYPE_SKIP; + cm1.hold_state = FEEDHOLD_SYNC; // ...FLUSH can be overridden by setting hold_exit after this function + } + if (cm1.hold_state < FEEDHOLD_MOTION_STOPPED) { + return (STAT_EAGAIN); + } + cm1.hold_state = FEEDHOLD_OFF; // cannot be in HOLD or command won't plan (see mp_plan_block_list()) + mp_replan_queue(mp_get_r()); // unplan current forward plan (bf head block), and reset all blocks + st_request_forward_plan(); // replan from the new bf buffer + return (STAT_OK); +} + +static stat_t _feedhold_no_actions() +{ + // initiate the feedhold + if (cm1.hold_state == FEEDHOLD_OFF) { // start a feedhold + cm1.hold_type = FEEDHOLD_TYPE_HOLD; +// cm1.hold_exit = FEEDHOLD_EXIT_STOP; // default exit for NO_ACTIONS is STOP... + + if (cm1.motion_state == MOTION_STOP) { // if motion has already stopped declare that you are in a feedhold + _check_motion_stopped(); + cm1.hold_state = FEEDHOLD_HOLD; + } else { + cm1.hold_state = FEEDHOLD_SYNC; // ... STOP can be overridden by setting hold_exit after this function + return (STAT_EAGAIN); + } + } + + // wait until feedhold reaches the hold point + if (cm1.hold_state < FEEDHOLD_MOTION_STOPPED) { + return (STAT_EAGAIN); + } + // complete the feedhold + mp_replan_queue(mp_get_r()); // unplan current forward plan (bf head block), and reset all blocks + st_request_forward_plan(); // replan from the new bf buffer + cm1.hold_state = FEEDHOLD_HOLD; + return (STAT_OK); +} + +static void _feedhold_actions_done_callback(float* vect, bool* flag) +{ + cm1.hold_state = FEEDHOLD_HOLD_ACTIONS_COMPLETE; // penultimate state before transitioning to FEEDHOLD_HOLD + sr_request_status_report(SR_REQUEST_IMMEDIATE); +} + +static stat_t _feedhold_with_actions() // Execute Case (5) +{ + // if entered while OFF start a feedhold + if (cm1.hold_state == FEEDHOLD_OFF) { + cm1.hold_type = FEEDHOLD_TYPE_ACTIONS; +// cm1.hold_exit = FEEDHOLD_EXIT_STOP; // default exit for ACTIONS is STOP... + if (cm1.motion_state == MOTION_STOP) { // if motion has already stopped declare that you are in a feedhold + _check_motion_stopped(); + cm1.hold_state = FEEDHOLD_HOLD; + } else { + cm1.hold_state = FEEDHOLD_SYNC; // ... STOP can be overridden by setting hold_exit after this function + return (STAT_EAGAIN); + } + } + + // Code to run once motion has stopped + if (cm1.hold_state == FEEDHOLD_MOTION_STOPPED) { + cm->hold_state = FEEDHOLD_HOLD_ACTIONS_PENDING; // next state + _enter_p2(); // enter p2 correctly + cm_set_g30_position(); // set position to return to on exit + + // execute feedhold actions + if (fp_NOT_ZERO(cm->feedhold_z_lift)) { // optional Z lift + cm_set_distance_mode(INCREMENTAL_DISTANCE_MODE); + bool flags[] = { 0,0,1,0,0,0 }; + float target[] = { 0,0, _to_inches(cm->feedhold_z_lift), 0,0,0 }; // convert to inches if in inches mode + cm_straight_traverse(target, flags, PROFILE_NORMAL); + cm_set_distance_mode(cm1.gm.distance_mode); // restore distance mode to p1 setting + } + spindle_control_sync(SPINDLE_PAUSE); // optional spindle pause + coolant_control_sync(COOLANT_PAUSE, COOLANT_BOTH); // optional coolant pause + mp_queue_command(_feedhold_actions_done_callback, nullptr, nullptr); + return (STAT_EAGAIN); + } + + // wait for hold actions to complete + if (cm1.hold_state == FEEDHOLD_HOLD_ACTIONS_PENDING) { + return (STAT_EAGAIN); + } + + // finalize feedhold entry after callback (this is needed so we can return STAT_OK) + if (cm1.hold_state == FEEDHOLD_HOLD_ACTIONS_COMPLETE) { + cm1.hold_state = FEEDHOLD_HOLD; + return (STAT_OK); + } + return (STAT_EAGAIN); // keep the compiler happy. Never executed. +} + +/**************************************************************************************** + * _feedhold_restart_no_actions() - perform hold restart with no actions + * _feedhold_restart_with_actions() - perform hold restart with actions + * _feedhold_restart_actions_done_callback() + */ + +static void _feedhold_restart_actions_done_callback(float* vect, bool* flag) +{ + cm1.hold_state = FEEDHOLD_EXIT_ACTIONS_COMPLETE; // penultimate state before transitioning to FEEDHOLD_OFF + sr_request_status_report(SR_REQUEST_IMMEDIATE); +} + +//+++++ Make this more robust so it handles being called before reaching HOLD state +static stat_t _feedhold_restart_no_actions() +{ + if (cm1.hold_state == FEEDHOLD_OFF) { + return (STAT_OK); // was called erroneously. Can happen for !%~ + } + cm = &cm1; // return to primary planner (p1) + mp = (mpPlanner_t *)cm->mp; // cm->mp is a void pointer + mr = mp->mr; + return (STAT_OK); +} + +static stat_t _feedhold_restart_with_actions() // Execute Cases (6) and (7) +{ + if (cm1.hold_state == FEEDHOLD_OFF) { + return (STAT_OK); // was called erroneously. Can happen for !%~ + } + + // Check to run first-time code + if (cm1.hold_state == FEEDHOLD_HOLD) { + // perform end-hold actions --- while still in secondary machine + coolant_control_sync(COOLANT_RESUME, COOLANT_BOTH); // resume coolant if paused + spindle_control_sync(SPINDLE_RESUME); // resume spindle if paused + + // do return move though an intermediate point; queue a wait + cm2.return_flags[AXIS_Z] = false; + cm_goto_g30_position(cm2.gmx.g30_position, cm2.return_flags); + mp_queue_command(_feedhold_restart_actions_done_callback, nullptr, nullptr); + cm1.hold_state = FEEDHOLD_EXIT_ACTIONS_PENDING; + return (STAT_EAGAIN); + } + + // wait for exit actions to complete + if (cm1.hold_state == FEEDHOLD_EXIT_ACTIONS_PENDING) { + return (STAT_EAGAIN); + } + + // finalize feedhold exit + if (cm1.hold_state == FEEDHOLD_EXIT_ACTIONS_COMPLETE) { + _exit_p2(); // re-enter p1 correctly + return (STAT_OK); + } + + return (STAT_EAGAIN); // still waiting +} + +static stat_t _run_restart_cycle(void) +{ + cm1.hold_state = FEEDHOLD_OFF; // must precede st_request_exec_move() + if (mp_has_runnable_buffer(&mp1)) { + cm_cycle_start(); + st_request_exec_move(); + } else { + cm_cycle_end(); + } + return (STAT_OK); +} diff --git a/g2core/cycle_homing.cpp b/g2core/cycle_homing.cpp index 8e9fecc0..e0c86fe2 100644 --- a/g2core/cycle_homing.cpp +++ b/g2core/cycle_homing.cpp @@ -2,8 +2,8 @@ * cycle_homing.cpp - homing cycle extension to canonical_machine * This file is part of the g2core project * - * Copyright (c) 2010 - 2017 Alden S. Hart, Jr. - * Copyright (c) 2013 - 2017 Robert Giseburt + * Copyright (c) 2010 - 2019 Alden S. Hart, Jr. + * Copyright (c) 2013 - 2019 Robert Giseburt * * This file ("the software") is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License, version 2 as published by the @@ -102,12 +102,12 @@ static stat_t _set_homing_func(stat_t (*func)(int8_t axis)) { */ gpioDigitalInputHandler _homing_handler { [&](const bool state, const inputEdgeFlag edge, const uint8_t triggering_pin_number) { - if (cm.cycle_state != CYCLE_HOMING) { return false; } + if (cm->cycle_type != CYCLE_HOMING) { return false; } if (triggering_pin_number != hm.homing_input) { return false; } if (edge != INPUT_EDGE_LEADING) { return false; } en_take_encoder_snapshot(); - cm_start_hold(); + cm_request_feedhold(FEEDHOLD_TYPE_SKIP, FEEDHOLD_EXIT_RESET_POSITION); return false; // allow others to see this notice }, @@ -121,8 +121,8 @@ gpioDigitalInputHandler _homing_handler { ***********************************************************************************/ /***************************************************************************** - * cm_homing_cycle_start() - G28.2 homing cycle using limit switches - * cm_homing_cycle_callback() - main loop callback for running the homing cycle + * cm_homing_cycle_start() - G28.2 homing cycle using limit switches + * cm_homing_cycle_start_no_set() - G28.4 Homing cycle - do not set values * * --- How does this work? --- * @@ -192,116 +192,120 @@ stat_t cm_homing_cycle_start(const float axes[], const bool flags[]) { hm.set_coordinates = true; // clear rotation matrix - canonical_machine_reset_rotation(); + canonical_machine_reset_rotation(cm); hm.axis = -1; // set to retrieve initial axis hm.func = _homing_axis_start; // bind initial processing function - cm.machine_state = MACHINE_CYCLE; - cm.cycle_state = CYCLE_HOMING; - cm.homing_state = HOMING_NOT_HOMED; + cm->machine_state = MACHINE_CYCLE; + cm->cycle_type = CYCLE_HOMING; + cm->homing_state = HOMING_NOT_HOMED; return (STAT_OK); } stat_t cm_homing_cycle_start_no_set(const float axes[], const bool flags[]) { cm_homing_cycle_start(axes, flags); - hm.set_coordinates = false; // set flag to not update position variables at the end of the cycle + hm.set_coordinates = false; // set flag to not update position variables at the end of the cycle return (STAT_OK); } -stat_t cm_homing_cycle_callback(void) { - if (cm.cycle_state != CYCLE_HOMING) { // exit if not in a homing cycle - return (STAT_NOOP); - } - if (hm.waiting_for_motion_end) { // sync to planner move ends (using callback) - return (STAT_EAGAIN); - } - return (hm.func(hm.axis)); // execute the current homing move -} - -/* - * Homing axis moves - these execute in sequence for each axis - * - * _homing_axis_start() - get next axis, initialize variables, call the clear - * _homing_axis_clear_init() - initiate a clear to move off a switch that is thrown at the start - * _homing_axis_search() - fast search for switch, closes switch - * _homing_axis_clear() - clear off the switch - * _homing_axis_latch() - slow drive until until switch closes again - * _homing_axis_final() - backoff from latch location to zero position - * _homing_axis_move() - helper that actually executes the above moves +/*********************************************************************************** + * cm_homing_cycle_callback() - main loop callback for running the homing cycle */ +stat_t cm_homing_cycle_callback(void) { + if (cm->cycle_type != CYCLE_HOMING) { // exit if not in a homing cycle + return (STAT_NOOP); + } + if (hm.waiting_for_motion_end) { // sync to planner move ends (using callback) + return (STAT_EAGAIN); + } + return (hm.func(hm.axis)); // execute the current homing move +} + +/*********************************************************************************** + * Homing axis moves and helpers - these execute in sequence for each axis + ***********************************************************************************/ + +/*********************************************************************************** + * _homing_axis_start() - get next axis, initialize variables, call the clear + */ static stat_t _homing_axis_start(int8_t axis) { + // get the first or next axis if ((axis = _get_next_axis(axis)) < 0) { // axes are done or error if (axis == -1) { // -1 is done - cm.homing_state = HOMING_HOMED; + cm->homing_state = HOMING_HOMED; return (_set_homing_func(_homing_finalize_exit)); } else if (axis == -2) { // -2 is error return (_homing_error_exit(-2, STAT_HOMING_ERROR_BAD_OR_NO_AXIS)); } } // clear the homed flag for axis so we'll be able to move w/o triggering soft limits - cm.homed[axis] = false; + cm->homed[axis] = false; // trap axis mis-configurations - if (!cm.a[axis].homing_input) { + if (fp_ZERO(cm->a[axis].homing_input)) { return (_homing_error_exit(axis, STAT_HOMING_ERROR_HOMING_INPUT_MISCONFIGURED)); } - if (fp_ZERO(cm.a[axis].search_velocity)) { + if (fp_ZERO(cm->a[axis].search_velocity)) { return (_homing_error_exit(axis, STAT_HOMING_ERROR_ZERO_SEARCH_VELOCITY)); } - if (fp_ZERO(cm.a[axis].latch_velocity)) { + if (fp_ZERO(cm->a[axis].latch_velocity)) { return (_homing_error_exit(axis, STAT_HOMING_ERROR_ZERO_LATCH_VELOCITY)); } // calculate and test travel distance - float travel_distance = std::abs(cm.a[axis].travel_max - cm.a[axis].travel_min) + cm.a[axis].latch_backoff; + float travel_distance = std::abs(cm->a[axis].travel_max - cm->a[axis].travel_min) + cm->a[axis].latch_backoff; if (fp_ZERO(travel_distance)) { return (_homing_error_exit(axis, STAT_HOMING_ERROR_TRAVEL_MIN_MAX_IDENTICAL)); } // Nothing to do about direction now that direction is explicit // However, here's a good place to stash the homing_switch: - hm.homing_input = cm.a[axis].homing_input; + hm.homing_input = cm->a[axis].homing_input; din_handlers[INPUT_ACTION_INTERNAL].registerHandler(&_homing_handler); - hm.axis = axis; // persist the axis - hm.search_velocity = std::abs(cm.a[axis].search_velocity); // search velocity is always positive - hm.latch_velocity = std::abs(cm.a[axis].latch_velocity); // latch velocity is always positive + hm.axis = axis; // persist the axis + hm.search_velocity = std::abs(cm->a[axis].search_velocity); // search velocity is always positive + hm.latch_velocity = std::abs(cm->a[axis].latch_velocity); // latch velocity is always positive - bool homing_to_max = cm.a[axis].homing_dir; + bool homing_to_max = cm->a[axis].homing_dir; // setup parameters for positive or negative travel (homing to the max or min switch) if (homing_to_max) { - hm.search_travel = travel_distance; // search travels in positive direction - hm.latch_backoff = std::abs(cm.a[axis].latch_backoff); // latch travels in positive direction - hm.zero_backoff = -max(0.0f, cm.a[axis].zero_backoff); // zero backoff is negative direction (or zero) - // will set the maximum position - // (plus any negative backoff) - hm.setpoint = cm.a[axis].travel_max + (max(0.0f, -cm.a[axis].zero_backoff)); + hm.search_travel = travel_distance; // search travels in positive direction + hm.latch_backoff = std::abs(cm->a[axis].latch_backoff); // latch travels in positive direction + hm.zero_backoff = -max(0.0f, cm->a[axis].zero_backoff);// zero backoff is negative direction (or zero) + // will set the maximum position + // (plus any negative backoff) + hm.setpoint = cm->a[axis].travel_max + (std::max(0.0f, -cm->a[axis].zero_backoff)); } else { hm.search_travel = -travel_distance; // search travels in negative direction - hm.latch_backoff = -std::abs(cm.a[axis].latch_backoff); // latch travels in negative direction - hm.zero_backoff = max(0.0f, cm.a[axis].zero_backoff); // zero backoff is positive direction (or zero) + hm.latch_backoff = -std::abs(cm->a[axis].latch_backoff); // latch travels in negative direction + hm.zero_backoff = std::max(0.0f, cm->a[axis].zero_backoff); // zero backoff is positive direction (or zero) // will set the minimum position // (minus any negative backoff) - hm.setpoint = cm.a[axis].travel_min + (max(0.0f, -cm.a[axis].zero_backoff)); + hm.setpoint = cm->a[axis].travel_min + (std::max(0.0f, -cm->a[axis].zero_backoff)); } // if homing is disabled for the axis then skip to the next axis - hm.saved_jerk = cm_get_axis_jerk(axis); // save the max jerk value - return (_set_homing_func(_homing_axis_clear_init)); // perform an initial clear + hm.saved_jerk = cm_get_axis_jerk(axis); // save the max jerk value + return (_set_homing_func(_homing_axis_clear_init)); // perform an initial clear } -// Handle an initial switch closure by backing off the closed switch -// NOTE: clear_init() relies on independent switches per axis (not shared) +/*********************************************************************************** + * _homing_axis_clear_init() - initiate a clear to move off a switch that is thrown at the start + * + * Handle an initial switch closure by backing off the closed switch + * NOTE: clear_init() relies on independent switches per axis (not shared) + */ static stat_t _homing_axis_clear_init(int8_t axis) // first clear move { if (gpio_read_input(hm.homing_input) == INPUT_ACTIVE) { // the switch is closed at startup // determine if the input switch for this axis is shared w/other axes for (uint8_t check_axis = AXIS_X; check_axis < AXES; check_axis++) { - if (axis != check_axis && cm.a[check_axis].homing_input == hm.homing_input) { + if (axis != check_axis && cm->a[check_axis].homing_input == hm.homing_input) { return (_homing_error_exit( axis, STAT_HOMING_ERROR_MUST_CLEAR_SWITCHES_BEFORE_HOMING)); // axis cannot be homed } @@ -311,51 +315,70 @@ static stat_t _homing_axis_clear_init(int8_t axis) // first clear move return (_set_homing_func(_homing_axis_search)); // start the search } +/*********************************************************************************** + * _homing_axis_search() - fast search for switch, closes switch + */ static stat_t _homing_axis_search(int8_t axis) // drive to switch { - cm_set_axis_jerk(axis, cm.a[axis].jerk_high); // use the high-speed jerk for search onward + cm_set_axis_max_jerk(axis, cm->a[axis].jerk_high); // use the high-speed jerk for search onward _homing_axis_move(axis, hm.search_travel, hm.search_velocity); return (_set_homing_func(_homing_axis_clear)); } +/*********************************************************************************** + * _homing_axis_clear() - clear off the switch + */ static stat_t _homing_axis_clear(int8_t axis) // drive away from switch at search speed { _homing_axis_move(axis, -hm.latch_backoff, hm.search_velocity); return (_set_homing_func(_homing_axis_latch)); } +/*********************************************************************************** + * _homing_axis_latch() - slow drive until until switch closes again + */ static stat_t _homing_axis_latch(int8_t axis) // drive to switch at low speed { _homing_axis_move(axis, hm.latch_backoff, hm.latch_velocity); return (_set_homing_func(_homing_axis_setpoint_backoff)); } -static stat_t _homing_axis_setpoint_backoff(int8_t axis) // backoff to zero or max setpoint position +/*********************************************************************************** + * _homing_axis_setpoint_backoff() - backoff to zero or max setpoint position + */ +static stat_t _homing_axis_setpoint_backoff(int8_t axis) // { _homing_axis_move(axis, hm.zero_backoff, hm.search_velocity); return (_set_homing_func(_homing_axis_set_position)); } -static stat_t _homing_axis_set_position(int8_t axis) // set axis zero / max and finish up +/*********************************************************************************** + * _homing_axis_set_position() - set axis zero / max and finish up + */ +static stat_t _homing_axis_set_position(int8_t axis) { if (hm.set_coordinates) { - cm_set_position(axis, hm.setpoint); - cm.homed[axis] = true; + cm_set_position_by_axis(axis, hm.setpoint); + cm->homed[axis] = true; } else { // handle G28.4 cycle - set position to the point of switch closure float contact_position[AXES]; kn_forward_kinematics(en_get_encoder_snapshot_vector(), contact_position); _homing_axis_move(axis, contact_position[AXIS_Z], hm.search_velocity); } - cm_set_axis_jerk(axis, hm.saved_jerk); // restore the max jerk value + cm_set_axis_max_jerk(axis, hm.saved_jerk); // restore the max jerk value din_handlers[INPUT_ACTION_INTERNAL].deregisterHandler(&_homing_handler); // end homing mode return (_set_homing_func(_homing_axis_start)); } +/*********************************************************************************** + * _homing_axis_move() - helper that actually executes the above moves + */ + static stat_t _homing_axis_move(int8_t axis, float target, float velocity) { - float vect[] = {0, 0, 0, 0, 0, 0}; - bool flags[] = {false, false, false, false, false, false}; + float vect[] = INIT_AXES_ZEROES; + bool flags[] = INIT_AXES_ZEROES; hm.waiting_for_motion_end = true; @@ -363,7 +386,7 @@ static stat_t _homing_axis_move(int8_t axis, float target, float velocity) { flags[axis] = true; cm_set_feed_rate(velocity); - stat_t status = cm_straight_feed(vect, flags); + stat_t status = cm_straight_feed(vect, flags, PROFILE_FAST); if (status != STAT_OK) { rpt_exception(status, "Homing move failed. Check min/max settings"); return (_homing_error_exit(axis, STAT_HOMING_CYCLE_FAILED)); @@ -378,7 +401,7 @@ static stat_t _homing_axis_move(int8_t axis, float target, float velocity) { static void _homing_axis_move_callback(float* vect, bool* flag) { hm.waiting_for_motion_end = false; } -/* +/*********************************************************************************** * _homing_error_exit() * * Generate an error message. Since the error exit returns via the homing callback @@ -400,7 +423,7 @@ static stat_t _homing_error_exit(int8_t axis, stat_t status) { return (STAT_HOMING_CYCLE_FAILED); // homing state remains HOMING_NOT_HOMED } -/* +/*********************************************************************************** * _homing_finalize_exit() - helper to finalize homing */ @@ -416,7 +439,7 @@ static stat_t _homing_finalize_exit(int8_t axis) // third part of return to hom return (STAT_OK); } -/* +/*********************************************************************************** * _get_next_axis() - return next axis in sequence based on axis in arg * * Accepts "axis" arg as the current axis; or -1 to retrieve the first axis diff --git a/g2core/cycle_jogging.cpp b/g2core/cycle_jogging.cpp index a12263e3..aadaedeb 100644 --- a/g2core/cycle_jogging.cpp +++ b/g2core/cycle_jogging.cpp @@ -34,6 +34,8 @@ #include "util.h" #include "xio.h" +#define JOGGING_START_VELOCITY ((float)10.0) + /**** Jogging singleton structure ****/ struct jmJoggingSingleton { // persistent jogging runtime variables @@ -84,12 +86,12 @@ static stat_t _jogging_finalize_exit(int8_t axis); stat_t cm_jogging_cycle_start(uint8_t axis) { // save relevant non-axis parameters from Gcode model - jog.saved_units_mode = cm_get_units_mode(ACTIVE_MODEL); // cm.gm.units_mode; - jog.saved_coord_system = cm_get_coord_system(ACTIVE_MODEL); // cm.gm.coord_system; - jog.saved_distance_mode = cm_get_distance_mode(ACTIVE_MODEL); // cm.gm.distance_mode; + jog.saved_units_mode = cm_get_units_mode(ACTIVE_MODEL); // cm->gm.units_mode; + jog.saved_coord_system = cm_get_coord_system(ACTIVE_MODEL); // cm->gm.coord_system; + jog.saved_distance_mode = cm_get_distance_mode(ACTIVE_MODEL); // cm->gm.distance_mode; jog.saved_feed_rate_mode = cm_get_feed_rate_mode(ACTIVE_MODEL); - jog.saved_feed_rate = (ACTIVE_MODEL)->feed_rate; // cm.gm.feed_rate; - jog.saved_jerk = cm.a[axis].jerk_max; + jog.saved_feed_rate = (ACTIVE_MODEL)->feed_rate; // cm->gm.feed_rate; + jog.saved_jerk = cm->a[axis].jerk_max; // set working values cm_set_units_mode(MILLIMETERS); @@ -98,21 +100,20 @@ stat_t cm_jogging_cycle_start(uint8_t axis) { cm_set_feed_rate_mode(UNITS_PER_MINUTE_MODE); jog.velocity_start = JOGGING_START_VELOCITY; // see canonical_machine.h for #define - jog.velocity_max = cm.a[axis].velocity_max; + jog.velocity_max = cm->a[axis].velocity_max; jog.start_pos = cm_get_absolute_position(RUNTIME, axis); - jog.dest_pos = cm_get_jogging_dest(); - jog.step = 0; + jog.dest_pos = cm_get_jogging_dest(); + jog.step = 0; jog.axis = axis; jog.func = _jogging_axis_start; // bind initial processing function - cm.machine_state = MACHINE_CYCLE; - cm.cycle_state = CYCLE_JOG; + cm->machine_state = MACHINE_CYCLE; + cm->cycle_type = CYCLE_JOG; return (STAT_OK); } - /* Jogging axis moves - these execute in sequence for each axis * cm_jogging_cycle_callback() - main loop callback for running the jogging cycle * _set_jogging_func() - a convenience for setting the next dispatch vector and exiting @@ -123,14 +124,14 @@ stat_t cm_jogging_cycle_start(uint8_t axis) { */ stat_t cm_jogging_cycle_callback(void) { - if (cm.cycle_state != CYCLE_JOG) { + if (cm->cycle_type != CYCLE_JOG) { return (STAT_NOOP); // exit if not in a jogging cycle } if (jog.func == _jogging_finalize_exit && cm_get_runtime_busy() == true) { return (STAT_EAGAIN); // sync to planner move ends } // if (jog.func == _jogging_axis_ramp_jog && mp_get_buffers_available() < PLANNER_BUFFER_HEADROOM) { - if (jog.func == _jogging_axis_ramp_jog && mp_planner_is_full()) { + if (jog.func == _jogging_axis_ramp_jog && mp_planner_is_full(mp)) { // +++++ return (STAT_EAGAIN); // prevent flooding the queue with jog moves } return (jog.func(jog.axis)); // execute the current jogging move @@ -142,7 +143,7 @@ static stat_t _set_jogging_func(stat_t (*func)(int8_t axis)) { } static stat_t _jogging_axis_start(int8_t axis) { - // cm_end_hold(); // ends hold if one is in effect +// cm_end_hold(); // ends hold if one is in effect return (_set_jogging_func(_jogging_axis_ramp_jog)); } @@ -176,13 +177,13 @@ static stat_t _jogging_axis_ramp_jog(int8_t axis) // run the jog ramp } static stat_t _jogging_axis_move(int8_t axis, float target, float velocity) { - float vect[] = {0, 0, 0, 0, 0, 0}; - bool flags[] = {false, false, false, false, false, false}; + float vect[] = INIT_AXES_ZEROES; + bool flags[] = INIT_AXES_FALSE; vect[axis] = target; flags[axis] = true; cm_set_feed_rate(velocity); - ritorno(cm_straight_feed(vect, flags)); + ritorno(cm_straight_feed(vect, flags, PROFILE_FAST)); return (STAT_EAGAIN); } diff --git a/g2core/cycle_probing.cpp b/g2core/cycle_probing.cpp index b19af100..4ddfed93 100644 --- a/g2core/cycle_probing.cpp +++ b/g2core/cycle_probing.cpp @@ -2,7 +2,7 @@ * cycle_probing.c - probing cycle extension to canonical_machine.c * This file is part of the g2core project * - * Copyright (c) 2010 - 2017 Alden S Hart, Jr., Sarah Tappon, Tom Cauchois, Robert Giseburt + * Copyright (c) 2010 - 2019 Alden S Hart, Jr., Sarah Tappon, Tom Cauchois, Robert Giseburt * With contributions from Other Machine Company. * * This file ("the software") is free software: you can redistribute it and/or modify @@ -39,7 +39,8 @@ #include "util.h" #include "xio.h" -/**** Probe singleton structure ****/ + +/**** Local stuff ****/ #define MINIMUM_PROBE_TRAVEL 0.254 // mm of travel below which the probe will err out @@ -53,7 +54,7 @@ struct pbProbingSingleton { // persistent probing runtime variables int8_t probe_input; // digital input to read bool trip_sense; // true if contact CLOSURE trips probe (true for G38.2 and G38.3) bool alarm_flag; // true if failure triggers alarm (true for G38.2 and G38.4) - bool wait_for_motion_end; // flag to know when the motion has ended + bool waiting_for_motion_complete; // true if waiting for a motion to complete stat_t (*func)(); // binding for callback function state machine // saved gcode model state @@ -82,12 +83,12 @@ void _store_probe_position(); stat_t cm_set_probe(nvObj_t *nv) { - if (!fp_ZERO(nv->value)) { - nv->valuetype = TYPE_BOOL; - nv->value = true; + if (!fp_ZERO(nv->value_int)) { + nv->valuetype = TYPE_BOOLEAN; + nv->value_int = true; _prepare_for_probe(); - cm.probe_state[0] = PROBE_SUCCEEDED; + cm->probe_state[0] = PROBE_SUCCEEDED; _store_probe_position(); } return (STAT_OK); @@ -101,11 +102,11 @@ stat_t cm_set_probe(nvObj_t *nv) void _prepare_for_probe() { // if the previous probe succeeded, roll probes to the next position - if (cm.probe_state[0] == PROBE_SUCCEEDED) { + if (cm->probe_state[0] == PROBE_SUCCEEDED) { for (uint8_t n = PROBES_STORED - 1; n > 0; n--) { - cm.probe_state[n] = cm.probe_state[n - 1]; + cm->probe_state[n] = cm->probe_state[n - 1]; for (uint8_t axis = 0; axis < AXES; axis++) { - cm.probe_results[n][axis] = cm.probe_results[n - 1][axis]; + cm->probe_results[n][axis] = cm->probe_results[n - 1][axis]; } } } @@ -113,14 +114,14 @@ void _prepare_for_probe() { void _store_probe_position() { for (uint8_t axis = 0; axis < AXES; axis++) { - cm.probe_results[0][axis] = cm_get_absolute_position(ACTIVE_MODEL, axis); + cm->probe_results[0][axis] = cm_get_absolute_position(ACTIVE_MODEL, axis); } } // helper static void _motion_end_callback(float* vect, bool* flag) { - pb.wait_for_motion_end = false; + pb.waiting_for_motion_complete = false; } /* @@ -129,11 +130,11 @@ static void _motion_end_callback(float* vect, bool* flag) */ gpioDigitalInputHandler _probing_handler { [&](const bool state, const inputEdgeFlag edge, const uint8_t triggering_pin_number) { - if (cm.cycle_state != CYCLE_PROBE) { return false; } + if (cm->cycle_type != CYCLE_PROBE) { return false; } if (triggering_pin_number != pb.probe_input) { return false; } en_take_encoder_snapshot(); - cm_start_hold(); + cm_request_feedhold(FEEDHOLD_TYPE_SKIP, FEEDHOLD_EXIT_STOP); return false; // allow others to see this notice }, @@ -167,9 +168,9 @@ gpioDigitalInputHandler _probing_handler { * in-progress) occupies one of those positions, which is the one reported by the * "prb" JSON. * - * Internally we store the active/most recent probe in cm.probe_results[0] and - * cm.probe_state[0]. Before we start a new probe, if cm.probe_state[0] == - * PROBE_SUCCEEDED, then we roll 0 to 1, and 1 to 2, up to PROBES_STORED-1. + * Internally the active/most recent probe is stored in cm->probe_results[0] and + * cm->probe_state[0]. Before a new probe is started, if cm->probe_state[0] == + * PROBE_SUCCEEDED, then 0 rolls to 1, and 1 to 2, up to PROBES_STORED-1. * The oldest probe is "lost." * * Alarms and exceptions: It is *not* necessarily an error condition for the @@ -197,19 +198,19 @@ gpioDigitalInputHandler _probing_handler { uint8_t cm_straight_probe(float target[], bool flags[], bool trip_sense, bool alarm_flag) { // error if zero feed rate - if (fp_ZERO(cm.gm.feed_rate)) { - return(cm_alarm(STAT_GCODE_FEEDRATE_NOT_SPECIFIED, "Feedrate is zero")); + if (fp_ZERO(cm->gm.feed_rate)) { + return(cm_alarm(STAT_FEEDRATE_NOT_SPECIFIED, "Feedrate is zero")); } // error if no axes specified if (!(flags[AXIS_X] | flags[AXIS_Y] | flags[AXIS_Z] | flags[AXIS_A] | flags[AXIS_B] | flags[AXIS_C])) { - return(cm_alarm(STAT_GCODE_AXIS_IS_MISSING, "Axis is missing")); + return(cm_alarm(STAT_AXIS_IS_MISSING, "Axis is missing")); } // initialize the probe input; error if no probe input specified - if ((pb.probe_input = cm.probe_input) == -1) { - return(cm_alarm(STAT_NO_PROBE_INPUT_CONFIGURED, "No probe input")); + if ((pb.probe_input = cm->probe_input) == -1) { + return(cm_alarm(STAT_NO_PROBE_INPUT_CONFIGURED, "Probe input not configured")); } // setup @@ -218,17 +219,17 @@ uint8_t cm_straight_probe(float target[], bool flags[], bool trip_sense, bool al pb.func = _probing_start; // bind probing start function cm_set_model_target(target, flags); // convert target to canonical form taking all offsets into account - copy_vector(pb.target, cm.gm.target); // cm_set_model_target() sets target in gm, move it to pb + copy_vector(pb.target, cm->gm.target); // cm_set_model_target() sets target in gm, move it to pb copy_vector(pb.flags, flags); // set axes involved in the move _prepare_for_probe(); // clear the old probe results - clear_vector(cm.probe_results[0]); // NOTE: relying on cm.probe_results will not detect a probe to 0,0,0. + clear_vector(cm->probe_results[0]); // NOTE: relying on cm->probe_results will not detect a probe to 0,0,0. // queue a function to let us know when we can start probing - cm.probe_state[0] = PROBE_WAITING; // wait until planner queue empties before starting movement - pb.wait_for_motion_end = true; + cm->probe_state[0] = PROBE_WAITING; // wait until planner queue empties before starting movement + pb.waiting_for_motion_complete = true; mp_queue_command(_motion_end_callback, nullptr, nullptr); // note: these args are ignored return (STAT_OK); } @@ -243,15 +244,33 @@ uint8_t cm_straight_probe(float target[], bool flags[], bool trip_sense, bool al uint8_t cm_probing_cycle_callback(void) { - if ((cm.cycle_state != CYCLE_PROBE) && (cm.probe_state[0] != PROBE_WAITING)) { - return (STAT_NOOP); // exit if not in a probing cycle + if ((cm->cycle_type != CYCLE_PROBE) && (cm->probe_state[0] != PROBE_WAITING)) { + return (STAT_NOOP); // exit if not in a probing cycle } - if (pb.wait_for_motion_end) { // sync to planner move ends (using callback) + if (pb.waiting_for_motion_complete) { // sync to planner move ends (using callback) return (STAT_EAGAIN); } - return (pb.func()); // execute the current probing move + return (pb.func()); // execute the current probing move } +/*********************************************************************************** + * _probe_move() - function to execute probing moves + * _motion_end_callback() - callback completes when motion has stopped + * + * target[] must be provided in machine canonical coordinates (absolute, mm) + * cm_set_absolute_override() also zeros work offsets, which are restored on exit. + */ + +static stat_t _probe_move(const float target[], const bool flags[]) +{ + cm_set_absolute_override(MODEL, ABSOLUTE_OVERRIDE_ON_DISPLAY_WITH_OFFSETS); + pb.waiting_for_motion_complete = true; // set this BEFORE the motion starts + cm_straight_feed(target, flags, PROFILE_FAST); // NB: feed rate was set earlier, so it's OK + mp_queue_command(_motion_end_callback, nullptr, nullptr); // the last two arguments are ignored anyway + return (STAT_EAGAIN); +} + + /*********************************************************************************** * _probing_start() - start the probe or skip it if contact is already active */ @@ -264,9 +283,9 @@ static uint8_t _probing_start() // runtime (specifically the digital input modes). Side effects would include // limit switches initiating probe actions instead of just killing movement - cm.probe_state[0] = PROBE_FAILED; - cm.machine_state = MACHINE_CYCLE; - cm.cycle_state = CYCLE_PROBE; + cm->probe_state[0] = PROBE_FAILED; + cm->machine_state = MACHINE_CYCLE; + cm->cycle_type = CYCLE_PROBE; // save relevant non-axis parameters from Gcode model pb.saved_distance_mode = (cmDistanceMode)cm_get_distance_mode(ACTIVE_MODEL); @@ -281,11 +300,11 @@ static uint8_t _probing_start() // Save the current jerk settings & change to the high-speed jerk settings for (uint8_t axis = 0; axis < AXES; axis++) { pb.saved_jerk[axis] = cm_get_axis_jerk(axis); // save the max jerk value - cm_set_axis_jerk(axis, cm.a[axis].jerk_high); // use the high-speed jerk for probe + cm_set_axis_max_jerk(axis, cm->a[axis].jerk_high); // use the high-speed jerk for probe } // Error if the probe target is too close to the current position - if (get_axis_vector_length(cm.gmx.position, pb.target) < MINIMUM_PROBE_TRAVEL) { + if (get_axis_vector_length(cm->gmx.position, pb.target) < MINIMUM_PROBE_TRAVEL) { return(_probing_exception_exit(STAT_PROBE_TRAVEL_TOO_SMALL)); } @@ -316,33 +335,17 @@ static stat_t _probing_backoff() // was taken by input interrupt at the time of closure. if (pb.trip_sense == gpio_read_input(pb.probe_input)) { // exclusive or for booleans - cm.probe_state[0] = PROBE_SUCCEEDED; + cm->probe_state[0] = PROBE_SUCCEEDED; float contact_position[AXES]; kn_forward_kinematics(en_get_encoder_snapshot_vector(), contact_position); _probe_move(contact_position, pb.flags); // NB: feed rate is the same as the probe move } else { - cm.probe_state[0] = PROBE_FAILED; + cm->probe_state[0] = PROBE_FAILED; } pb.func = _probing_finish; return (STAT_EAGAIN); } -/*********************************************************************************** - * _probe_move() - function to execute probing moves - * - * target[] must be provided in machine canonical coordinates (absolute, mm) - * cm_set_absolute_override() also zeros work offsets, which are restored on exit. - */ - -static stat_t _probe_move(const float target[], const bool flags[]) -{ - cm_set_absolute_override(MODEL, ABSOLUTE_OVERRIDE_ON); - pb.wait_for_motion_end = true; // set this BEFORE the motion starts - cm_straight_feed(target, flags); - mp_queue_command(_motion_end_callback, nullptr, nullptr); // the last two arguments are ignored anyway - return (STAT_EAGAIN); -} - /*********************************************************************************** * _probe_restore_settings() - helper for both exits * _probing_exception_exit() - exit for probes that hit an exception @@ -354,7 +357,7 @@ static void _probe_restore_settings() din_handlers[INPUT_ACTION_INTERNAL].deregisterHandler(&_probing_handler); for (uint8_t axis = 0; axis < AXES; axis++) { // restore axis jerks - cm.a[axis].jerk_max = pb.saved_jerk[axis]; + cm->a[axis].jerk_max = pb.saved_jerk[axis]; } cm_set_absolute_override(MODEL, ABSOLUTE_OVERRIDE_OFF); // release abs override and restore work offsets cm_set_distance_mode(pb.saved_distance_mode); @@ -379,7 +382,7 @@ static stat_t _probing_finish() _store_probe_position(); // handle failed probes - successful probes already set the flag - if (cm.probe_state[0] == PROBE_FAILED) { + if (cm->probe_state[0] == PROBE_FAILED) { if (pb.alarm_flag) { cm_alarm(STAT_PROBE_CYCLE_FAILED, "probing failed"); } @@ -394,28 +397,28 @@ static stat_t _probing_finish() static void _send_probe_report() { - if (cm.probe_report_enable) { + if (cm->probe_report_enable) { // If probe was successful the 'e' word == 1, otherwise e == 0 to signal an error char buf[32]; char* bufp = buf; - bufp += sprintf(bufp, "{\"prb\":{\"e\":%i, \"", (int)cm.probe_state[0]); + bufp += sprintf(bufp, "{\"prb\":{\"e\":%i, \"", (int)cm->probe_state[0]); if (pb.flags[AXIS_X]) { - sprintf(bufp, "x\":%0.3f}}\n", cm.probe_results[0][AXIS_X]); + sprintf(bufp, "x\":%0.3f}}\n", cm->probe_results[0][AXIS_X]); } if (pb.flags[AXIS_Y]) { - sprintf(bufp, "y\":%0.3f}}\n", cm.probe_results[0][AXIS_Y]); + sprintf(bufp, "y\":%0.3f}}\n", cm->probe_results[0][AXIS_Y]); } if (pb.flags[AXIS_Z]) { - sprintf(bufp, "z\":%0.3f}}\n", cm.probe_results[0][AXIS_Z]); + sprintf(bufp, "z\":%0.3f}}\n", cm->probe_results[0][AXIS_Z]); } if (pb.flags[AXIS_A]) { - sprintf(bufp, "a\":%0.3f}}\n", cm.probe_results[0][AXIS_A]); + sprintf(bufp, "a\":%0.3f}}\n", cm->probe_results[0][AXIS_A]); } if (pb.flags[AXIS_B]) { - sprintf(bufp, "b\":%0.3f}}\n", cm.probe_results[0][AXIS_B]); + sprintf(bufp, "b\":%0.3f}}\n", cm->probe_results[0][AXIS_B]); } if (pb.flags[AXIS_C]) { - sprintf(bufp, "c\":%0.3f}}\n", cm.probe_results[0][AXIS_C]); + sprintf(bufp, "c\":%0.3f}}\n", cm->probe_results[0][AXIS_C]); } xio_writeline(buf); } @@ -428,13 +431,13 @@ static void _send_probe_report() { stat_t cm_get_prbr(nvObj_t *nv) { - nv->value = (float)cm.probe_report_enable; - nv->valuetype = TYPE_INT; + nv->value_int = cm->probe_report_enable; + nv->valuetype = TYPE_INTEGER; // ++++ should probably be type boolean return (STAT_OK); } stat_t cm_set_prbr(nvObj_t *nv) { - cm.probe_report_enable = fp_NOT_ZERO(nv->value); + cm->probe_report_enable = nv->value_int; return (STAT_OK); } diff --git a/g2core/device/i2c_as5601/i2c_as5601.h b/g2core/device/i2c_as5601/i2c_as5601.h index 362155ac..caf25b61 100644 --- a/g2core/device/i2c_as5601/i2c_as5601.h +++ b/g2core/device/i2c_as5601/i2c_as5601.h @@ -37,21 +37,6 @@ // #include "MotateBuffer.h" #include "MotateUtilities.h" // for to/fromLittle/BigEndian -class ExternalEncoder { - public: - using callback_t = std::function; - enum ReturnFormat { ReturnDegrees, ReturnRadians, ReturnFraction }; - - virtual void setCallback(std::function &&handler); - virtual void setCallback(std::function &handler); - - virtual void requestAngleDegrees(); - virtual void requestAngleRadians(); - virtual void requestAngleFraction(); - - virtual float getQuadratureFraction(); -}; - // Complete class for I2C_AS5601 drivers. template class I2C_AS5601 final : public ExternalEncoder, public gpioDigitalInputHandler { diff --git a/g2core/device/step_dir_driver/step_dir_driver.h b/g2core/device/step_dir_driver/step_dir_driver.h index 3d4ba3b3..813de444 100644 --- a/g2core/device/step_dir_driver/step_dir_driver.h +++ b/g2core/device/step_dir_driver/step_dir_driver.h @@ -2,8 +2,8 @@ * step_dir_driver.cpp - control over a Step/Direction/Enable stepper motor driver * This file is part of G2 project * - * Copyright (c) 2016 Alden S. Hart, Jr. - * Copyright (c) 2016 Robert Giseburt + * Copyright (c) 2016 - 2018 Alden S. Hart, Jr. + * Copyright (c) 2016 - 2018 Robert Giseburt * * This file ("the software") is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License, version 2 as published by the @@ -25,8 +25,8 @@ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -#ifndef STEPP_DIR_DRIVER_H_ONCE -#define STEPP_DIR_DRIVER_H_ONCE +#ifndef STEP_DIR_DRIVER_H_ONCE +#define STEP_DIR_DRIVER_H_ONCE #include "MotatePins.h" #include "MotateTimers.h" @@ -37,6 +37,7 @@ using Motate::pin_number; using Motate::OutputPin; using Motate::PWMOutputPin; using Motate::kStartHigh; +using Motate::kStartLow; using Motate::kNormal; using Motate::Timeout; @@ -60,14 +61,24 @@ struct StepDirStepper final : Stepper { OutputPin _step; uint8_t _step_downcount; OutputPin _dir; - OutputPin _enable{kStartHigh}; + OutputPin _enable; OutputPin _ms0; OutputPin _ms1; OutputPin _ms2; PWMOutputPin _vref; + ioMode _step_polarity; // IO_ACTIVE_LOW or IO_ACTIVE_HIGH + ioMode _enable_polarity; // IO_ACTIVE_LOW or IO_ACTIVE_HIGH + // sets default pwm freq for all motor vrefs (commented line below also sets HiZ) - StepDirStepper(const uint32_t frequency = 250000) : Stepper{}, _vref{kNormal, frequency} {}; + StepDirStepper(ioMode step_polarity, ioMode enable_polarity, const uint32_t frequency = 250000) : + Stepper{}, + _step{step_polarity==IO_ACTIVE_LOW?kStartHigh:kStartLow}, + _enable{enable_polarity==IO_ACTIVE_LOW?kStartHigh:kStartLow}, + _vref{kNormal, frequency}, + _step_polarity{step_polarity}, + _enable_polarity{enable_polarity} + {}; /* Optional override of init */ @@ -120,19 +131,37 @@ struct StepDirStepper final : Stepper { void _enableImpl() override { if (!_enable.isNull()) { - _enable.clear(); + if (_enable_polarity == IO_ACTIVE_HIGH) { + _enable.set(); + } else { + _enable.clear(); + } } }; void _disableImpl() override { if (!_enable.isNull()) { - _enable.set(); + if (_enable_polarity == IO_ACTIVE_HIGH) { + _enable.clear(); + } else { + _enable.set(); + } } }; - void stepStart() override { _step.set(); }; + void stepStart() override { + if (_step_polarity == IO_ACTIVE_LOW) + _step.clear(); + else + _step.set(); + }; - void stepEnd() override { _step.clear(); }; + void stepEnd() override { + if (_step_polarity == IO_ACTIVE_LOW) + _step.set(); + else + _step.clear(); + }; void setDirection(uint8_t new_direction) override { if (!_dir.isNull()) { @@ -149,6 +178,30 @@ struct StepDirStepper final : Stepper { _vref = new_pl * POWER_LEVEL_SCALE_FACTOR; } }; + + ioMode getStepPolarity() const override + { + return _step_polarity; + }; + + void setStepPolarity(ioMode new_sp) override + { + _step_polarity = new_sp; + stepEnd(); + }; + + ioMode getEnablePolarity() const override + { + return _enable_polarity; + }; + + void setEnablePolarity(ioMode new_mp) override + { + _enable_polarity = new_mp; + // this is a misnomer, but handles the logic we need for asserting the newly adjusted enable line correctly + motionStopped(); + }; + }; -#endif // STEPP_DIR_DRIVER_H_ONCE +#endif // STEP_DIR_DRIVER_H_ONCE diff --git a/g2core/device/trinamic/tmc2130.h b/g2core/device/trinamic/tmc2130.h index 3dd70100..bf918256 100644 --- a/g2core/device/trinamic/tmc2130.h +++ b/g2core/device/trinamic/tmc2130.h @@ -708,21 +708,21 @@ struct Trinamic2130 final : Stepper { // NV interface helpers stat_t get_ts(nvObj_t *nv) { - nv->value = TSTEP.value; - nv->valuetype = TYPE_INT; + nv->value_int = TSTEP.value; + nv->valuetype = TYPE_INTEGER; return STAT_OK; }; static stat_t get_ts_fn(nvObj_t *nv) { return get_fn<&type::get_ts>(nv); }; // no set stat_t get_pth(nvObj_t *nv) { - nv->value = TPWMTHRS.value; - nv->valuetype = TYPE_INT; + nv->value_int = TPWMTHRS.value; + nv->valuetype = TYPE_INTEGER; return STAT_OK; }; static stat_t get_pth_fn(nvObj_t *nv) { return get_fn<&type::get_pth>(nv); }; stat_t set_pth(nvObj_t *nv) { - int32_t v = nv->value; + int32_t v = nv->value_int; if (v < 0) { nv->valuetype = TYPE_NULL; return (STAT_INPUT_LESS_THAN_MIN_VALUE); @@ -738,13 +738,13 @@ struct Trinamic2130 final : Stepper { static stat_t set_pth_fn(nvObj_t *nv) { return get_fn<&type::set_pth>(nv); }; stat_t get_cth(nvObj_t *nv) { - nv->value = TCOOLTHRS.value; - nv->valuetype = TYPE_INT; + nv->value_int = TCOOLTHRS.value; + nv->valuetype = TYPE_INTEGER; return STAT_OK; }; static stat_t get_cth_fn(nvObj_t *nv) { return get_fn<&type::get_cth>(nv); }; stat_t set_cth(nvObj_t *nv) { - int32_t v = nv->value; + int32_t v = nv->value_int; if (v < 0) { nv->valuetype = TYPE_NULL; return (STAT_INPUT_LESS_THAN_MIN_VALUE); @@ -760,13 +760,13 @@ struct Trinamic2130 final : Stepper { static stat_t set_cth_fn(nvObj_t *nv) { return get_fn<&type::set_cth>(nv); }; stat_t get_hth(nvObj_t *nv) { - nv->value = THIGH.value; - nv->valuetype = TYPE_INT; + nv->value_int = THIGH.value; + nv->valuetype = TYPE_INTEGER; return STAT_OK; }; static stat_t get_hth_fn(nvObj_t *nv) { return get_fn<&type::get_hth>(nv); }; stat_t set_hth(nvObj_t *nv) { - int32_t v = nv->value; + int32_t v = nv->value_int; if (v < 0) { nv->valuetype = TYPE_NULL; return (STAT_INPUT_LESS_THAN_MIN_VALUE); @@ -783,13 +783,13 @@ struct Trinamic2130 final : Stepper { stat_t get_sgt(nvObj_t *nv) { int32_t v = COOLCONF.sgt; - nv->value = (int32_t)(63&v)-(v&64); - nv->valuetype = TYPE_INT; + nv->value_int = (int32_t)(63&v)-(v&64); + nv->valuetype = TYPE_INTEGER; return STAT_OK; }; static stat_t get_sgt_fn(nvObj_t *nv) { return get_fn<&type::get_sgt>(nv); }; stat_t set_sgt(nvObj_t *nv) { - int32_t v = nv->value; + int32_t v = nv->value_int; if (v < -64) { nv->valuetype = TYPE_NULL; return (STAT_INPUT_LESS_THAN_MIN_VALUE); @@ -805,24 +805,24 @@ struct Trinamic2130 final : Stepper { static stat_t set_sgt_fn(nvObj_t *nv) { return get_fn<&type::set_sgt>(nv); }; stat_t get_csa(nvObj_t *nv) { - nv->value = DRV_STATUS.CS_ACTUAL; - nv->valuetype = TYPE_INT; + nv->value_int = DRV_STATUS.CS_ACTUAL; + nv->valuetype = TYPE_INTEGER; return STAT_OK; }; static stat_t get_csa_fn(nvObj_t *nv) { return get_fn<&type::get_csa>(nv); }; // no set stat_t get_sgr(nvObj_t *nv) { - nv->value = DRV_STATUS.SG_RESULT; - nv->valuetype = TYPE_INT; + nv->value_int = DRV_STATUS.SG_RESULT; + nv->valuetype = TYPE_INTEGER; return STAT_OK; }; static stat_t get_sgr_fn(nvObj_t *nv) { return get_fn<&type::get_sgr>(nv); }; // no set stat_t get_sgs(nvObj_t *nv) { - nv->value = DRV_STATUS.stallGuard; - nv->valuetype = TYPE_BOOL; + nv->value_int = DRV_STATUS.stallGuard; + nv->valuetype = TYPE_BOOLEAN; return STAT_OK; }; static stat_t get_sgs_fn(nvObj_t *nv) { return get_fn<&type::get_sgs>(nv); }; @@ -830,13 +830,13 @@ struct Trinamic2130 final : Stepper { stat_t get_tbl(nvObj_t *nv) { - nv->value = CHOPCONF.TBL; - nv->valuetype = TYPE_INT; + nv->value_int = CHOPCONF.TBL; + nv->valuetype = TYPE_INTEGER; return STAT_OK; }; static stat_t get_tbl_fn(nvObj_t *nv) { return get_fn<&type::get_tbl>(nv); }; stat_t set_tbl(nvObj_t *nv) { - int32_t v = nv->value; + int32_t v = nv->value_int; if (v < 0) { nv->valuetype = TYPE_NULL; return (STAT_INPUT_LESS_THAN_MIN_VALUE); @@ -852,13 +852,13 @@ struct Trinamic2130 final : Stepper { static stat_t set_tbl_fn(nvObj_t *nv) { return get_fn<&type::set_tbl>(nv); }; stat_t get_pgrd(nvObj_t *nv) { - nv->value = PWMCONF.PWM_GRAD; - nv->valuetype = TYPE_INT; + nv->value_int = PWMCONF.PWM_GRAD; + nv->valuetype = TYPE_INTEGER; return STAT_OK; }; static stat_t get_pgrd_fn(nvObj_t *nv) { return get_fn<&type::get_pgrd>(nv); }; stat_t set_pgrd(nvObj_t *nv) { - int32_t v = nv->value; + int32_t v = nv->value_int; if (v < 0) { nv->valuetype = TYPE_NULL; return (STAT_INPUT_LESS_THAN_MIN_VALUE); @@ -874,13 +874,13 @@ struct Trinamic2130 final : Stepper { static stat_t set_pgrd_fn(nvObj_t *nv) { return get_fn<&type::set_pgrd>(nv); }; stat_t get_pamp(nvObj_t *nv) { - nv->value = PWMCONF.PWM_AMPL; - nv->valuetype = TYPE_INT; + nv->value_int = PWMCONF.PWM_AMPL; + nv->valuetype = TYPE_INTEGER; return STAT_OK; }; static stat_t get_pamp_fn(nvObj_t *nv) { return get_fn<&type::get_pamp>(nv); }; stat_t set_pamp(nvObj_t *nv) { - int32_t v = nv->value; + int32_t v = nv->value_int; if (v < 0) { nv->valuetype = TYPE_NULL; return (STAT_INPUT_LESS_THAN_MIN_VALUE); @@ -896,13 +896,13 @@ struct Trinamic2130 final : Stepper { static stat_t set_pamp_fn(nvObj_t *nv) { return get_fn<&type::set_pamp>(nv); }; stat_t get_hend(nvObj_t *nv) { - nv->value = CHOPCONF.HEND_OFFSET; - nv->valuetype = TYPE_INT; + nv->value_int = CHOPCONF.HEND_OFFSET; + nv->valuetype = TYPE_INTEGER; return STAT_OK; }; static stat_t get_hend_fn(nvObj_t *nv) { return get_fn<&type::get_hend>(nv); }; stat_t set_hend(nvObj_t *nv) { - int32_t v = nv->value; + int32_t v = nv->value_int; if (v < 0) { nv->valuetype = TYPE_NULL; return (STAT_INPUT_LESS_THAN_MIN_VALUE); @@ -918,13 +918,13 @@ struct Trinamic2130 final : Stepper { static stat_t set_hend_fn(nvObj_t *nv) { return get_fn<&type::set_hend>(nv); }; stat_t get_hsrt(nvObj_t *nv) { - nv->value = CHOPCONF.HSTRT_TFD012; - nv->valuetype = TYPE_INT; + nv->value_int = CHOPCONF.HSTRT_TFD012; + nv->valuetype = TYPE_INTEGER; return STAT_OK; }; static stat_t get_hsrt_fn(nvObj_t *nv) { return get_fn<&type::get_hsrt>(nv); }; stat_t set_hsrt(nvObj_t *nv) { - int32_t v = nv->value; + int32_t v = nv->value_int; if (v < 0) { nv->valuetype = TYPE_NULL; return (STAT_INPUT_LESS_THAN_MIN_VALUE); @@ -940,13 +940,13 @@ struct Trinamic2130 final : Stepper { static stat_t set_hsrt_fn(nvObj_t *nv) { return get_fn<&type::set_hsrt>(nv); }; stat_t get_smin(nvObj_t *nv) { - nv->value = COOLCONF.semin; - nv->valuetype = TYPE_INT; + nv->value_int = COOLCONF.semin; + nv->valuetype = TYPE_INTEGER; return STAT_OK; }; static stat_t get_smin_fn(nvObj_t *nv) { return get_fn<&type::get_smin>(nv); }; stat_t set_smin(nvObj_t *nv) { - int32_t v = nv->value; + int32_t v = nv->value_int; if (v < 0) { nv->valuetype = TYPE_NULL; return (STAT_INPUT_LESS_THAN_MIN_VALUE); @@ -962,13 +962,13 @@ struct Trinamic2130 final : Stepper { static stat_t set_smin_fn(nvObj_t *nv) { return get_fn<&type::set_smin>(nv); }; stat_t get_smax(nvObj_t *nv) { - nv->value = COOLCONF.semax; - nv->valuetype = TYPE_INT; + nv->value_int = COOLCONF.semax; + nv->valuetype = TYPE_INTEGER; return STAT_OK; }; static stat_t get_smax_fn(nvObj_t *nv) { return get_fn<&type::get_smax>(nv); }; stat_t set_smax(nvObj_t *nv) { - int32_t v = nv->value; + int32_t v = nv->value_int; if (v < 0) { nv->valuetype = TYPE_NULL; return (STAT_INPUT_LESS_THAN_MIN_VALUE); @@ -984,13 +984,13 @@ struct Trinamic2130 final : Stepper { static stat_t set_smax_fn(nvObj_t *nv) { return get_fn<&type::set_smax>(nv); }; stat_t get_sup(nvObj_t *nv) { - nv->value = COOLCONF.seup; - nv->valuetype = TYPE_INT; + nv->value_int = COOLCONF.seup; + nv->valuetype = TYPE_INTEGER; return STAT_OK; }; static stat_t get_sup_fn(nvObj_t *nv) { return get_fn<&type::get_sup>(nv); }; stat_t set_sup(nvObj_t *nv) { - int32_t v = nv->value; + int32_t v = nv->value_int; if (v < 0) { nv->valuetype = TYPE_NULL; return (STAT_INPUT_LESS_THAN_MIN_VALUE); @@ -1006,13 +1006,13 @@ struct Trinamic2130 final : Stepper { static stat_t set_sup_fn(nvObj_t *nv) { return get_fn<&type::set_sup>(nv); }; stat_t get_sdn(nvObj_t *nv) { - nv->value = COOLCONF.sedn; - nv->valuetype = TYPE_INT; + nv->value_int = COOLCONF.sedn; + nv->valuetype = TYPE_INTEGER; return STAT_OK; }; static stat_t get_sdn_fn(nvObj_t *nv) { return get_fn<&type::get_sdn>(nv); }; stat_t set_sdn(nvObj_t *nv) { - int32_t v = nv->value; + int32_t v = nv->value_int; if (v < 0) { nv->valuetype = TYPE_NULL; return (STAT_INPUT_LESS_THAN_MIN_VALUE); diff --git a/g2core/encoder.cpp b/g2core/encoder.cpp old mode 100755 new mode 100644 index 184f9183..905a672d --- a/g2core/encoder.cpp +++ b/g2core/encoder.cpp @@ -2,7 +2,7 @@ * encoder.c - encoder interface * This file is part of the g2core project * - * Copyright (c) 2013 - 2016 Alden S. Hart, Jr. + * Copyright (c) 2013 - 2018 Alden S. Hart, Jr. * * This file ("the software") is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License, version 2 as published by the diff --git a/g2core/encoder.h b/g2core/encoder.h old mode 100755 new mode 100644 index 6d4b0f44..1d1d0dc7 --- a/g2core/encoder.h +++ b/g2core/encoder.h @@ -2,7 +2,7 @@ * encoder.h - encoder interface * This file is part of g2core project * - * Copyright (c) 2013 - 2016 Alden S. Hart, Jr. + * Copyright (c) 2013 - 2018 Alden S. Hart, Jr. * * This file ("the software") is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License, version 2 as published by the diff --git a/g2core/error.h b/g2core/error.h index c5c3e46d..3b54c9ff 100644 --- a/g2core/error.h +++ b/g2core/error.h @@ -2,8 +2,8 @@ * error.h - g2core status codes * This file is part of the g2core project * - * Copyright (c) 2010 - 2017 Alden S. Hart, Jr. - * Copyright (c) 2010 - 2017 Robert Giseburt + * Copyright (c) 2010 - 2018 Alden S. Hart, Jr. + * Copyright (c) 2010 - 2018 Robert Giseburt * * This file ("the software") is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License, version 2 as published by the @@ -161,10 +161,11 @@ char *get_status_message(stat_t status); #define STAT_ERROR_84 84 #define STAT_ERROR_85 85 #define STAT_ERROR_86 86 -#define STAT_ERROR_87 87 // Assertion failures - build down from 99 until they meet the system internal errors +#define STAT_SPINDLE_ASSERTION_FAILURE 86 +#define STAT_EXEC_ALINE_ASSERTION_FAILURE 87 #define STAT_BUFFER_FREE_ASSERTION_FAILURE 88 #define STAT_STATE_MANAGEMENT_ASSERTION_FAILURE 89 #define STAT_CONFIG_ASSERTION_FAILURE 90 @@ -223,19 +224,19 @@ char *get_status_message(stat_t status); #define STAT_GCODE_COMMAND_UNSUPPORTED 131 // G command is not supported #define STAT_MCODE_COMMAND_UNSUPPORTED 132 // M command is not supported #define STAT_GCODE_MODAL_GROUP_VIOLATION 133 // gcode modal group error -#define STAT_GCODE_AXIS_IS_MISSING 134 // command requires at least one axis present -#define STAT_GCODE_AXIS_CANNOT_BE_PRESENT 135 // error if G80 has axis words -#define STAT_GCODE_AXIS_IS_INVALID 136 // an axis is specified that is illegal for the command -#define STAT_GCODE_AXIS_IS_NOT_CONFIGURED 137 // WARNING: attempt to program an axis that is disabled -#define STAT_GCODE_AXIS_NUMBER_IS_MISSING 138 // axis word is missing its value -#define STAT_GCODE_AXIS_NUMBER_IS_INVALID 139 // axis word value is illegal +#define STAT_AXIS_IS_MISSING 134 // command requires at least one axis present +#define STAT_AXIS_CANNOT_BE_PRESENT 135 // error if G80 has axis words +#define STAT_AXIS_IS_INVALID 136 // an axis is specified that is illegal for the command +#define STAT_AXIS_IS_NOT_CONFIGURED 137 // WARNING: attempt to program an axis that is disabled +#define STAT_AXIS_NUMBER_IS_MISSING 138 // axis word is missing its value +#define STAT_AXIS_NUMBER_IS_INVALID 139 // axis word value is illegal -#define STAT_GCODE_ACTIVE_PLANE_IS_MISSING 140 // active plane is not programmed -#define STAT_GCODE_ACTIVE_PLANE_IS_INVALID 141 // active plane selected is not valid for this command -#define STAT_GCODE_FEEDRATE_NOT_SPECIFIED 142 // move has no feedrate -#define STAT_GCODE_INVERSE_TIME_MODE_CANNOT_BE_USED 143 // G38.2 and some canned cycles cannot accept inverse time mode -#define STAT_GCODE_ROTARY_AXIS_CANNOT_BE_USED 144 // G38.2 and some other commands cannot have rotary axes -#define STAT_GCODE_G53_WITHOUT_G0_OR_G1 145 // G0 or G1 must be active for G53 +#define STAT_ACTIVE_PLANE_IS_MISSING 140 // active plane is not programmed +#define STAT_ACTIVE_PLANE_IS_INVALID 141 // active plane selected is not valid for this command +#define STAT_FEEDRATE_NOT_SPECIFIED 142 // move has no feedrate +#define STAT_INVERSE_TIME_MODE_CANNOT_BE_USED 143 // G38.2 and some canned cycles cannot accept inverse time mode +#define STAT_ROTARY_AXIS_CANNOT_BE_USED 144 // G38.2 and some other commands cannot have rotary axes +#define STAT_GCODE_G53_WITHOUT_G0_OR_G1 145 // G0 or G1 must be active for G53 #define STAT_REQUESTED_VELOCITY_EXCEEDS_LIMITS 146 #define STAT_CUTTER_COMPENSATION_CANNOT_BE_ENABLED 147 #define STAT_PROGRAMMED_POINT_SAME_AS_CURRENT_POINT 148 @@ -244,9 +245,9 @@ char *get_status_message(stat_t status); #define STAT_SPINDLE_SPEED_MAX_EXCEEDED 150 #define STAT_SPINDLE_MUST_BE_OFF 151 #define STAT_SPINDLE_MUST_BE_TURNING 152 // some canned cycles require spindle to be turning when called -#define STAT_ARC_ERROR_RESERVED 153 // RESERVED +#define STAT_ARC_SPECIFICATION_ERROR 153 // generic arc specification error #define STAT_ARC_HAS_IMPOSSIBLE_CENTER_POINT 154 // trap (.05 inch/.5 mm) OR ((.0005 inch/.005mm) AND .1% of radius condition -#define STAT_ARC_SPECIFICATION_ERROR 155 // generic arc specification error +#define STAT_ARC_HAS_ROTARY_AXIS 155 // arc specification includes a rotary axis #define STAT_ARC_AXIS_MISSING_FOR_SELECTED_PLANE 156 // arc is missing axis (axes) required by selected plane #define STAT_ARC_OFFSETS_MISSING_FOR_SELECTED_PLANE 157 // one or both offsets are not specified #define STAT_ARC_RADIUS_OUT_OF_TOLERANCE 158 // WARNING - radius arc is too large - accuracy in question @@ -257,7 +258,7 @@ char *get_status_message(stat_t status); #define STAT_P_WORD_IS_ZERO 162 #define STAT_P_WORD_IS_NEGATIVE 163 // dwells require positive P values #define STAT_P_WORD_IS_NOT_AN_INTEGER 164 // G10s and other commands require integer P numbers -#define STAT_P_WORD_IS_NOT_VALID_TOOL_NUMBER 165 +#define STAT_P_WORD_IS_NOT_VALID_TOOL_NUMBER 165 // invalid tool number #define STAT_D_WORD_IS_MISSING 166 #define STAT_D_WORD_IS_INVALID 167 #define STAT_E_WORD_IS_MISSING 168 @@ -335,7 +336,7 @@ char *get_status_message(stat_t status); #define STAT_SOFT_LIMIT_EXCEEDED_AMAX 228 // soft limit error - A maximum #define STAT_SOFT_LIMIT_EXCEEDED_BMIN 229 // soft limit error - B minimum -#define STAT_SOFT_LIMIT_EXCEEDED_BMAX 220 // soft limit error - B maximum +#define STAT_SOFT_LIMIT_EXCEEDED_BMAX 230 // soft limit error - B maximum #define STAT_SOFT_LIMIT_EXCEEDED_CMIN 231 // soft limit error - C minimum #define STAT_SOFT_LIMIT_EXCEEDED_CMAX 232 // soft limit error - C maximum #define STAT_SOFT_LIMIT_EXCEEDED_ARC 233 // soft limit err on arc @@ -465,9 +466,9 @@ static const char stat_82[] = "82"; static const char stat_83[] = "83"; static const char stat_84[] = "84"; static const char stat_85[] = "85"; -static const char stat_86[] = "86"; -static const char stat_87[] = "87"; +static const char stat_86[] = "Spindle assertion failure"; +static const char stat_87[] = "mp_exec_aline() assertion failure"; static const char stat_88[] = "Buffer free assertion failure"; static const char stat_89[] = "State management assertion failure"; static const char stat_90[] = "Config assertion failure"; @@ -539,9 +540,9 @@ static const char stat_149[] = "Spindle speed below minimum"; static const char stat_150[] = "Spindle speed exceeded maximum"; static const char stat_151[] = "Spindle must be off for this command"; static const char stat_152[] = "Spindle must be turning for this command"; -static const char stat_153[] = "153"; +static const char stat_153[] = "Arc specification error"; static const char stat_154[] = "Arc specification error - impossible center point"; -static const char stat_155[] = "Arc specification error"; +static const char stat_155[] = "Arc specification error - arc has rotary axis(es)"; static const char stat_156[] = "Arc specification error - missing axis(es)"; static const char stat_157[] = "Arc specification error - missing offset(s)"; //--------------------------------------1--------10--------20--------30--------40--------50--------60-64 diff --git a/g2core/g2core.cppproj b/g2core/g2core.cppproj index c37cda30..4f42ceb0 100644 --- a/g2core/g2core.cppproj +++ b/g2core/g2core.cppproj @@ -68,12 +68,12 @@ - 10000000 + 2000000 SWD com.atmel.avrdbg.tool.atmelice - J41800030015 + J41800019454 Atmel-ICE True @@ -100,9 +100,9 @@ True true - J41800030015 + J41800019454 0x284E0A60 - 10000000 + 2000000 @@ -252,8 +252,8 @@ True - BOARD=sbv300 COLOR=0 VERBOSE=1 - clean BOARD=sbv300 + CONFIG=sbv300 COLOR=0 VERBOSE=1 + clean CONFIG=sbv300 Makefile bin\sbv300\ @@ -376,8 +376,8 @@ True - BOARD=g2v9k CONFIG=ShapeokoDualY COLOR=0 VERBOSE=1 - clean BOARD=g2v9k CONFIG=ShapeokoDualY + CONFIG=ShopbotTestV9 COLOR=0 VERBOSE=1 DEBUG=2 + clean CONFIG=ShopbotTestV9 Makefile bin\G2v9k\ @@ -1177,7 +1177,7 @@ bin\Makeblock-v9\ True - CONFIG=Makeblock COLOR=0 VERBOSE=1 + CONFIG=Makeblock COLOR=0 VERBOSE=1 DEBUG=2 clean CONFIG=Makeblock Makefile @@ -1653,6 +1653,9 @@ bin\gQuadratic on AxiDraw v3\ + + compile + compile @@ -1824,16 +1827,16 @@ compile - + compile - + compile compile - + compile @@ -1851,6 +1854,9 @@ compile + + compile + compile @@ -1866,6 +1872,12 @@ compile + + compile + + + compile + compile @@ -1920,7 +1932,7 @@ compile - + compile @@ -2016,6 +2028,9 @@ compile + + compile + compile @@ -2067,6 +2082,7 @@ + diff --git a/g2core/g2core.h b/g2core/g2core.h index 3d54b190..e7070412 100644 --- a/g2core/g2core.h +++ b/g2core/g2core.h @@ -2,8 +2,8 @@ * g2core.h - g2core main header file * This file is part of the g2core project * - * Copyright (c) 2010 - 2017 Alden S. Hart, Jr. - * Copyright (c) 2010 - 2017 Robert Giseburt + * Copyright (c) 2010 - 2019 Alden S. Hart, Jr. + * Copyright (c) 2010 - 2019 Robert Giseburt * * This file ("the software") is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License, version 2 as published by the @@ -60,16 +60,21 @@ typedef uint16_t magic_t; // magic number size // Note: If you change COORDS you must adjust the entries in cfgArray table in config.c +#define AXES 9 // number of axes supported in this version +#define HOMING_AXES 4 // number of axes that can be homed (assumes Zxyabc sequence) +#define COORDS 6 // number of supported coordinate systems (index starts at 1) +#define TOOLS 32 // number of entries in tool table (index starts at 1) + typedef enum { AXIS_X = 0, AXIS_Y, AXIS_Z, + AXIS_U, + AXIS_V, + AXIS_W, AXIS_A, AXIS_B, AXIS_C, - AXIS_U, // reserved - AXIS_V, // reserved - AXIS_W, // reserved AXIS_COREXY_A = AXIS_X, // CoreXY uses A and B AXIS_COREXY_B = AXIS_Y, AXIS_4WIRE_A = AXIS_X, // 4Wire uses A, B, C, D @@ -79,6 +84,18 @@ typedef enum { AXIS_4WIRE_Z = AXIS_B, // 4Wire uses A, B, C, D } cmAxes; +typedef enum { // external representation of axes (used in initialization) + AXIS_X_EXTERNAL = 0, + AXIS_Y_EXTERNAL, + AXIS_Z_EXTERNAL, + AXIS_A_EXTERNAL, + AXIS_B_EXTERNAL, + AXIS_C_EXTERNAL, + AXIS_U_EXTERNAL, + AXIS_V_EXTERNAL, + AXIS_W_EXTERNAL +} cmAxesExternal; + typedef enum { OFS_I = 0, OFS_J, diff --git a/g2core/g2core_info.h b/g2core/g2core_info.h index 501bd1c5..6e7a3a65 100644 --- a/g2core/g2core_info.h +++ b/g2core/g2core_info.h @@ -2,8 +2,8 @@ * g2core_info.h - g2core build information * This file is part of the g2core project * - * Copyright (c) 2010 - 2017 Alden S. Hart, Jr. - * Copyright (c) 2010 - 2017 Robert Giseburt + * Copyright (c) 2010 - 2018 Alden S. Hart, Jr. + * Copyright (c) 2010 - 2018 Robert Giseburt * * This file ("the software") is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License, version 2 as published by the @@ -21,15 +21,13 @@ #ifndef G2CORE_INFO_H_ONCE #define G2CORE_INFO_H_ONCE -#define G2CORE_FIRMWARE_BUILD 100.26 // Merged fixes for #267 and PR #258 +#define G2CORE_FIRMWARE_BUILD 101.03 // Issue #320, #354 - Added stepper polarity {1pl:n}, Fixed SR setting bug +#define G2CORE_FIRMWARE_VERSION 0.99 + #ifdef GIT_VERSION -#define G2CORE_FIRMWARE_BUILD_STRING GIT_VERSION +#define G2CORE_FIRMWARE_BUILD_STRING GIT_VERSION #else -#define G2CORE_FIRMWARE_BUILD_STRING "unknown" +#define G2CORE_FIRMWARE_BUILD_STRING "unknown" #endif -#define G2CORE_FIRMWARE_VERSION 0.99 // firmware major version -#define G2CORE_HARDWARE_PLATFORM HW_PLATFORM_V9 // hardware platform indicator (2 = Native Arduino Due) -#define G2CORE_HARDWARE_VERSION HW_VERSION_TINYGV9K // hardware platform revision number -#define G2CORE_HARDWARE_VERSION_MAX (G2CORE_HARDWARE_VERSION) #endif // G2CORE_INFO_H_ONCE diff --git a/g2core/gcode.h b/g2core/gcode.h new file mode 100644 index 00000000..1b04063b --- /dev/null +++ b/g2core/gcode.h @@ -0,0 +1,242 @@ +/* + * gcode.h - rs274/ngc Gcode model and parser support + * This file is part of the g2core project + * + * Copyright (c) 2010 - 2018 Alden S. Hart, Jr. + * + * This file ("the software") is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License, version 2 as published by the + * Free Software Foundation. You should have received a copy of the GNU General Public + * License, version 2 along with the software. If not, see . + * + * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT WITHOUT ANY + * WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES + * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT + * SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF + * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + */ + +#ifndef GCODE_H_ONCE +#define GCODE_H_ONCE + +#include "hardware.h" + +/**** Gcode-specific definitions ****/ + +typedef enum { // G Modal Group 1 + MOTION_MODE_STRAIGHT_TRAVERSE=0, // G0 - straight traverse + MOTION_MODE_STRAIGHT_FEED, // G1 - straight feed + MOTION_MODE_CW_ARC, // G2 - clockwise arc feed + MOTION_MODE_CCW_ARC, // G3 - counter-clockwise arc feed + MOTION_MODE_CANCEL_MOTION_MODE, // G80 + MOTION_MODE_STRAIGHT_PROBE, // G38.2 + MOTION_MODE_CANNED_CYCLE_81, // G81 - drilling + MOTION_MODE_CANNED_CYCLE_82, // G82 - drilling with dwell + MOTION_MODE_CANNED_CYCLE_83, // G83 - peck drilling + MOTION_MODE_CANNED_CYCLE_84, // G84 - right hand tapping + MOTION_MODE_CANNED_CYCLE_85, // G85 - boring, no dwell, feed out + MOTION_MODE_CANNED_CYCLE_86, // G86 - boring, spindle stop, rapid out + MOTION_MODE_CANNED_CYCLE_87, // G87 - back boring + MOTION_MODE_CANNED_CYCLE_88, // G88 - boring, spindle stop, manual out + MOTION_MODE_CANNED_CYCLE_89 // G89 - boring, dwell, feed out +} cmMotionMode; + +typedef enum { // canonical plane - translates to: + // axis_0 axis_1 axis_2 + CANON_PLANE_XY = 0, // G17 X Y Z + CANON_PLANE_XZ, // G18 X Z Y + CANON_PLANE_YZ // G19 Y Z X +} cmCanonicalPlane; + +typedef enum { + INCHES = 0, // G20 + MILLIMETERS, // G21 + DEGREES // ABC axes (this value used for displays only) +} cmUnitsMode; + +typedef enum { + ABSOLUTE_COORDS = 0, // machine coordinate system + G54, // G54 coordinate system + G55, // G55 coordinate system + G56, // G56 coordinate system + G57, // G57 coordinate system + G58, // G58 coordinate system + G59 // G59 coordinate system +} cmCoordSystem; +#define COORD_SYSTEM_MAX G59 // set this manually to the last one + +typedef enum { + ABSOLUTE_OVERRIDE_OFF = 0, // G53 disabled + ABSOLUTE_OVERRIDE_ON_DISPLAY_WITH_OFFSETS, // G53 enabled for movement, displays use current offsets + ABSOLUTE_OVERRIDE_ON_DISPLAY_WITH_NO_OFFSETS // G53 enabled for movement, displays use no offset +} cmAbsoluteOverride; + +typedef enum { // G Modal Group 13 + PATH_EXACT_PATH = 0, // G61 - hits corners but does not stop if it does not need to. + PATH_EXACT_STOP, // G61.1 - stops at all corners + PATH_CONTINUOUS // G64 and typically the default mode +} cmPathControl; + +typedef enum { + ABSOLUTE_DISTANCE_MODE = 0, // G90 / G90.1 + INCREMENTAL_DISTANCE_MODE // G91 / G91.1 +} cmDistanceMode; + +typedef enum { + INVERSE_TIME_MODE = 0, // G93 + UNITS_PER_MINUTE_MODE, // G94 + UNITS_PER_REVOLUTION_MODE// G95 (unimplemented) +} cmFeedRateMode; + +typedef enum { + ORIGIN_OFFSET_SET=0, // G92 - set origin offsets + ORIGIN_OFFSET_CANCEL, // G92.1 - zero out origin offsets + ORIGIN_OFFSET_SUSPEND, // G92.2 - do not apply offsets, but preserve the values + ORIGIN_OFFSET_RESUME // G92.3 - resume application of the suspended offsets +} cmOriginOffset; + +typedef enum { + PROGRAM_STOP = 0, + PROGRAM_END +} cmProgramFlow; + +typedef enum { // used for spindle and arc dir + DIRECTION_CW = 0, + DIRECTION_CCW +} cmDirection; + +typedef enum { // axis types. Enum must be in this order + AXIS_TYPE_SYSTEM=-2, // no axis, system parameter + AXIS_TYPE_UNDEFINED=-1, // invalid type + AXIS_TYPE_LINEAR, // linear axis + AXIS_TYPE_ROTARY // rotary axis +} cmAxisType; + +typedef enum { // axis modes (ordered: see _cm_get_feed_time()) + AXIS_DISABLED = 0, // kill axis + AXIS_STANDARD, // axis in coordinated motion w/standard behaviors + AXIS_INHIBITED, // axis is computed but not activated + AXIS_RADIUS // rotary axis calibrated to circumference +} cmAxisMode; +#define AXIS_MODE_LINEAR_MAX AXIS_INHIBITED +#define AXIS_MODE_ROTARY_MAX AXIS_RADIUS + +/* Gcode state structures */ + +/**************************************************************************************** + * GCODE MODEL - The following GCodeModel/GCodeInput structs are used: + * + * - gm is the core Gcode model state. It keeps the internal gcode state model in + * normalized canonical form. All values are unit converted (to mm) and in the + * machine coordinate system (absolute coordinate system). Gm is owned by the + * canonical machine layer and should be accessed only through cm_ routines. + * + * The gm core struct is copied and passed as context to the runtime where it is used + * for planning, move execution, feedholds, and reporting. + * + * - gmx is the extended gcode model variables that are only used by the canonical + * machine and do not need to be passed further down. It keeps "global" gcode state + * that does not change when you go down through the planner to the runtime. Other + * Gcode model state is kept in the singletons for various sub-systems, such as arcs + * spindle, coolant, and others (i.e. not ALL gcode global state is in gmx) + * + * - gn is used by the gcode interpreter and is re-initialized for each gcode block. + * It accepts data in the new gcode block in the formats present in the block + * (pre-normalized forms). During initialization some state elements are necessarily + * restored from gm. + * + * - gf is used by the gcode parser interpreter to hold flags for any data that has + * changed in gn during the parse. gf.target[] values are also used by the canonical + * machine during set_target(). + * + * - cfg (config struct in config.h) is also used heavily and contains some values that + * might be considered to be Gcode model values. The distinction is that all values + * in the config are persisted and restored, whereas the gm structs are transient. + * So cfg has the G54 - G59 offsets, but gm has the G92 offsets. cfg has the power-on / + * reset gcode default values, but gm has the operating state for the values + * (which may have changed). + */ + +typedef struct GCodeState { // Gcode model state - used by model, planning and runtime + int32_t linenum; // Gcode block line number + cmMotionMode motion_mode; // Group1: G0, G1, G2, G3, G38.2, G80, G81, G82 + // G83, G84, G85, G86, G87, G88, G89 + + float target[AXES]; // XYZABC target where the move should go + float target_comp[AXES]; // summation compensation (Kahan) overflow value + float display_offset[AXES]; // work offsets from the machine coordinate system (for reporting only) + + float feed_rate; // F - normalized to millimeters/minute or in inverse time mode + float P_word; // P - parameter used for dwell time in seconds, G10 coord select... + + cmFeedRateMode feed_rate_mode; // See cmFeedRateMode for settings + cmCanonicalPlane select_plane; // G17,G18,G19 - values to set plane to + cmUnitsMode units_mode; // G20,G21 - 0=inches (G20), 1 = mm (G21) + cmPathControl path_control; // G61... EXACT_PATH, EXACT_STOP, CONTINUOUS + cmDistanceMode distance_mode; // G90=use absolute coords, G91=incremental movement + cmDistanceMode arc_distance_mode; // G90.1=use absolute IJK offsets, G91.1=incremental IJK offsets + cmAbsoluteOverride absolute_override;// G53 TRUE = move using machine coordinates - this block only + cmCoordSystem coord_system; // G54-G59 - select coordinate system 1-9 + uint8_t tool; // G // M6 tool change - moves "tool_select" to "tool" + uint8_t tool_select; // G // T value - T sets this value + + void reset() { + linenum = 0; + motion_mode = MOTION_MODE_STRAIGHT_TRAVERSE; + + for (uint8_t i = 0; i< AXES; i++) { + target[i] = 0.0; + display_offset[i] = 0.0; + } + + feed_rate = 0.0; + P_word = 0.0; + + feed_rate_mode = INVERSE_TIME_MODE; + select_plane = CANON_PLANE_XY; + units_mode = INCHES; + path_control = PATH_EXACT_PATH; + distance_mode = ABSOLUTE_DISTANCE_MODE; + arc_distance_mode = ABSOLUTE_DISTANCE_MODE; + absolute_override = ABSOLUTE_OVERRIDE_OFF; + coord_system = ABSOLUTE_COORDS; + tool = 0; + tool_select = 0; + + }; +} GCodeState_t; + +typedef struct GCodeStateExtended { // Gcode dynamic state extensions - used by model and arcs + uint16_t magic_start; // magic number to test memory integrity + uint8_t next_action; // handles G modal group 1 moves & non-modals + uint8_t program_flow; // used only by the gcode_parser + int32_t last_line_number; // used with line checksums + + float position[AXES]; // XYZABC model position (Note: not used in gn or gf) + float g92_offset[AXES]; // XYZABC G92 offsets (aka origin offsets) (Note: not used in gn or gf) + float g28_position[AXES]; // XYZABC stored machine position for G28 + float g30_position[AXES]; // XYZABC stored machine position for G30 + float p1_position[AXES]; // XYZABC stored machine position for return to p1 planner + + bool m48_enable; // master feedrate / spindle speed override enable + bool mfo_enable; // feedrate override enable + float mfo_factor; // 1.0000 x F feed rate. Go up or down from there + bool mto_enable; // traverse override enable + float mto_factor; // valid from 0.05 to 1.00 + + bool g92_offset_enable; // G92 offsets enabled/disabled. 0=disabled, 1=enabled + bool block_delete_switch; // set true to enable block deletes (true is default) + + uint16_t magic_end; +} GCodeStateX_t; + +/* + * Global Scope Functions + */ +void gcode_parser_init(void); +stat_t gcode_parser(char* block); +stat_t gc_get_gc(nvObj_t* nv); +stat_t gc_run_gc(nvObj_t* nv); + +#endif // End of include guard: GCODE_H_ONCE diff --git a/g2core/gcode_parser.cpp b/g2core/gcode_parser.cpp index 60b7b63d..b6486fc3 100644 --- a/g2core/gcode_parser.cpp +++ b/g2core/gcode_parser.cpp @@ -2,8 +2,8 @@ * gcode_parser.cpp - rs274/ngc Gcode parser * This file is part of the g2core project * - * Copyright (c) 2010 - 2017 Alden S. Hart, Jr. - * Copyright (c) 2016 - 2017 Rob Giseburt + * Copyright (c) 2010 - 2019 Alden S. Hart, Jr. + * Copyright (c) 2016 - 2019 Rob Giseburt * * This file ("the software") is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License, version 2 as published by the @@ -20,7 +20,7 @@ #include "g2core.h" // #1 #include "config.h" // #2 #include "controller.h" -#include "gcode_parser.h" +#include "gcode.h" #include "canonical_machine.h" #include "settings.h" #include "spindle.h" @@ -60,8 +60,8 @@ typedef enum { // Used for detecting gcode errors. See #define MODAL_GROUP_COUNT (MODAL_GROUP_M9+1) // Note 1: Our G0 omits G4,G30,G53,G92.1,G92.2,G92.3 as these have no axis components to error check -/* The difference between NextAction and MotionMode is that NextAction is - * used by the current block, and may carry non-modal commands, whereas +/* The difference between NextAction and MotionMode (in canonical machine) is that +* NextAction is used by the current block, and may carry non-modal commands, whereas * MotionMode persists across blocks (as G modal group 1) */ @@ -83,15 +83,15 @@ typedef enum { // these are in order to optimiz NEXT_ACTION_SET_TL_OFFSET, // G43 NEXT_ACTION_SET_ADDITIONAL_TL_OFFSET, // G43.2 NEXT_ACTION_CANCEL_TL_OFFSET, // G49 - NEXT_ACTION_SET_ORIGIN_OFFSETS, // G92 - NEXT_ACTION_RESET_ORIGIN_OFFSETS, // G92.1 - NEXT_ACTION_SUSPEND_ORIGIN_OFFSETS, // G92.2 - NEXT_ACTION_RESUME_ORIGIN_OFFSETS, // G92.3 + NEXT_ACTION_SET_G92_OFFSETS, // G92 + NEXT_ACTION_RESET_G92_OFFSETS, // G92.1 + NEXT_ACTION_SUSPEND_G92_OFFSETS, // G92.2 + NEXT_ACTION_RESUME_G92_OFFSETS, // G92.3 NEXT_ACTION_JSON_COMMAND_SYNC, // M100 NEXT_ACTION_JSON_COMMAND_ASYNC, // M100.1 NEXT_ACTION_JSON_WAIT, // M101 -#if MARLIN_COMPAT_ENABLED == true +#if MARLIN_COMPAT_ENABLED == true // supported Marlin Gcode and M codes. Also E NEXT_ACTION_MARLIN_TRAM_BED, // G29 NEXT_ACTION_MARLIN_DISABLE_MOTORS, // M84 NEXT_ACTION_MARLIN_SET_MT, // M84 (with S), M85 @@ -118,17 +118,16 @@ typedef struct GCodeInputValue { // Gcode inputs - meaning depends on context gpNextAction next_action; // handles G modal group 1 moves & non-modals cmMotionMode motion_mode; // Group1: G0, G1, G2, G3, G38.2, G80, G81, G82, G83, G84, G85, G86, G87, G88, G89 uint8_t program_flow; // used only by the gcode_parser - uint32_t linenum; // N word + uint32_t linenum; // gcode N word float target[AXES]; // XYZABC where the move should go float arc_offset[3]; // IJK - used by arc commands - float arc_radius; // R - radius value in arc radius mode - - float F_word; // F - normalized to millimeters/minute + float arc_radius; // R word - radius value in arc radius mode + float F_word; // F word - feedrate as present in the F word (will be normalized later) + float P_word; // P word - parameter used for dwell time in seconds, G10 commands + float S_word; // S word - usually in RPM uint8_t H_word; // H word - used by G43s uint8_t L_word; // L word - used by G10s - float P_word; // P - parameter used for dwell time in seconds, G10 coord select... - float S_word; // S word - in RPM uint8_t feed_rate_mode; // See cmFeedRateMode for settings uint8_t select_plane; // G17,G18,G19 - values to set plane to @@ -139,17 +138,19 @@ typedef struct GCodeInputValue { // Gcode inputs - meaning depends on context uint8_t arc_distance_mode; // G90.1=use absolute IJK offsets, G91.1=incremental IJK offsets uint8_t origin_offset_mode; // G92...TRUE=in origin offset mode uint8_t absolute_override; // G53 TRUE = move using machine coordinates - this block only (G53) + uint8_t tool; // Tool after T and M6 (tool_select and tool_change) uint8_t tool_select; // T value - T sets this value uint8_t tool_change; // M6 tool change flag - moves "tool_select" to "tool" - uint8_t mist_coolant; // TRUE = mist on (M7), FALSE = off (M9) - uint8_t flood_coolant; // TRUE = flood on (M8), FALSE = off (M9) + uint8_t coolant_mist; // TRUE = mist on (M7) + uint8_t coolant_flood; // TRUE = flood on (M8) + uint8_t coolant_off; // TRUE = turn off all coolants (M9) uint8_t spindle_control; // 0=OFF (M5), 1=CW (M3), 2=CCW (M4) bool m48_enable; // M48/M49 input (enables for feed and spindle) - bool mfo_control; // M50 feedrate override control - bool mto_control; // M50.1 traverse override control - bool sso_control; // M51 spindle speed override control + bool fro_control; // M50 feedrate override control + bool tro_control; // M50.1 traverse override control + bool spo_control; // M51 spindle speed override control #if MARLIN_COMPAT_ENABLED == true float E_word; // E - "extruder" - may be interpreted any number of ways @@ -170,10 +171,10 @@ typedef struct GCodeFlags { // Gcode input flags bool arc_radius; bool F_word; - bool H_word; - bool L_word; bool P_word; bool S_word; + bool H_word; + bool L_word; bool feed_rate_mode; bool select_plane; @@ -184,18 +185,20 @@ typedef struct GCodeFlags { // Gcode input flags bool arc_distance_mode; bool origin_offset_mode; bool absolute_override; + bool tool; bool tool_select; bool tool_change; - bool mist_coolant; - bool flood_coolant; + bool coolant_mist; + bool coolant_flood; + bool coolant_off; bool spindle_control; bool m48_enable; - bool mfo_control; - bool mto_control; - bool sso_control; - + bool fro_control; + bool tro_control; + bool spo_control; + bool checksum; #if MARLIN_COMPAT_ENABLED == true @@ -215,13 +218,13 @@ GCodeValue_t gv; // gcode input values GCodeFlag_t gf; // gcode input flags // local helper functions and macros -void _normalize_gcode_block(char *str, char **active_comment, uint8_t *block_delete_flag); -stat_t _get_next_gcode_word(char **pstr, char *letter, float *value); -stat_t _point(float value); -stat_t _verify_checksum(char *str); -stat_t _validate_gcode_block(char *active_comment); -stat_t _parse_gcode_block(char *line, char *active_comment); // Parse the block into the GN/GF structs -stat_t _execute_gcode_block(char *active_comment); // Execute the gcode block +static void _normalize_gcode_block(char *str, char **active_comment, uint8_t *block_delete_flag); +static stat_t _get_next_gcode_word(char **pstr, char *letter, float *value, int32_t *value_int); +static stat_t _point(float value); +static stat_t _verify_checksum(char *str); +static stat_t _validate_gcode_block(char *active_comment); +static stat_t _parse_gcode_block(char *line, char *active_comment); // Parse the block into the GN/GF structs +static stat_t _execute_gcode_block(char *active_comment); // Execute the gcode block #define SET_MODAL(m,parm,val) ({gv.parm=val; gf.parm=true; gp.modals[m]=true; break;}) #define SET_NON_MODAL(parm,val) ({gv.parm=val; gf.parm=true; break;}) @@ -282,7 +285,7 @@ stat_t gcode_parser(char *block) * Returns STAT_OK is it's valid. * Returns STAT_CHECKSUM_MATCH_FAILED if the checksum doesn't match. */ -stat_t _verify_checksum(char *str) +static stat_t _verify_checksum(char *str) { bool has_line_number = false; // -1 means we don't have one if (*str == 'N') { @@ -302,50 +305,59 @@ stat_t _verify_checksum(char *str) *(str-1) = 0; // null terminate, the parser won't like this * here! gf.checksum = true; if (strtol(str, NULL, 10) != checksum) { - _debug_trap("checksum failure"); + debug_trap("checksum failure"); return STAT_CHECKSUM_MATCH_FAILED; } if (!has_line_number) { - _debug_trap("line number missing with checksum"); + debug_trap("line number missing with checksum"); return STAT_MISSING_LINE_NUMBER_WITH_CHECKSUM; } } return STAT_OK; } -/* +/**************************************************************************************** * _normalize_gcode_block() - normalize a block (line) of gcode in place * - * Normalization functions: - * - Isolate "active comments" - * - Many of the following are performed in active comments as well - * - Strings are handled special (TODO) - * - Active comments are moved to the end of the string, and multiple active comments are merged into one - * - convert all letters to upper case - * - remove white space, control and other invalid characters - * - remove (erroneous) leading zeros that might be taken to mean Octal - * - identify and return start of comments and messages - * - signal if a block-delete character (/) was encountered in the first space + * Baseline normalization functions: + * - Isolate comments. See below. + * The rest of this applies just to the GCODE string itself (not the comments): + * - Remove white space, control and other invalid characters + * - Convert all letters to upper case + * - Remove (erroneous) leading zeros that might be taken to mean Octal + * - Signal if a block-delete character (/) was encountered in the first space * - NOTE: Assumes no leading whitespace as this was removed at the controller dispatch level * * So this: "g1 x100 Y100 f400" becomes this: "G1X100Y100F400" * - * Comment and message handling: + * Comment, active comment and message handling: + * - Comment fields start with a '(' char or alternately a semicolon ';' or percent '%' + * - Semicolon ';' or percent '%' end the line. All characters past are discarded + * - Multiple embedded comments are acceptable if '(' form * - Active comments start with exactly "({" and end with "})" (no relaxing, invalid is invalid) - * - Comments field start with a '(' char or alternately a semicolon ';' - * - Active comments are moved to the end of the string and merged. - * - Messages are converted to ({msg:"blah"}) active comments. + * - Active comments are moved to the end of the string + * - Multiple active comments are merged and moved to the end of the string + * - Gcode message comments (MSG) are converted to ({msg:"blah"}) active comments * - The 'MSG' specifier in comment can have mixed case but cannot cannot have embedded white spaces - * - Other "plain" comments will be discarded. - * - Multiple embedded comments are acceptable. - * - Multiple active comments will be merged. - * - Only ONE MSG comment will be accepted. + * - Only ONE MSG comment will be accepted + * - Other "plain" comments are discarded * * Returns: * - com points to comment string or to NUL if no comment * - msg points to message string or to NUL if no comment * - block_delete_flag is set true if block delete encountered, false otherwise */ +/* Active comment notes: + * + * We will convert as follows: + * FROM: G0 ({blah: t}) x10 (comment) + * TO : g0x10\0{blah:t} + * NOTES: Active comments moved to the end, stripped of (), everything lowercased, and plain comment removed. + * + * FROM: M100 ({a:t}) (comment) ({b:f}) (comment) + * TO : m100\0{a:t,b:f} + * NOTES: multiple active comments merged, stripped of (), and actual comments ignored. + */ char _normalize_scratch[RX_BUFFER_SIZE]; @@ -355,28 +367,13 @@ void _normalize_gcode_block(char *str, char **active_comment, uint8_t *block_del char *gc_rd = str; // read pointer char *gc_wr = _normalize_scratch; // write pointer - - char *ac_rd = str; // read pointer + char *ac_rd = str; // Active Comment read pointer char *ac_wr = _normalize_scratch; // Active Comment write pointer - bool last_char_was_digit = false; // used for octal stripping - /* Active comment notes: - - We will convert as follows: - FROM: G0 ({blah: t}) x10 (comment) - TO : g0x10\0{blah:t} - NOTES: Active comments moved to the end, stripped of (), everything lowercased, and plain comment removed. - - FROM: M100 ({a:t}) (comment) ({b:f}) (comment) - TO : m100\0{a:t,b:f} - NOTES: multiple active comments merged, stripped of (), and actual comments ignored. - */ - // Move the ac_wr point forward one for every non-AC character we KEEP (plus one for a NULL in between) ac_wr++; // account for the in-between NULL - // mark block deletes if (*gc_rd == '/') { *block_delete_flag = true; @@ -386,10 +383,8 @@ void _normalize_gcode_block(char *str, char **active_comment, uint8_t *block_del } while (*gc_rd != 0) { - // check for ';' or '%' comments that end the line. - if ((*gc_rd == ';') || (*gc_rd == '%')) { - // go ahead and snap the string off cleanly here - *gc_rd = 0; + if ((*gc_rd == ';') || (*gc_rd == '%')) { // check for ';' or '%' comments that end the line + *gc_rd = 0; // go ahead and snap the string off cleanly here break; } @@ -402,7 +397,7 @@ void _normalize_gcode_block(char *str, char **active_comment, uint8_t *block_del ((*(gc_rd+2) == 'g') || (*(gc_rd+2) == 'G')) )) { if (ac_rd == nullptr) { - ac_rd = gc_rd; // note the start of the first AC + ac_rd = gc_rd; // note the start of the first AC } // skip the comment, handling strings carefully @@ -414,7 +409,6 @@ void _normalize_gcode_block(char *str, char **active_comment, uint8_t *block_del if ((*gc_rd == '\\') && (*(gc_rd+1) != 0)) { gc_rd++; // Skip it, it's escaped. } - } else if ((*gc_rd == ')')) { break; } @@ -424,9 +418,7 @@ void _normalize_gcode_block(char *str, char **active_comment, uint8_t *block_del } } else { *(gc_rd-1) = ' '; // Change the '(' to a space to simplify the comment copy later - - // skip ahead until we find a ')' (or NULL) - while ((*gc_rd != 0) && (*gc_rd != ')')) { + while ((*gc_rd != 0) && (*gc_rd != ')')) { // skip ahead until we find a ')' (or NULL) gc_rd++; } } @@ -434,6 +426,7 @@ void _normalize_gcode_block(char *str, char **active_comment, uint8_t *block_del bool do_copy = false; // Perform Octal stripping - remove invalid leading zeros in number strings + // Otherwise number conversions can fail, as Gcode does not support octal but C libs do // Change 0123.004 to 123.004, or -0234.003 to -234.003 if (isdigit(*gc_rd) || (*gc_rd == '.')) { // treat '.' as a digit so we don't strip after one if (last_char_was_digit || (*gc_rd != '0') || !isdigit(*(gc_rd+1))) { @@ -445,22 +438,17 @@ void _normalize_gcode_block(char *str, char **active_comment, uint8_t *block_del last_char_was_digit = false; do_copy = true; } - if (do_copy) { *(gc_wr++) = toupper(*gc_rd); ac_wr++; // move the ac start position } } - gc_rd++; } // Enforce null termination *gc_wr = 0; - - // note the beginning of the comments - char *comment_start = ac_wr; - + char *comment_start = ac_wr; // note the beginning of the comments if (ac_rd != nullptr) { // Now we'll copy the comments to the scratch @@ -554,7 +542,6 @@ void _normalize_gcode_block(char *str, char **active_comment, uint8_t *block_del break; } } - ac_rd++; } } @@ -568,8 +555,7 @@ void _normalize_gcode_block(char *str, char **active_comment, uint8_t *block_del *active_comment = str + (comment_start - _normalize_scratch); } - -/* +/**************************************************************************************** * _get_next_gcode_word() - get gcode word consisting of a letter and a value * * This function requires the Gcode string to be normalized. @@ -577,7 +563,7 @@ void _normalize_gcode_block(char *str, char **active_comment, uint8_t *block_del * G0X... is not interpreted as hexadecimal. This is trapped. */ -stat_t _get_next_gcode_word(char **pstr, char *letter, float *value) +static stat_t _get_next_gcode_word(char **pstr, char *letter, float *value, int32_t *value_int) { if (**pstr == NUL) { return (STAT_COMPLETE); } // no more words @@ -588,22 +574,25 @@ stat_t _get_next_gcode_word(char **pstr, char *letter, float *value) *letter = **pstr; (*pstr)++; +// Removed support for hex numbers // // X-axis-becomes-a-hexadecimal-number get-value case, e.g. G0X100 --> G255 // if ((**pstr == '0') && (*(*pstr+1) == 'X')) { // *value = 0; // (*pstr)++; -// return (STAT_OK); // pointer points to X +// return (STAT_OK); // pointer points to X // } // // // get-value general case -// char *end = *pstr; +// char *end; +// *value_int = atol(*pstr); // needed to get an accurate line number for N > 8,388,608 // *value = strtof(*pstr, &end); // get-value general case char *end = *pstr; *value = c_atof(end); + *value_int = atol(*pstr); // needed to get an accurate line number for N > 8,388,608 - if(end == *pstr) { + if (end == *pstr) { #if MARLIN_COMPAT_ENABLED == true if (mst.marlin_flavor) { *value = 0; @@ -615,23 +604,23 @@ stat_t _get_next_gcode_word(char **pstr, char *letter, float *value) #endif } // more robust test then checking for value=0; *pstr = end; - return (STAT_OK); // pointer points to next character after the word + return (STAT_OK); // pointer points to next character after the word } /* * _point() - isolate the decimal point value as an integer */ -uint8_t _point(const float value) +static uint8_t _point(const float value) { return((uint8_t)(std::round(value*10.0) - std::trunc(value)*10.0)); // isolate the decimal point as an int } -/* +/**************************************************************************************** * _validate_gcode_block() - check for some gross Gcode block semantic violations */ -stat_t _validate_gcode_block(char *active_comment) +static stat_t _validate_gcode_block(char *active_comment) { // Check for modal group violations. From NIST, section 3.4 "It is an error to put // a G-code from group 1 and a G-code from group 0 on the same line if both of them @@ -640,19 +629,19 @@ stat_t _validate_gcode_block(char *active_comment) // uses axis words appears on the line, the activity of the group 1 G-code is suspended // for that line. The axis word-using G-codes from group 0 are G10, G28, G30, and G92" -// if ((cm.gn.modals[MODAL_GROUP_G0] == true) && (cm.gn.modals[MODAL_GROUP_G1] == true)) { +// if ((gp.modals[MODAL_GROUP_G0] == true) && (gp.modals[MODAL_GROUP_G1] == true)) { // return (STAT_MODAL_GROUP_VIOLATION); // } // look for commands that require an axis word to be present -// if ((cm.gn.modals[MODAL_GROUP_G0] == true) || (cm.gn.modals[MODAL_GROUP_G1] == true)) { +// if ((gp.modals[MODAL_GROUP_G0] == true) || (gp.modals[MODAL_GROUP_G1] == true)) { // if (_axis_changed() == false) // return (STAT_GCODE_AXIS_IS_MISSING); // } return (STAT_OK); } -/* +/**************************************************************************************** * _parse_gcode_block() - parses one line of NULL terminated G-Code. * * All the parser does is load the state values in gn (next model state) and set flags @@ -660,11 +649,12 @@ stat_t _validate_gcode_block(char *active_comment) * contain only uppercase characters and signed floats (no whitespace). */ -stat_t _parse_gcode_block(char *buf, char *active_comment) +static stat_t _parse_gcode_block(char *buf, char *active_comment) { char *pstr = (char *)buf; // persistent pointer into gcode block for parsing words char letter; // parsed letter, eg.g. G or X or Y float value = 0; // value parsed from letter (e.g. 2 for G2) + int32_t value_int = 0; // integer value parsed from letter - needed for line numbers stat_t status = STAT_OK; // set initial state for new move @@ -675,13 +665,13 @@ stat_t _parse_gcode_block(char *buf, char *active_comment) // Causes a later exception if // (1) INVERSE_TIME_MODE is active and a feed rate is not provided or // (2) INVERSE_TIME_MODE is changed to UNITS_PER_MINUTE and a new feed rate is missing - if (cm.gm.feed_rate_mode == INVERSE_TIME_MODE) {// new feed rate req'd when in INV_TIME_MODE + if (cm->gm.feed_rate_mode == INVERSE_TIME_MODE) {// new feed rate required when in INV_TIME_MODE gv.F_word = 0; gf.F_word = true; } // extract commands and parameters - while((status = _get_next_gcode_word(&pstr, &letter, &value)) == STAT_OK) { + while((status = _get_next_gcode_word(&pstr, &letter, &value, &value_int)) == STAT_OK) { switch(letter) { case 'G': switch((uint8_t)value) { @@ -710,7 +700,6 @@ stat_t _parse_gcode_block(char *buf, char *active_comment) #if MARLIN_COMPAT_ENABLED == true case 29: SET_NON_MODAL (next_action, NEXT_ACTION_MARLIN_TRAM_BED); #endif - case 30: { switch (_point(value)) { case 0: SET_MODAL (MODAL_GROUP_G0, next_action, NEXT_ACTION_GOTO_G30_POSITION); @@ -739,7 +728,7 @@ stat_t _parse_gcode_block(char *buf, char *active_comment) break; } case 49: SET_NON_MODAL (next_action, NEXT_ACTION_CANCEL_TL_OFFSET); - case 53: SET_NON_MODAL (absolute_override, true); + case 53: SET_NON_MODAL (absolute_override, ABSOLUTE_OVERRIDE_ON_DISPLAY_WITH_NO_OFFSETS); case 54: SET_MODAL (MODAL_GROUP_G12, coord_system, G54); case 55: SET_MODAL (MODAL_GROUP_G12, coord_system, G55); case 56: SET_MODAL (MODAL_GROUP_G12, coord_system, G56); @@ -774,10 +763,10 @@ stat_t _parse_gcode_block(char *buf, char *active_comment) } case 92: { switch (_point(value)) { - case 0: SET_MODAL (MODAL_GROUP_G0, next_action, NEXT_ACTION_SET_ORIGIN_OFFSETS); - case 1: SET_NON_MODAL (next_action, NEXT_ACTION_RESET_ORIGIN_OFFSETS); - case 2: SET_NON_MODAL (next_action, NEXT_ACTION_SUSPEND_ORIGIN_OFFSETS); - case 3: SET_NON_MODAL (next_action, NEXT_ACTION_RESUME_ORIGIN_OFFSETS); + case 0: SET_MODAL (MODAL_GROUP_G0, next_action, NEXT_ACTION_SET_G92_OFFSETS); + case 1: SET_NON_MODAL (next_action, NEXT_ACTION_RESET_G92_OFFSETS); + case 2: SET_NON_MODAL (next_action, NEXT_ACTION_SUSPEND_G92_OFFSETS); + case 3: SET_NON_MODAL (next_action, NEXT_ACTION_RESUME_G92_OFFSETS); default: status = STAT_GCODE_COMMAND_UNSUPPORTED; } break; @@ -796,23 +785,23 @@ stat_t _parse_gcode_block(char *buf, char *active_comment) SET_MODAL (MODAL_GROUP_M4, program_flow, PROGRAM_STOP); case 2: case 30: SET_MODAL (MODAL_GROUP_M4, program_flow, PROGRAM_END); - case 3: SET_MODAL (MODAL_GROUP_M7, spindle_control, SPINDLE_CONTROL_CW); - case 4: SET_MODAL (MODAL_GROUP_M7, spindle_control, SPINDLE_CONTROL_CCW); - case 5: SET_MODAL (MODAL_GROUP_M7, spindle_control, SPINDLE_CONTROL_OFF); + case 3: SET_MODAL (MODAL_GROUP_M7, spindle_control, SPINDLE_CW); + case 4: SET_MODAL (MODAL_GROUP_M7, spindle_control, SPINDLE_CCW); + case 5: SET_MODAL (MODAL_GROUP_M7, spindle_control, SPINDLE_OFF); case 6: SET_NON_MODAL (tool_change, true); - case 7: SET_MODAL (MODAL_GROUP_M8, mist_coolant, true); - case 8: SET_MODAL (MODAL_GROUP_M8, flood_coolant, true); - case 9: SET_MODAL (MODAL_GROUP_M8, flood_coolant, false); + case 7: SET_MODAL (MODAL_GROUP_M8, coolant_mist, COOLANT_ON); + case 8: SET_MODAL (MODAL_GROUP_M8, coolant_flood, COOLANT_ON); + case 9: SET_MODAL (MODAL_GROUP_M8, coolant_off, COOLANT_OFF); case 48: SET_MODAL (MODAL_GROUP_M9, m48_enable, true); case 49: SET_MODAL (MODAL_GROUP_M9, m48_enable, false); - case 50: SET_MODAL (MODAL_GROUP_M9, mfo_control, true); + case 50: switch (_point(value)) { - case 0: SET_MODAL (MODAL_GROUP_M9, mfo_control, true); - case 1: SET_MODAL (MODAL_GROUP_M9, mto_control, true); + case 0: SET_MODAL (MODAL_GROUP_M9, fro_control, true); + case 1: SET_MODAL (MODAL_GROUP_M9, tro_control, true); default: status = STAT_GCODE_COMMAND_UNSUPPORTED; } break; - case 51: SET_MODAL (MODAL_GROUP_M9, sso_control, true); + case 51: SET_MODAL (MODAL_GROUP_M9, spo_control, true); case 100: switch (_point(value)) { case 0: SET_NON_MODAL (next_action, NEXT_ACTION_JSON_COMMAND_SYNC); @@ -822,7 +811,7 @@ stat_t _parse_gcode_block(char *buf, char *active_comment) break; case 101: SET_NON_MODAL (next_action, NEXT_ACTION_JSON_WAIT); -#if MARLIN_COMPAT_ENABLED == true +#if MARLIN_COMPAT_ENABLED == true // Note: case ordering and presence/absence of break;s is very important case 20:marlin_list_sd_response(); status = STAT_COMPLETE; break; // List SD card case 21: // Initialize SD card case 22: status = STAT_COMPLETE; break; // Release SD card @@ -838,7 +827,7 @@ stat_t _parse_gcode_block(char *buf, char *active_comment) case 105: SET_NON_MODAL (next_action, NEXT_ACTION_MARLIN_PRINT_TEMPERATURES);// request temperature report case 106: SET_NON_MODAL (next_action, NEXT_ACTION_MARLIN_SET_FAN_SPEED); // set fan speed range 0 - 255 case 107: SET_NON_MODAL (next_action, NEXT_ACTION_MARLIN_STOP_FAN); // stop fan (speed = 0) - case 108: SET_NON_MODAL (next_action, NEXT_ACTION_MARLIN_CANCEL_WAIT_TEMP); // cancel wait for temparature + case 108: SET_NON_MODAL (next_action, NEXT_ACTION_MARLIN_CANCEL_WAIT_TEMP); // cancel wait for temperature case 114: SET_NON_MODAL (next_action, NEXT_ACTION_MARLIN_PRINT_POSITION); // request position report case 109: gf.marlin_wait_for_temp = true; // NO break! // set wait for temp and execute M104 @@ -870,21 +859,20 @@ stat_t _parse_gcode_block(char *buf, char *active_comment) case 'A': SET_NON_MODAL (target[AXIS_A], value); case 'B': SET_NON_MODAL (target[AXIS_B], value); case 'C': SET_NON_MODAL (target[AXIS_C], value); - // case 'U': SET_NON_MODAL (target[AXIS_U], value); // reserved - // case 'V': SET_NON_MODAL (target[AXIS_V], value); // reserved - // case 'W': SET_NON_MODAL (target[AXIS_W], value); // reserved + case 'U': SET_NON_MODAL (target[AXIS_U], value); + case 'V': SET_NON_MODAL (target[AXIS_V], value); + case 'W': SET_NON_MODAL (target[AXIS_W], value); case 'H': SET_NON_MODAL (H_word, value); case 'I': SET_NON_MODAL (arc_offset[0], value); case 'J': SET_NON_MODAL (arc_offset[1], value); case 'K': SET_NON_MODAL (arc_offset[2], value); case 'L': SET_NON_MODAL (L_word, value); case 'R': SET_NON_MODAL (arc_radius, value); - case 'N': SET_NON_MODAL (linenum,(uint32_t)value); // line number - + case 'N': SET_NON_MODAL (linenum, value_int); // line number handled as special case to preserve integer value + #if MARLIN_COMPAT_ENABLED == true case 'E': SET_NON_MODAL (E_word, value); // extruder value #endif - default: status = STAT_GCODE_COMMAND_UNSUPPORTED; } if(status != STAT_OK) break; @@ -894,27 +882,29 @@ stat_t _parse_gcode_block(char *buf, char *active_comment) return (_execute_gcode_block(active_comment)); // if successful execute the block } -/* +/**************************************************************************************** * _execute_gcode_block() - execute parsed block * - * Conditionally (based on whether a flag is set in gf) call the canonical - * machining functions in order of execution as per RS274NGC_3 table 8 - * (below, with modifications): + * Conditionally (based on whether a flag is set in gf) call the canonical machining + * functions in order of execution. Derived from RS274NGC_3 table 8: * * 0. record the line number * 1. comment (includes message) [handled during block normalization] + * 1a. enable or disable overrides (M48, M49) + * 1b. set feed override rate (M50) + * 1c. set traverse override rate (M50.1) + * 1d. set spindle override rate (M51) * 2. set feed rate mode (G93, G94 - inverse time or per minute) * 3. set feed rate (F) * 3a. Marlin functions (optional) * 3b. set feed override rate (M50.1) * 3c. set traverse override rate (M50.2) * 4. set spindle speed (S) - * 4a. set spindle override rate (M51.1) * 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) + * // 9. enable or disable overrides (M48, M49, M50, M51) (see 1a) * 10. dwell (G4) * 11. set active plane (G17, G18, G19) * 12. set length units (G20, G21) @@ -931,7 +921,7 @@ stat_t _parse_gcode_block(char *buf, char *active_comment) * 20. perform motion (G0 to G3, G80-G89) as modified (possibly) by G53 * 21. stop and end (M0, M1, M2, M30, M60) * - * Values in gn are in original units and should not be unit converted prior + * Values in gv are in original units and should not be unit converted prior * to calling the canonical functions (which do the unit conversions) */ @@ -942,6 +932,19 @@ stat_t _execute_gcode_block(char *active_comment) if (gf.linenum) { cm_set_model_linenum(gv.linenum); } + + EXEC_FUNC(cm_m48_enable, m48_enable); + + if (gf.fro_control) { // feedrate override + ritorno(cm_fro_control(gv.P_word, gf.P_word)); + } + if (gf.tro_control) { // traverse override + ritorno(cm_tro_control(gv.P_word, gf.P_word)); + } + if (gf.spo_control) { // spindle speed override + ritorno(spindle_override_control(gv.P_word, gf.P_word)); + } + EXEC_FUNC(cm_set_feed_rate_mode, feed_rate_mode); // G93, G94 EXEC_FUNC(cm_set_feed_rate, F_word); // F @@ -949,27 +952,23 @@ stat_t _execute_gcode_block(char *active_comment) if (gf.linenum && gf.checksum) { ritorno(cm_check_linenum()); } + + EXEC_FUNC(spindle_speed_sync, S_word); // S + EXEC_FUNC(cm_select_tool, tool_select); // T - tool_select is where it's written + EXEC_FUNC(cm_change_tool, tool_change); // M6 - is where it's effected - EXEC_FUNC(cm_set_spindle_speed, S_word); // S - if (gf.sso_control) { // spindle speed override - ritorno(cm_sso_control(gv.P_word, gf.P_word)); + if (gf.spindle_control) { // M3, M4, M5 (spindle OFF, CW, CCW) + ritorno(spindle_control_sync((spControl)gv.spindle_control)); } - - EXEC_FUNC(cm_select_tool, tool_select); // tool_select is where it's written - EXEC_FUNC(cm_change_tool, tool_change); // M6 - EXEC_FUNC(cm_spindle_control, spindle_control); // spindle CW, CCW, OFF - - EXEC_FUNC(cm_mist_coolant_control, mist_coolant); // M7, M9 - EXEC_FUNC(cm_flood_coolant_control, flood_coolant); // M8, M9 also disables mist coolant if OFF - EXEC_FUNC(cm_m48_enable, m48_enable); - - if (gf.mfo_control) { // manual feedrate override - ritorno(cm_mfo_control(gv.P_word, gf.P_word)); + if (gf.coolant_mist) { + ritorno(coolant_control_sync((coControl)gv.coolant_mist, COOLANT_MIST)); // M7 } - if (gf.mto_control) { // manual traverse override - ritorno(cm_mto_control(gv.P_word, gf.P_word)); + if (gf.coolant_flood) { + ritorno(coolant_control_sync((coControl)gv.coolant_flood, COOLANT_FLOOD)); // M8 + } + if (gf.coolant_off) { + ritorno(coolant_control_sync((coControl)gv.coolant_off, COOLANT_BOTH)); // M9 } - if (gv.next_action == NEXT_ACTION_DWELL) { // G4 - dwell ritorno(cm_dwell(gv.P_word)); // return if error, otherwise complete the block } @@ -990,9 +989,7 @@ stat_t _execute_gcode_block(char *active_comment) ritorno(cm_cancel_tl_offset()); break; } - default: - // quiet the compiler warning about all the things we don't handle - break; + default: {} // quiet the compiler warning about all the things we don't handle here } EXEC_FUNC(cm_set_coord_system, coord_system); // G54, G55, G56, G57, G58, G59 @@ -1020,22 +1017,25 @@ stat_t _execute_gcode_block(char *active_comment) case NEXT_ACTION_STRAIGHT_PROBE_AWAY_ERR:{ status = cm_straight_probe(gv.target, gf.target, false, true); break;} // G38.4 case NEXT_ACTION_STRAIGHT_PROBE_AWAY: { status = cm_straight_probe(gv.target, gf.target, false, false); break;}// G38.5 - case NEXT_ACTION_SET_G10_DATA: { status = cm_set_g10_data(gv.P_word, gf.P_word, gv.L_word, gf.L_word, gv.target, gf.target); break;} // G10 - case NEXT_ACTION_SET_ORIGIN_OFFSETS: { status = cm_set_origin_offsets(gv.target, gf.target); break;} // G92 - case NEXT_ACTION_RESET_ORIGIN_OFFSETS: { status = cm_reset_origin_offsets(); break;} // G92.1 - case NEXT_ACTION_SUSPEND_ORIGIN_OFFSETS: { status = cm_suspend_origin_offsets(); break;} // G92.2 - case NEXT_ACTION_RESUME_ORIGIN_OFFSETS: { status = cm_resume_origin_offsets(); break;} // G92.3 + case NEXT_ACTION_SET_G10_DATA: { status = cm_set_g10_data(gv.P_word, gf.P_word, // G10 + gv.L_word, gf.L_word, + gv.target, gf.target); break;} + + case NEXT_ACTION_SET_G92_OFFSETS: { status = cm_set_g92_offsets(gv.target, gf.target); break;} // G92 + case NEXT_ACTION_RESET_G92_OFFSETS: { status = cm_reset_g92_offsets(); break;} // G92.1 + case NEXT_ACTION_SUSPEND_G92_OFFSETS: { status = cm_suspend_g92_offsets(); break;} // G92.2 + case NEXT_ACTION_RESUME_G92_OFFSETS: { status = cm_resume_g92_offsets(); break;} // G92.3 case NEXT_ACTION_JSON_COMMAND_SYNC: { status = cm_json_command(active_comment); break;} // M100.0 case NEXT_ACTION_JSON_COMMAND_ASYNC: { status = cm_json_command_immediate(active_comment); break;} // M100.1 case NEXT_ACTION_JSON_WAIT: { status = cm_json_wait(active_comment); break;} // M101 - + case NEXT_ACTION_DEFAULT: { - cm_set_absolute_override(MODEL, gv.absolute_override); // apply absolute override + cm_set_absolute_override(MODEL, gv.absolute_override); // apply absolute override & display as absolute switch (gv.motion_mode) { - case MOTION_MODE_CANCEL_MOTION_MODE: { cm.gm.motion_mode = gv.motion_mode; break;} // G80 - case MOTION_MODE_STRAIGHT_TRAVERSE: { status = cm_straight_traverse(gv.target, gf.target); break;} // G0 - case MOTION_MODE_STRAIGHT_FEED: { status = cm_straight_feed(gv.target, gf.target); break;} // G1 + case MOTION_MODE_CANCEL_MOTION_MODE: { cm->gm.motion_mode = gv.motion_mode; break;} // G80 + case MOTION_MODE_STRAIGHT_TRAVERSE: { status = cm_straight_traverse(gv.target, gf.target, PROFILE_NORMAL); break;} // G0 + case MOTION_MODE_STRAIGHT_FEED: { status = cm_straight_feed(gv.target, gf.target, PROFILE_NORMAL); break;} // G1 case MOTION_MODE_CW_ARC: // G2 case MOTION_MODE_CCW_ARC: { status = cm_arc_feed(gv.target, gf.target, // G3 gv.arc_offset, gf.arc_offset, @@ -1043,11 +1043,11 @@ stat_t _execute_gcode_block(char *active_comment) gv.P_word, gf.P_word, gp.modals[MODAL_GROUP_G1], gv.motion_mode); - break; + break; } default: break; } - cm_set_absolute_override(MODEL, ABSOLUTE_OVERRIDE_OFF); // un-set absolute override once the move is planned + cm_set_absolute_override(MODEL, ABSOLUTE_OVERRIDE_OFF); // un-set absolute override once the move is planned } default: // quiet the compiler warning about all the things we don't handle @@ -1066,7 +1066,7 @@ stat_t _execute_gcode_block(char *active_comment) } /*********************************************************************************** - * _execute_gcode_block_marlin() - collect Marlin exection functions here + * _execute_gcode_block_marlin() - collect Marlin Gcode execution functions here */ static stat_t _execute_gcode_block_marlin() @@ -1077,7 +1077,7 @@ static stat_t _execute_gcode_block_marlin() if (gv.next_action != NEXT_ACTION_MARLIN_RESET_LINE_NUMBERS) { ritorno(cm_check_linenum()); } - cm.gmx.last_line_number = cm.gm.linenum; + cm->gmx.last_line_number = cm->gm.linenum; // since this is handled, clear gf.checksum so it doesn't again gf.checksum = false; @@ -1091,13 +1091,13 @@ static stat_t _execute_gcode_block_marlin() // adjust T real quick if (mst.marlin_flavor && gf.tool_select) { gv.tool_select += 1; - cm.gm.tool_select = gv.tool_select; // We need to go ahead and apply to tool select, and in Marlin 0 is valid, so add 1 - cm.gm.tool = cm.gm.tool_select; // Also, in Marlin, tool changes are effective immediately :facepalm: + cm->gm.tool_select = gv.tool_select; // We need to go ahead and apply to tool select, and in Marlin 0 is valid, so add 1 + cm->gm.tool = cm->gm.tool_select; // Also, in Marlin, tool changes are effective immediately :facepalm: gf.tool_select = false; // prevent a tool_select command from being buffered (planning to zero) } - else if (cm.gm.tool_select == 0) { - cm.gm.tool_select = 1; // We need to ensure we have a valid tool selected, often Marlin gcode won't have a T word at all - cm.gm.tool = cm.gm.tool_select; // Also, in Marlin, tool changes are effective immediately :facepalm: + else if (cm->gm.tool_select == 0) { + cm->gm.tool_select = 1; // We need to ensure we have a valid tool selected, often Marlin gcode won't have a T word at all + cm->gm.tool = cm->gm.tool_select; // Also, in Marlin, tool changes are effective immediately :facepalm: } if (mst.marlin_flavor && @@ -1114,17 +1114,17 @@ static stat_t _execute_gcode_block_marlin() } if (gf.E_word) { // Ennn T0 -> Annn - if (cm.gm.tool_select == 1) { + if (cm->gm.tool_select == 1) { gf.target[AXIS_A] = true; gv.target[AXIS_A] = gv.E_word; } // Ennn T1 -> Bnnn - else if (cm.gm.tool_select == 2) { + else if (cm->gm.tool_select == 2) { gf.target[AXIS_B] = true; gv.target[AXIS_B] = gv.E_word; } else { - _debug_trap("invalid tool selection"); + debug_trap("invalid tool selection"); return (STAT_INPUT_VALUE_RANGE_ERROR); } } @@ -1152,7 +1152,7 @@ static stat_t _execute_gcode_block_marlin() if (gf.S_word) { temp = gv.S_word; } if (gf.P_word) { temp = gv.P_word; } // we treat them the same, for now - uint8_t tool = (gv.next_action == NEXT_ACTION_MARLIN_SET_EXTRUDER_TEMP) ? cm.gm.tool_select : 3; + uint8_t tool = (gv.next_action == NEXT_ACTION_MARLIN_SET_EXTRUDER_TEMP) ? cm->gm.tool_select : 3; ritorno(marlin_set_temperature(tool, temp, gf.marlin_wait_for_temp)); gf.P_word = false; @@ -1161,7 +1161,7 @@ static stat_t _execute_gcode_block_marlin() } case NEXT_ACTION_MARLIN_CANCEL_WAIT_TEMP: { // M108 js.json_mode = MARLIN_COMM_MODE; // we use M105 to know when to switch - cm_request_feedhold(); + cm_request_feedhold(FEEDHOLD_TYPE_HOLD, FEEDHOLD_EXIT_STOP); cm_request_queue_flush(); break; } diff --git a/g2core/gpio.cpp b/g2core/gpio.cpp index 8a5b8f60..02bf650f 100644 --- a/g2core/gpio.cpp +++ b/g2core/gpio.cpp @@ -2,8 +2,8 @@ * gpio.cpp - digital IO handling functions * This file is part of the g2core project * - * Copyright (c) 2015 - 2107 Alden S. Hart, Jr. - * Copyright (c) 2015 - 2017 Robert Giseburt + * Copyright (c) 2015 - 2019 Alden S. Hart, Jr. + * Copyright (c) 2015 - 2019 Robert Giseburt * * This file ("the software") is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License, version 2 as published by the @@ -389,24 +389,24 @@ stat_t ai_set_p5(nvObj_t *nv) { return ai_set_parameter(nv, 4); }; static const char fmt_gpio_ac[] = "[%sac] input action%15d [0=none,1=stop,2=fast_stop,3=halt,4=alarm,5=shutdown,6=panic,7=reset]\n"; static const char fmt_gpio_fn[] = "[%sfn] input function%13d [0=none,1=limit,2=interlock,3=shutdown,4=probe]\n"; static const char fmt_gpio_in[] = "[%sin] input external number%6d [0=none,1-16=inX shows the value of this din]\n"; - static const char fmt_gpio_state[] = "Input %s state: %5d\n"; + static const char fmt_gpio_state[] = "Input %s state: %5ld\n"; static const char fmt_gpio_out_en[] = "[%smo] output enabled%12d [-1=unavailable,0=disabled,1=enabled]\n"; static const char fmt_gpio_out_po[] = "[%smo] output polarity%12d [0=normal/active-high,1=inverted/active-low]\n"; static const char fmt_gpio_out_out[] = "[%sout] output external number%5d [0=none,1-16=outX shows the value of this dout]\n"; - static const char fmt_gpio_out_state[] = "Output %s state: %5d\n"; + static const char fmt_gpio_out_state[] = "Output %s state: %5ld\n"; static const char fmt_ain_value[] = "Analog input %s voltage: %5.2fV\n"; static const char fmt_ain_resistance[] = "Analog input %s resistance: %5.2fohm\n"; static const char fmt_gpio_ai_en[] = "[%smo] analog input enabled%12d [-1=unavailable,0=disabled,1=enabled]\n"; static const char fmt_gpio_ai_ain[] = "[%sout] analog input external number%5d [0=none,1-8=ainX shows the value of this ai]\n"; - static const char fmt_ai_type[] = "[%s] input type%17d [0=disabled,1=internal,2=external]\n"; - static const char fmt_ai_circuit[] = "[%s] analog circuit%13d [0=disabled,1=pull-up,2=external,3=inverted op-amp,4=constant current inverted op-amp]\n"; + static const char fmt_ai_type[] = "[%s] input type%17ld [0=disabled,1=internal,2=external]\n"; + static const char fmt_ai_circuit[] = "[%s] analog circuit%13ld [0=disabled,1=pull-up,2=external,3=inverted op-amp,4=constant current inverted op-amp]\n"; static const char fmt_ai_parameter[] = "[%s] circuit parameter%6.4f [usage varies by circuit type]\n"; static void _print_di(nvObj_t *nv, const char *format) { - sprintf(cs.out_buf, format, nv->group, (int)nv->value); + sprintf(cs.out_buf, format, nv->group, nv->value_int); xio_writeline(cs.out_buf); } void din_print_en(nvObj_t *nv) {_print_di(nv, fmt_gpio_in_en);} @@ -415,7 +415,7 @@ stat_t ai_set_p5(nvObj_t *nv) { return ai_set_parameter(nv, 4); }; void din_print_fn(nvObj_t *nv) {_print_di(nv, fmt_gpio_fn);} void din_print_in(nvObj_t *nv) {_print_di(nv, fmt_gpio_in);} void din_print_state(nvObj_t *nv) { - sprintf(cs.out_buf, fmt_gpio_state, nv->token, (int)nv->value); + sprintf(cs.out_buf, fmt_gpio_state, nv->token, nv->value_int); xio_writeline(cs.out_buf); } @@ -423,37 +423,37 @@ stat_t ai_set_p5(nvObj_t *nv) { return ai_set_parameter(nv, 4); }; void dout_print_po(nvObj_t *nv) {_print_di(nv, fmt_gpio_out_po);} void dout_print_out(nvObj_t *nv) {_print_di(nv, fmt_gpio_out_out);} void dout_print_out_state(nvObj_t *nv) { - sprintf(cs.out_buf, fmt_gpio_out_state, nv->token, (int)nv->value); + sprintf(cs.out_buf, fmt_gpio_out_state, nv->token, nv->value_int); xio_writeline(cs.out_buf); } void ain_print_value(nvObj_t *nv) { - sprintf(cs.out_buf, fmt_ain_value, nv->token, (float)nv->value); + sprintf(cs.out_buf, fmt_ain_value, nv->token, nv->value_flt); xio_writeline(cs.out_buf); } void ain_print_resistance(nvObj_t *nv) { - sprintf(cs.out_buf, fmt_ain_resistance, nv->token, (float)nv->value); + sprintf(cs.out_buf, fmt_ain_resistance, nv->token, nv->value_flt); xio_writeline(cs.out_buf); } void ai_print_en(nvObj_t *nv) {_print_di(nv, fmt_gpio_ai_en);} void ai_print_ain(nvObj_t *nv) {_print_di(nv, fmt_gpio_ai_ain);} void ai_print_type(nvObj_t *nv) { - sprintf(cs.out_buf, fmt_ai_type, nv->token, (int)nv->value); + sprintf(cs.out_buf, fmt_ai_type, nv->token, nv->value_int); xio_writeline(cs.out_buf); } void ai_print_circuit(nvObj_t *nv) { - sprintf(cs.out_buf, fmt_ai_circuit, nv->token, (int)nv->value); + sprintf(cs.out_buf, fmt_ai_circuit, nv->token, nv->value_int); xio_writeline(cs.out_buf); } void ai_print_p(nvObj_t *nv) { - sprintf(cs.out_buf, fmt_ai_parameter, nv->token, (float)nv->value); + sprintf(cs.out_buf, fmt_ai_parameter, nv->token, nv->value_flt); xio_writeline(cs.out_buf); } diff --git a/g2core/gpio.h b/g2core/gpio.h index 246afc59..10ba279a 100644 --- a/g2core/gpio.h +++ b/g2core/gpio.h @@ -2,8 +2,8 @@ * gpio.h - Digital IO handling functions * This file is part of the g2core project * - * Copyright (c) 2015 - 2017 Alden S. Hart, Jr. - * Copyright (c) 2015 - 2017 Robert Giseburt + * Copyright (c) 2015 - 2019 Alden S. Hart, Jr. + * Copyright (c) 2015 - 2019 Robert Giseburt * * This file ("the software") is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License, version 2 as published by the @@ -238,24 +238,24 @@ struct gpioDigitalInput { nv->valuetype = TYPE_NULL; return (STAT_OK); } - nv->value = getState(); - nv->valuetype = TYPE_BOOL; + nv->value_int = getState(); + nv->valuetype = TYPE_BOOLEAN; return (STAT_OK); }; // no setState stat_t getEnabled(nvObj_t *nv) { - nv->value = getEnabled(); - nv->valuetype = TYPE_INT; + nv->value_int = getEnabled(); + nv->valuetype = TYPE_INTEGER; return (STAT_OK); }; stat_t setEnabled(nvObj_t *nv) { - if ((nv->value < IO_DISABLED) || (nv->value > IO_ENABLED)) { + if ((nv->value_int < IO_DISABLED) || (nv->value_int > IO_ENABLED)) { return (STAT_INPUT_VALUE_RANGE_ERROR); } - if (!setEnabled((ioEnabled)nv->value)) { + if (!setEnabled((ioEnabled)nv->value_int)) { return STAT_PARAMETER_IS_READ_ONLY; } return (STAT_OK); @@ -263,16 +263,16 @@ struct gpioDigitalInput { stat_t getPolarity(nvObj_t *nv) { - nv->value = getPolarity(); - nv->valuetype = TYPE_INT; + nv->value_int = getPolarity(); + nv->valuetype = TYPE_INTEGER; return (STAT_OK); }; stat_t setPolarity(nvObj_t *nv) { - if ((nv->value < IO_ACTIVE_HIGH) || (nv->value > IO_ACTIVE_LOW)) { + if ((nv->value_int < IO_ACTIVE_HIGH) || (nv->value_int > IO_ACTIVE_LOW)) { return (STAT_INPUT_VALUE_RANGE_ERROR); } - if (!setPolarity((ioPolarity)nv->value)) { + if (!setPolarity((ioPolarity)nv->value_int)) { return STAT_PARAMETER_IS_READ_ONLY; } return (STAT_OK); @@ -280,16 +280,16 @@ struct gpioDigitalInput { stat_t getAction(nvObj_t *nv) { - nv->value = getAction(); - nv->valuetype = TYPE_INT; + nv->value_int = getAction(); + nv->valuetype = TYPE_INTEGER; return (STAT_OK); }; stat_t setAction(nvObj_t *nv) { - if ((nv->value < INPUT_ACTION_NONE) || (nv->value > INPUT_ACTION_MAX)) { + if ((nv->value_int < INPUT_ACTION_NONE) || (nv->value_int > INPUT_ACTION_MAX)) { return (STAT_INPUT_VALUE_RANGE_ERROR); } - if (!setAction((inputAction)nv->value)) { + if (!setAction((inputAction)nv->value_int)) { return STAT_PARAMETER_IS_READ_ONLY; } return (STAT_OK); @@ -298,16 +298,16 @@ struct gpioDigitalInput { stat_t getExternalNumber(nvObj_t *nv) { - nv->value = getExternalNumber(); - nv->valuetype = TYPE_INT; + nv->value_int = getExternalNumber(); + nv->valuetype = TYPE_INTEGER; return (STAT_OK); }; stat_t setExternalNumber(nvObj_t *nv) { - if ((nv->value < 0) || (nv->value > 14)) { + if ((nv->value_int < 0) || (nv->value_int > 14)) { return (STAT_INPUT_VALUE_RANGE_ERROR); } - if (!setExternalNumber(nv->value)) { + if (!setExternalNumber(nv->value_int)) { return STAT_PARAMETER_IS_READ_ONLY; } return (STAT_OK); @@ -555,17 +555,17 @@ struct gpioDigitalOutput { stat_t getEnabled(nvObj_t *nv) { - nv->value = getEnabled(); - nv->valuetype = TYPE_INT; + nv->value_int = getEnabled(); + nv->valuetype = TYPE_INTEGER; return (STAT_OK); }; stat_t setEnabled(nvObj_t *nv) { - int32_t value = nv->value; + int32_t value = nv->value_int; if ((value != IO_DISABLED) && (value != IO_ENABLED)) { return (STAT_INPUT_VALUE_RANGE_ERROR); } - if (!setEnabled((ioEnabled)nv->value)) { + if (!setEnabled((ioEnabled)nv->value_int)) { return STAT_PARAMETER_IS_READ_ONLY; } return (STAT_OK); @@ -573,16 +573,16 @@ struct gpioDigitalOutput { stat_t getPolarity(nvObj_t *nv) { - nv->value = getPolarity(); - nv->valuetype = TYPE_INT; + nv->value_int = getPolarity(); + nv->valuetype = TYPE_INTEGER; return (STAT_OK); }; stat_t setPolarity(nvObj_t *nv) { - if ((nv->value < IO_ACTIVE_HIGH) || (nv->value > IO_ACTIVE_LOW)) { + if ((nv->value_int < IO_ACTIVE_HIGH) || (nv->value_int > IO_ACTIVE_LOW)) { return (STAT_INPUT_VALUE_RANGE_ERROR); } - if (!setPolarity((ioPolarity)nv->value)) { + if (!setPolarity((ioPolarity)nv->value_int)) { return STAT_PARAMETER_IS_READ_ONLY; } return (STAT_OK); @@ -592,16 +592,16 @@ struct gpioDigitalOutput { { auto enabled = getEnabled(); if (enabled != IO_ENABLED) { - nv->value = 0; + nv->value_int = 0; nv->valuetype = TYPE_NULL; // reports back as NULL } else { nv->valuetype = TYPE_FLOAT; nv->precision = 2; - nv->value = getValue(); // read it as a float + nv->value_flt = getValue(); // read it as a float bool invert = (getPolarity() == IO_ACTIVE_LOW); if (invert) { - nv->value = 1.0 - nv->value; + nv->value_flt = 1.0 - nv->value_flt; } } return (STAT_OK); @@ -612,7 +612,7 @@ struct gpioDigitalOutput { if (enabled != IO_ENABLED) { nv->valuetype = TYPE_NULL; // reports back as NULL } else { - float value = nv->value; // read it as a float + float value = nv->value_flt; // read it as a float bool invert = (getPolarity() == IO_ACTIVE_LOW); if (invert) { @@ -628,16 +628,16 @@ struct gpioDigitalOutput { stat_t getExternalNumber(nvObj_t *nv) { - nv->value = getExternalNumber(); - nv->valuetype = TYPE_INT; + nv->value_int = getExternalNumber(); + nv->valuetype = TYPE_INTEGER; return (STAT_OK); }; stat_t setExternalNumber(nvObj_t *nv) { - if ((nv->value < 0) || (nv->value > 14)) { + if ((nv->value_int < 0) || (nv->value_int > 14)) { return (STAT_INPUT_VALUE_RANGE_ERROR); } - if (!setExternalNumber(nv->value)) { + if (!setExternalNumber(nv->value_int)) { return STAT_PARAMETER_IS_READ_ONLY; } return (STAT_OK); @@ -678,7 +678,7 @@ struct gpioDigitalOutputWriter final { stat_t getValue(nvObj_t *nv) { if (!pin) { - nv->value = 0; + nv->value_int = 0; nv->valuetype = TYPE_NULL; // reports back as NULL return (STAT_OK); } @@ -881,17 +881,17 @@ struct gpioAnalogInput { stat_t getEnabled(nvObj_t *nv) { - nv->value = getEnabled(); - nv->valuetype = TYPE_INT; + nv->value_int = getEnabled(); + nv->valuetype = TYPE_INTEGER; return (STAT_OK); }; stat_t setEnabled(nvObj_t *nv) { - int32_t value = nv->value; + int32_t value = nv->value_int; if ((value != IO_DISABLED) && (value != IO_ENABLED)) { return (STAT_INPUT_VALUE_RANGE_ERROR); } - if (!setEnabled((ioEnabled)nv->value)) { + if (!setEnabled((ioEnabled)nv->value_int)) { return STAT_PARAMETER_IS_READ_ONLY; } return (STAT_OK); @@ -903,7 +903,7 @@ struct gpioAnalogInput { nv->valuetype = TYPE_NULL; return (STAT_OK); } - nv->value = getValue(); + nv->value_flt = getValue(); nv->valuetype = TYPE_FLOAT; return (STAT_OK); }; @@ -915,7 +915,7 @@ struct gpioAnalogInput { nv->valuetype = TYPE_NULL; return (STAT_OK); } - nv->value = getResistance(); + nv->value_flt = getResistance(); nv->valuetype = TYPE_FLOAT; return (STAT_OK); }; @@ -923,16 +923,16 @@ struct gpioAnalogInput { stat_t getType(nvObj_t *nv) { - nv->value = getType(); - nv->valuetype = TYPE_INT; + nv->value_int = getType(); + nv->valuetype = TYPE_INTEGER; return (STAT_OK); }; stat_t setType(nvObj_t *nv) { - if ((getEnabled() != IO_ENABLED) || (nv->value > AIN_TYPE_EXTERNAL)) { + if ((getEnabled() != IO_ENABLED) || (nv->value_int > AIN_TYPE_EXTERNAL)) { return (STAT_INPUT_VALUE_RANGE_ERROR); } - if (!setType((AnalogInputType_t)nv->value)) { + if (!setType((AnalogInputType_t)nv->value_int)) { return STAT_PARAMETER_IS_READ_ONLY; } return (STAT_OK); @@ -940,16 +940,16 @@ struct gpioAnalogInput { stat_t getCircuit(nvObj_t *nv) { - nv->value = getCircuit(); - nv->valuetype = TYPE_INT; + nv->value_int = getCircuit(); + nv->valuetype = TYPE_INTEGER; return (STAT_OK); }; stat_t setCircuit(nvObj_t *nv) { - if ((nv->value < AIN_CIRCUIT_DISABLED) || (nv->value > AIN_CIRCUIT_MAX)) { + if ((nv->value_int < AIN_CIRCUIT_DISABLED) || (nv->value_int > AIN_CIRCUIT_MAX)) { return (STAT_INPUT_VALUE_RANGE_ERROR); } - if (!setCircuit((AnalogCircuit_t)nv->value)) { + if (!setCircuit((AnalogCircuit_t)nv->value_int)) { return STAT_PARAMETER_IS_READ_ONLY; } return (STAT_OK); @@ -957,13 +957,13 @@ struct gpioAnalogInput { stat_t getParameter(nvObj_t *nv, const uint8_t p) { - nv->value = getParameter(p); + nv->value_flt = getParameter(p); nv->valuetype = TYPE_FLOAT; return (STAT_OK); }; stat_t setParameter(nvObj_t *nv, const uint8_t p) { - if (!setParameter(p, nv->value)) { + if (!setParameter(p, nv->value_int)) { return STAT_PARAMETER_IS_READ_ONLY; } return (STAT_OK); @@ -972,16 +972,16 @@ struct gpioAnalogInput { stat_t getExternalNumber(nvObj_t *nv) { - nv->value = getExternalNumber(); - nv->valuetype = TYPE_INT; + nv->value_int = getExternalNumber(); + nv->valuetype = TYPE_INTEGER; return (STAT_OK); }; stat_t setExternalNumber(nvObj_t *nv) { - if ((nv->value < 0) || (nv->value > 14)) { + if ((nv->value_int < 0) || (nv->value_int > 14)) { return (STAT_INPUT_VALUE_RANGE_ERROR); } - if (!setExternalNumber(nv->value)) { + if (!setExternalNumber(nv->value_int)) { return STAT_PARAMETER_IS_READ_ONLY; } return (STAT_OK); @@ -1022,7 +1022,7 @@ struct gpioAnalogInputReader final { stat_t getValue(nvObj_t *nv) { if (!pin) { - nv->value = 0; + nv->value_int = 0; nv->valuetype = TYPE_NULL; // reports back as NULL return (STAT_OK); } @@ -1030,7 +1030,7 @@ struct gpioAnalogInputReader final { }; stat_t getResistance(nvObj_t *nv) { if (!pin) { - nv->value = 0; + nv->value_int = 0; nv->valuetype = TYPE_NULL; // reports back as NULL return (STAT_OK); } diff --git a/g2core/help.cpp b/g2core/help.cpp old mode 100755 new mode 100644 index 6c39190c..7463aba6 --- a/g2core/help.cpp +++ b/g2core/help.cpp @@ -2,7 +2,7 @@ * help.cpp - collected help routines * This file is part of the g2core project * - * Copyright (c) 2010 - 2016 Alden S. Hart, Jr. + * Copyright (c) 2010 - 2018 Alden S. Hart, Jr. * * This file ("the software") is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License, version 2 as published by the diff --git a/g2core/help.h b/g2core/help.h old mode 100755 new mode 100644 index 1efc14c5..d192f614 --- a/g2core/help.h +++ b/g2core/help.h @@ -2,7 +2,7 @@ * help.h - collected help and assorted display routines * This file is part of the g2core project * - * Copyright (c) 2010 - 2016 Alden S. Hart, Jr. + * Copyright (c) 2010 - 2018 Alden S. Hart, Jr. * * This file ("the software") is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License, version 2 as published by the diff --git a/g2core/json_parser.cpp b/g2core/json_parser.cpp index 531df60a..ff11b7fe 100644 --- a/g2core/json_parser.cpp +++ b/g2core/json_parser.cpp @@ -2,8 +2,8 @@ * json_parser.cpp - JSON parser * This file is part of the g2core project * - * Copyright (c) 2011 - 2016 Alden S. Hart, Jr. - * Copyright (c) 2016 Rob Giseburt + * Copyright (c) 2011 - 2019 Alden S. Hart, Jr. + * Copyright (c) 2016 - 2019 Rob Giseburt * * This file ("the software") is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License, version 2 as published by the @@ -96,7 +96,7 @@ stat_t json_parser(char *str, bool suppress_response) // suppress_response defau if (suppress_response || (status == STAT_COMPLETE)) { // skip the print if returning from something that already did it. return status; } - nv_print_list(status, TEXT_NO_PRINT, JSON_RESPONSE_FORMAT); + nv_print_list(status, TEXT_MULTILINE_FORMATTED, JSON_RESPONSE_FORMAT); sr_request_status_report(SR_REQUEST_TIMED); // generate incremental status report to show any changes return STAT_OK; } @@ -107,26 +107,29 @@ void json_parse_for_exec(char *str, bool execute) nvObj_t *nv = nv_reset_exec_nv_list(); // get a fresh nvObj list stat_t status = _json_parser_kernal(nv, str); if ((status == STAT_OK) && (execute)) { - // execute the command nv = nv_exec; - status = _json_parser_execute(nv); + status = _json_parser_execute(nv); // execute the command } } static stat_t _json_parser_execute(nvObj_t *nv) { do { - if (nv->valuetype == TYPE_PARENT) { - // anything? - } else if (nv->valuetype == TYPE_NULL) { // means GET the value + if (nv->valuetype == TYPE_PARENT) { // added as partial fix for Issue #298: + // Reading values with nested JSON changes values in inches mode + if (strcmp(nv->token, "sr") == 0) { // Hack to execute Set Status Report (SR parent) See end note (*) + return (nv_set(nv)); + } + + } else if (nv->valuetype == TYPE_NULL) { // means run the GET function to get the value ritorno(nv_get(nv)); // ritorno returns w/status on any errors if (nv->valuetype == TYPE_PARENT) { // This will be true if you read a group. Exit now return (STAT_OK); } - } else { + } else { // otherwise, run the SET function cm_parse_clear(*nv->stringp); // parse Gcode and clear alarms if M30 or M2 is found ritorno(cm_is_alarmed()); // return error status if in alarm, shutdown or panic - ritorno(nv_set(nv)); // set value or call a function (e.g. gcode) + ritorno(nv_set(nv)); // run the SET function to set value or execute something (e.g. gcode) nv_persist(nv); } if ((nv = nv->nx) == NULL) { @@ -137,6 +140,10 @@ static stat_t _json_parser_execute(nvObj_t *nv) { return (STAT_OK); // only successful commands exit through this point } +// (*) Note: The JSON / token system is essentially flat, as it was derived from a command-line flat-ASCII approach +// If the JSON objects had proper recursive descent handlers that just passed the remaining string (at that level) +// off for further processing, we would not need to do this hack. A fix is in the works. For now, this is OK. + static stat_t _json_parser_kernal(nvObj_t *nv, char *str) { stat_t status; @@ -173,6 +180,7 @@ static stat_t _json_parser_kernal(nvObj_t *nv, char *str) if ((nv_index_is_group(nv->index)) && (nv_group_is_prefixed(nv->token))) { strncpy(group, nv->token, GROUP_LEN); // record the group ID } + nv_coerce_types(nv); // adjust types based on type fields in configApp table if ((nv = nv->nx) == NULL) { return (STAT_JSON_TOO_MANY_PAIRS); // Not supposed to encounter a NULL } @@ -288,11 +296,12 @@ static stat_t _get_nv_pair(nvObj_t *nv, char **pstr, int8_t *depth) // nulls (gets) if ((**pstr == 'n') || ((**pstr == '\"') && (*(*pstr+1) == '\"'))) { // process null value nv->valuetype = TYPE_NULL; - nv->value = TYPE_NULL; + nv->value_int = TYPE_NULL; // numbers } else if (isdigit(**pstr) || (**pstr == '-')) { // value is a number - nv->value = (float)strtod(*pstr, &tmp); // tmp is the end pointer + nv->value_int = atol(*pstr); // get the number as an integer + nv->value_flt = (float)strtod(*pstr, &tmp); // get the number as a float - tmp is the end pointer if ((tmp == *pstr) || // if start pointer equals end the conversion failed (strchr(terminators, *tmp) == NULL)) { // terminators are the only legal chars at the end of a number @@ -320,7 +329,7 @@ static stat_t _get_nv_pair(nvObj_t *nv, char **pstr, int8_t *depth) // if string begins with 0x it might be data, needs to be at least 3 chars long if( strlen(*pstr)>=3 && (*pstr)[0]=='0' && (*pstr)[1]=='x') { - uint32_t *v = (uint32_t*)&nv->value; + uint32_t *v = (uint32_t*)&nv->value_flt; *v = strtoul((const char *)*pstr, 0L, 0); nv->valuetype = TYPE_DATA; } else { @@ -330,11 +339,11 @@ static stat_t _get_nv_pair(nvObj_t *nv, char **pstr, int8_t *depth) // boolean true/false } else if (**pstr == 't') { - nv->valuetype = TYPE_BOOL; - nv->value = true; + nv->valuetype = TYPE_BOOLEAN; + nv->value_int = true; } else if (**pstr == 'f') { - nv->valuetype = TYPE_BOOL; - nv->value = false; + nv->valuetype = TYPE_BOOLEAN; + nv->value_int = false; // arrays } else if (**pstr == '[') { @@ -422,12 +431,15 @@ uint16_t json_serialize(nvObj_t *nv, char *out_buf, uint16_t size) need_a_comma = false; break; } - case (TYPE_FLOAT): { preprocess_float(nv); - str += floattoa(str, nv->value, nv->precision); + case (TYPE_FLOAT): { convert_outgoing_float(nv); + str += floattoa(str, nv->value_flt, nv->precision); break; } - case (TYPE_INT): { // str += inttoa(str, (int)nv->value); // doesn't handle negative numbers - str += sprintf(str, "%d", (int)nv->value); +// case (TYPE_INT): { // str += inttoa(str, (int)nv->value); // doesn't handle negative numbers +// str += sprintf(str, "%d", (int)nv->value); +// break; +// } + case (TYPE_INTEGER):{ str += sprintf(str, "%d", (int)nv->value_int); break; } case (TYPE_STRING): { *str++ = '"'; @@ -436,7 +448,7 @@ uint16_t json_serialize(nvObj_t *nv, char *out_buf, uint16_t size) *str++ = '"'; break; } - case (TYPE_BOOL): { if (fp_FALSE(nv->value)) { + case (TYPE_BOOLEAN):{ if (nv->value_int) { strcpy(str, "false"); str += 5; } else { @@ -445,7 +457,7 @@ uint16_t json_serialize(nvObj_t *nv, char *out_buf, uint16_t size) } break; } - case (TYPE_DATA): { uint32_t *v = (uint32_t*)&nv->value; + case (TYPE_DATA): { uint32_t *v = (uint32_t*)&nv->value_flt; str += sprintf(str, "\"0x%lx\"", *v); break; } @@ -455,6 +467,7 @@ uint16_t json_serialize(nvObj_t *nv, char *out_buf, uint16_t size) strcpy(str++, "]"); break; } + default: {} } } if (str >= str_max) { return (-1);} // signal buffer overrun @@ -532,7 +545,7 @@ void json_print_response(uint8_t status, const bool only_to_muted /*= false*/) } if (js.json_verbosity == JV_EXCEPTIONS) { // cutout for JV_EXCEPTIONS mode if (status == STAT_OK) { - if (cm.machine_state != MACHINE_INITIALIZING) { // always do full echo during startup + if (cm->machine_state != MACHINE_INITIALIZING) { // always do full echo during startup return; } } @@ -544,7 +557,7 @@ void json_print_response(uint8_t status, const bool only_to_muted /*= false*/) nv_reset_nv_list(); nv_add_string((const char *)"err", escape_string(cs.bufp, cs.saved_buf)); - } else if ((cm.machine_state != MACHINE_INITIALIZING) || (status == STAT_INITIALIZING)) { // always do full echo during startup + } else if ((cm->machine_state != MACHINE_INITIALIZING) || (status == STAT_INITIALIZING)) { // always do full echo during startup uint8_t nv_type; do { if ((nv_type = nv_get_type(nv)) == NV_TYPE_NULL) break; @@ -565,7 +578,7 @@ void json_print_response(uint8_t status, const bool only_to_muted /*= false*/) } } else if (nv_type == NV_TYPE_LINENUM) { // kill line number echo if not enabled - if ((js.echo_json_linenum == false) || (fp_ZERO(nv->value))) { // do not report line# 0 + if ((js.echo_json_linenum == false) || (fp_ZERO(nv->value_int))) { // do not report line# 0 nv->valuetype = TYPE_EMPTY; } } @@ -615,16 +628,36 @@ void json_print_response(uint8_t status, const bool only_to_muted /*= false*/) ***********************************************************************************/ /* - * json_set_jv() + * js_get_ej() - get JSON communications mode + * js_set_ej() - set JSON communications mode + * + * This one is a bit different: + * - cs.comm_mode is the setting for the *communications mode* (persistent) + * - js.json_mode is the actual current mode + * + * If comm_mode is set to TEXT_MORE (0) or JSON_MODE (1) then json_mode should also be changed + * If comm_mode is set to AUTO_MORE (0) then json_mode should not be changed */ -stat_t json_set_jv(nvObj_t *nv) +stat_t js_get_ej(nvObj_t *nv) { return(get_integer(nv, cs.comm_mode)); } +stat_t js_set_ej(nvObj_t *nv) { - if ((uint8_t)nv->value >= JV_MAX_VALUE) { - nv->valuetype = TYPE_NULL; - return (STAT_INPUT_EXCEEDS_MAX_VALUE); - } - js.json_verbosity = (jsonVerbosity)nv->value; + ritorno (set_integer(nv, (uint8_t &)cs.comm_mode, TEXT_MODE, AUTO_MODE)); + if (commMode(nv->value_int) < AUTO_MODE) { // set json_mode to 0 or 1, but don't change it if comm_mode == 2 + js.json_mode = commMode(nv->value_int); + } + return (STAT_OK); +} + +/* + * js_get_jv() - get JSON verbosity + * js_set_jv() - set JSON verbosity and related flags + */ + +stat_t js_get_jv(nvObj_t *nv) { return(get_integer(nv, js.json_verbosity)); } +stat_t js_set_jv(nvObj_t *nv) +{ + ritorno (set_integer(nv, (uint8_t &)js.json_verbosity, JV_SILENT, JV_MAX_VALUE)); js.echo_json_footer = false; js.echo_json_messages = false; @@ -637,11 +670,11 @@ stat_t json_set_jv(nvObj_t *nv) js.echo_json_messages = true; js.echo_json_configs = true; } else { - if (nv->value >= JV_FOOTER) js.echo_json_footer = true; - if (nv->value >= JV_MESSAGES) js.echo_json_messages = true; - if (nv->value >= JV_CONFIGS) js.echo_json_configs = true; - if (nv->value >= JV_LINENUM) js.echo_json_linenum = true; - if (nv->value >= JV_VERBOSE) js.echo_json_gcode_block = true; + if (nv->value_int >= JV_FOOTER) js.echo_json_footer = true; + if (nv->value_int >= JV_MESSAGES) js.echo_json_messages = true; + if (nv->value_int >= JV_CONFIGS) js.echo_json_configs = true; + if (nv->value_int >= JV_LINENUM) js.echo_json_linenum = true; + if (nv->value_int >= JV_VERBOSE) js.echo_json_gcode_block = true; } return(STAT_OK); } @@ -649,7 +682,7 @@ stat_t json_set_jv(nvObj_t *nv) /* * json_set_ej() - set JSON communications mode */ - +/* stat_t json_set_ej(nvObj_t *nv) { if (nv->value < TEXT_MODE) { @@ -667,6 +700,7 @@ stat_t json_set_ej(nvObj_t *nv) } return (set_ui8(nv)); } +*/ /*********************************************************************************** * TEXT MODE SUPPORT diff --git a/g2core/json_parser.h b/g2core/json_parser.h index 9b0e7e1b..500b05ca 100644 --- a/g2core/json_parser.h +++ b/g2core/json_parser.h @@ -2,8 +2,8 @@ * json_parser.h - JSON parser and JSON support * This file is part of the g2core project * - * Copyright (c) 2011 - 2017 Alden S. Hart, Jr. - * Copyright (c) 2016 - 2017 Rob Giseburt + * Copyright (c) 2011 - 2019 Alden S. Hart, Jr. + * Copyright (c) 2016 - 2019 Rob Giseburt * * This file ("the software") is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License, version 2 as published by the @@ -48,11 +48,11 @@ typedef enum { JV_CONFIGS, // [3] returns footer, messages, config commands JV_LINENUM, // [4] returns footer, messages, config commands, gcode line numbers if present JV_VERBOSE, // [5] returns footer, messages, config commands, gcode blocks - JV_EXCEPTIONS, // (unused) [6] returns only on messages, configs, and non-zero status - JV_STATUS, // (unused) [7] returns status and any messages in abbreviated format - JV_STATUS_COUNT, // (unused) [8] returns status, count and messages in abbreviated format - JV_MAX_VALUE + JV_EXCEPTIONS, // [6] returns only on messages, configs, and non-zero status + JV_STATUS, // [7] returns status and any messages in abbreviated format + JV_STATUS_COUNT // [8] returns status, count and messages in abbreviated format } jsonVerbosity; +#define JV_MAX_VALUE JV_STATUS_COUNT typedef enum { // json output print modes JSON_NO_PRINT = 0, // don't print anything if you find yourself in JSON mode @@ -89,8 +89,10 @@ void json_print_object(nvObj_t *nv); void json_print_response(uint8_t status, const bool only_to_muted = false); void json_print_list(stat_t status, uint8_t flags); -stat_t json_set_jv(nvObj_t *nv); -stat_t json_set_ej(nvObj_t *nv); +stat_t js_get_ej(nvObj_t *nv); +stat_t js_set_ej(nvObj_t *nv); +stat_t js_get_jv(nvObj_t *nv); +stat_t js_set_jv(nvObj_t *nv); #ifdef __TEXT_MODE diff --git a/g2core/kinematics.cpp b/g2core/kinematics.cpp old mode 100755 new mode 100644 index 6547c29b..c7c83eda --- a/g2core/kinematics.cpp +++ b/g2core/kinematics.cpp @@ -2,7 +2,8 @@ * kinematics.cpp - inverse kinematics routines * This file is part of the g2core project * - * Copyright (c) 2010 - 2016 Alden S. Hart, Jr. + * Copyright (c) 2010 - 2019 Alden S. Hart, Jr. + * Copyright (c) 2016 - 2019 Rob Giseburt * * This file ("the software") is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License, version 2 as published by the @@ -616,7 +617,7 @@ struct FourCableKinematics : KinematicsBase { // J = (A_1-A_0)/T cable_jerk[joint] = (cable_accel[joint]-prev_cable_accel[joint])/segment_time; - // float jmax = cm.a[AXIS_X].jerk_max * JERK_MULTIPLIER * 1.5; // 1.5 margin for cartesian plan to cable jerk + // float jmax = cm->a[AXIS_X].jerk_max * JERK_MULTIPLIER * 1.5; // 1.5 margin for cartesian plan to cable jerk bool jerk_or_velocity_adjusted = false; // if (cable_jerk[joint] > jmax) { // cable_jerk[joint] = jmax; @@ -636,7 +637,7 @@ struct FourCableKinematics : KinematicsBase { } // limit velocity - const double vmax = cm.a[AXIS_X].velocity_max * 1.7; + const double vmax = cm->a[AXIS_X].velocity_max * 1.7; if (cable_vel[joint] < -vmax) { cable_vel[joint] = -vmax; jerk_or_velocity_adjusted = true; @@ -990,7 +991,7 @@ struct FourCableKinematics : KinematicsBase { // cable_vel[joint] = 50.0; // it's already stopped, back it off some // } - float jmax = cm.a[AXIS_X].jerk_high * JERK_MULTIPLIER; + float jmax = cm->a[AXIS_X].jerk_high * JERK_MULTIPLIER; cable_jerk[joint] = sensor_diff[joint] * jmax; cable_accel[joint] = cable_accel[joint] + cable_jerk[joint]*segment_time; @@ -1002,7 +1003,7 @@ struct FourCableKinematics : KinematicsBase { cable_vel[joint] = cable_vel[joint] + cable_accel[joint]*segment_time; // limit velocity - const double vmax = cm.a[AXIS_X].velocity_max; + const double vmax = cm->a[AXIS_X].velocity_max; if (cable_vel[joint] < -vmax) { cable_vel[joint] = -vmax; // error_offset = 0; // none of it was applied @@ -1137,7 +1138,7 @@ void kn_config_changed() { for (uint8_t motor = 0; motor < MOTORS; motor++) { auto axis = st_cfg.mot[motor].motor_map; - if (cm.a[axis].axis_mode == AXIS_INHIBITED) { + if (cm->a[axis].axis_mode == AXIS_INHIBITED) { motor_map[motor] = -1; steps_per_unit[motor] = 1; // this is the denominator above, avoid 0 } diff --git a/g2core/kinematics.h b/g2core/kinematics.h old mode 100755 new mode 100644 index e78419e2..d9bd7fb8 --- a/g2core/kinematics.h +++ b/g2core/kinematics.h @@ -2,7 +2,7 @@ * kinematics.h - inverse kinematics routines * This file is part of the g2core project * - * Copyright (c) 2013 - 2016 Alden S. Hart, Jr. + * Copyright (c) 2013 - 2018 Alden S. Hart, Jr. * * This file ("the software") is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License, version 2 as published by the diff --git a/g2core/main.cpp b/g2core/main.cpp index 6e233608..b0a57ff2 100644 --- a/g2core/main.cpp +++ b/g2core/main.cpp @@ -2,8 +2,8 @@ * main.cpp - g2core - An embedded rs274/ngc CNC controller * This file is part of the g2core project. * - * Copyright (c) 2010 - 2016 Alden S. Hart, Jr. - * Copyright (c) 2013 - 2016 Robert Giseburt + * Copyright (c) 2010 - 2018 Alden S. Hart, Jr. + * Copyright (c) 2013 - 2018 Robert Giseburt * * This file ("the software") is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License, version 2 as published by the @@ -30,6 +30,7 @@ #include "report.h" #include "planner.h" #include "stepper.h" +#include "coolant.h" #include "encoder.h" #include "spindle.h" #include "temperature.h" @@ -50,17 +51,19 @@ stat_t status_code; // allocate a variable for the ritorno macro -/************* System Globals For Diagnostics ****************/ +/************* System Globals For Debugging and Diagnostics ****************/ +// See also: util.h for debugging and diagnostics // Using motate pins for profiling -// see https://github.com/synthetos/g2/wiki/Using-Pin-Changes-for-Timing-(and-light-debugging) +// Usage: https://github.com/synthetos/g2/wiki/Using-Pin-Changes-for-Timing-(and-light-debugging) using namespace Motate; OutputPin debug_pin1; OutputPin debug_pin2; OutputPin debug_pin3; -//OutputPin debug_pin4; +OutputPin debug_pin4; +// or these to disable the pin //OutputPin<-1> debug_pin1; //OutputPin<-1> debug_pin2; //OutputPin<-1> debug_pin3; @@ -72,7 +75,7 @@ using namespace Motate; extern OutputPin debug_pin1; extern OutputPin debug_pin2; extern OutputPin debug_pin3; -//extern OutputPin debug_pin4; +extern OutputPin debug_pin4; //extern OutputPin<-1> debug_pin1; //extern OutputPin<-1> debug_pin2; @@ -93,31 +96,34 @@ extern OutputPin debug_pin3; void application_init_services(void) { - hardware_init(); // system hardware setup - must be first - persistence_init(); // set up EEPROM or other NVM - must be second - xio_init(); // xtended io subsystem - must be third + hardware_init(); // system hardware setup - must be first + persistence_init(); // set up EEPROM or other NVM - must be second + xio_init(); // xtended io subsystem - must be third } void application_init_machine(void) { - cm.machine_state = MACHINE_INITIALIZING; + cm = &cm1; // set global canonical machine pointer to primary machine + cm->machine_state = MACHINE_INITIALIZING; - stepper_init(); // stepper subsystem - encoder_init(); // virtual encoders - gpio_init(); // inputs and outputs - pwm_init(); // pulse width modulation drivers - planner_init(); // motion planning subsystem - canonical_machine_init(); // canonical machine + stepper_init(); // stepper subsystem + encoder_init(); // virtual encoders + gpio_init(); // inputs and outputs + pwm_init(); // pulse width modulation drivers + canonical_machine_inits(); // combined inits for CMs and planner } void application_init_startup(void) { // start the application - controller_init(); // should be first startup init (requires xio_init()) - config_init(); // apply the config settings from persistence - canonical_machine_reset(); - spindle_init(); // should be after PWM and canonical machine inits and config_init() + controller_init(); // should be first startup init (requires xio_init()) + config_init(); // apply the config settings from persistence + canonical_machine_reset(&cm1); // initialize both CMs but only reset the primary + gcode_parser_init(); // baseline Gcode parser + spindle_init(); // should be after PWM and canonical machine inits and config_init() spindle_reset(); + coolant_init(); + coolant_reset(); temperature_init(); gpio_reset(); } diff --git a/g2core/marlin_compatibility.cpp b/g2core/marlin_compatibility.cpp index 964df8bb..44af95bc 100644 --- a/g2core/marlin_compatibility.cpp +++ b/g2core/marlin_compatibility.cpp @@ -2,8 +2,8 @@ * marlin_compatibility.cpp - support for marlin protocol and gcode * This file is part of the g2core project * - * Copyright (c) 2017 Alden S. Hart, Jr. - * Copyright (c) 2017 Rob Giseburt + * Copyright (c) 2017 - 2018 Alden S. Hart, Jr. + * Copyright (c) 2017 - 2018 Rob Giseburt * * This file ("the software") is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License, version 2 as published by the @@ -17,6 +17,15 @@ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/* + * This file contains most of the code needed to support marlin compatibility mode. + * Other files that are affected include: + * - gcode_parser.cpp/.h big additions for handling Marlin gcode and other functions + * - canonical_machine.cpp/.h setting targets for relative_extruder_mode coordinates + * - controller.cpp main-loop callback, fake_stk500, specialized dispatching + * - report.cpp system ready message suppressed for Marlin + * - xio.cpp/.h Marlin protocol support + */ #include "g2core.h" // #1 #include "config.h" // #2 #include "settings.h" @@ -102,7 +111,7 @@ nvObj_t *_get_specific_nv(const char *key) { */ void _report_temperatures(char *(&str)) { // Tool 0 is extruder 1 - uint8_t tool = cm.gm.tool; + uint8_t tool = cm->gm.tool; str_concat(str, " T:"); str += floattoa(str, cm_get_temperature(tool), 2); @@ -126,16 +135,16 @@ void _report_temperatures(char *(&str)) { */ void _report_position(char *(&str)) { str_concat(str, " X:"); - str += floattoa(str, cm_get_work_position(ACTIVE_MODEL, 0), 2); + str += floattoa(str, cm_get_display_position(ACTIVE_MODEL, 0), 2); str_concat(str, " Y:"); - str += floattoa(str, cm_get_work_position(ACTIVE_MODEL, 1), 2); + str += floattoa(str, cm_get_display_position(ACTIVE_MODEL, 1), 2); str_concat(str, " Z:"); - str += floattoa(str, cm_get_work_position(ACTIVE_MODEL, 2), 2); + str += floattoa(str, cm_get_display_position(ACTIVE_MODEL, 2), 2); - uint8_t tool = cm.gm.tool; + uint8_t tool = cm->gm.tool; if ((tool > 0) && (tool < 3)) { str_concat(str, " E:"); - str += floattoa(str, cm_get_work_position(ACTIVE_MODEL, 2), tool + 2); // A or B, depending on tool + str += floattoa(str, cm_get_display_position(ACTIVE_MODEL, 2), tool + 2); // A or B, depending on tool } } @@ -267,7 +276,7 @@ void _marlin_end_temperature_updates(float* vect, bool* flag) { bool _queue_next_temperature_commands() { if (MarlinSetTempState::Idle != set_temp_state) { - if (mp_planner_is_full()) { + if (mp_planner_is_full(mp)) { return false; } @@ -290,7 +299,7 @@ bool _queue_next_temperature_commands() } set_temp_state = MarlinSetTempState::StartingUpdates; - if (mp_planner_is_full()) { + if (mp_planner_is_full(mp)) { return false; } } @@ -299,7 +308,7 @@ bool _queue_next_temperature_commands() mp_queue_command(_marlin_start_temperature_updates, nullptr, nullptr); set_temp_state = MarlinSetTempState::StartingWait; - if (mp_planner_is_full()) { + if (mp_planner_is_full(mp)) { return false; } } @@ -312,7 +321,7 @@ bool _queue_next_temperature_commands() cm_json_wait(buffer); set_temp_state = MarlinSetTempState::StoppingUpdates; - if (mp_planner_is_full()) { + if (mp_planner_is_full(mp)) { return false; } } @@ -348,7 +357,7 @@ stat_t marlin_set_temperature(uint8_t tool, float temperature, bool wait) { */ stat_t marlin_request_temperature_report() // M105 { - uint8_t tool = cm.gm.tool; + uint8_t tool = cm->gm.tool; if ((tool < 1) || (tool > 2)) { return STAT_INPUT_VALUE_RANGE_ERROR; } @@ -512,12 +521,12 @@ void marlin_response(const stat_t status, char *buf) } else if (status == STAT_CHECKSUM_MATCH_FAILED) { str_concat(str, "Error:checksum mismatch, Last Line: "); - str += inttoa(str, cm.gmx.last_line_number); + str += inttoa(str, cm->gmx.last_line_number); request_resend = true; } else if (status == STAT_LINE_NUMBER_OUT_OF_SEQUENCE) { str_concat(str, "Error:Line Number is not Last Line Number+1, Last Line: "); - str += inttoa(str, cm.gmx.last_line_number); + str += inttoa(str, cm->gmx.last_line_number); request_resend = true; } else { @@ -537,7 +546,7 @@ void marlin_response(const stat_t status, char *buf) if (request_resend) { str = buffer; str_concat(str, "Resend: "); - str += inttoa(str, cm.gmx.last_line_number+1); + str += inttoa(str, cm->gmx.last_line_number+1); *str++ = '\n'; *str++ = 0; xio_writeline(buffer); diff --git a/g2core/marlin_compatibility.h b/g2core/marlin_compatibility.h index 168eb2ae..25d4bab1 100644 --- a/g2core/marlin_compatibility.h +++ b/g2core/marlin_compatibility.h @@ -2,8 +2,8 @@ * marlin_compatibility.cpp - support for marlin protocol and gcode * This file is part of the g2core project * - * Copyright (c) 2017 Alden S. Hart, Jr. - * Copyright (c) 2017 Rob Giseburt + * Copyright (c) 2017 - 2018 Alden S. Hart, Jr. + * Copyright (c) 2017 - 2018 Rob Giseburt * * This file ("the software") is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License, version 2 as published by the diff --git a/g2core/persistence.cpp b/g2core/persistence.cpp index e400be8c..cce395c0 100644 --- a/g2core/persistence.cpp +++ b/g2core/persistence.cpp @@ -2,7 +2,7 @@ * persistence.cpp - persistence functions * This file is part of the g2core project * - * Copyright (c) 2013 - 2017 Alden S. Hart Jr. + * Copyright (c) 2013 - 2018 Alden S. Hart Jr. * * This file ("the software") is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License, version 2 as published by the @@ -47,7 +47,7 @@ nvmSingleton_t nvm; void persistence_init() { - return; + return; } /* @@ -58,8 +58,8 @@ void persistence_init() stat_t read_persistent_value(nvObj_t *nv) { - nv->value = 0; - return (STAT_OK); + nv->value_flt = 0; + return (STAT_OK); } /* @@ -71,8 +71,8 @@ stat_t read_persistent_value(nvObj_t *nv) stat_t write_persistent_value(nvObj_t *nv) { -// if (cm.cycle_state != CYCLE_OFF) { // can't write when machine is moving +// if (cm->cycle_state != CYCLE_OFF) { // can't write when machine is moving // return(rpt_exception(STAT_FILE_NOT_OPEN, "write_persistent_value() can't write when machine is in cycle")); // } - return (STAT_OK); + return (STAT_OK); } diff --git a/g2core/persistence.h b/g2core/persistence.h old mode 100755 new mode 100644 index 8e9341c5..3a64d27e --- a/g2core/persistence.h +++ b/g2core/persistence.h @@ -2,7 +2,7 @@ * persistence.h - persistence code * This file is part of the g2code project * - * Copyright (c) 2013 - 2016 Alden S. Hart Jr. + * Copyright (c) 2013 - 2018 Alden S. Hart Jr. * * This file ("the software") is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License, version 2 as published by the diff --git a/g2core/plan_arc.cpp b/g2core/plan_arc.cpp index 3faeacb1..f5daab97 100644 --- a/g2core/plan_arc.cpp +++ b/g2core/plan_arc.cpp @@ -2,7 +2,7 @@ * plan_arc.c - arc planning and motion execution * This file is part of the g2core project * - * Copyright (c) 2010 - 2017 Alden S. Hart, Jr. + * Copyright (c) 2010 - 2019 Alden S. Hart, Jr. * * This file ("the software") is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License, version 2 as published by the @@ -24,10 +24,6 @@ #include "planner.h" #include "util.h" -// Allocate arc planner singleton structure - -arc_t arc; - // Local functions static stat_t _compute_arc(const bool radius_f); @@ -47,10 +43,10 @@ static stat_t _test_arc_soft_limits(void); /* * cm_arc_init() - initialize arc structures */ -void cm_arc_init() +void cm_arc_init(cmMachine_t *_cm) { - arc.magic_start = MAGICNUM; - arc.magic_end = MAGICNUM; + _cm->arc.magic_start = MAGICNUM; + _cm->arc.magic_end = MAGICNUM; } /* @@ -59,9 +55,9 @@ void cm_arc_init() * OK to call if no arc is running */ -void cm_abort_arc() +void cm_abort_arc(cmMachine_t *_cm) { - arc.run_state = BLOCK_INACTIVE; + _cm->arc.run_state = BLOCK_INACTIVE; } /* @@ -69,30 +65,28 @@ void cm_abort_arc() * * cm_arc_cycle_callback() is called from the controller main loop. Each time it's called * it queues as many arc segments (lines) as it can before it blocks, then returns. - * - * Parts of this routine were informed by the grbl project. */ -stat_t cm_arc_callback() +stat_t cm_arc_callback(cmMachine_t *_cm) { - if (arc.run_state == BLOCK_INACTIVE) { + if (_cm->arc.run_state == BLOCK_INACTIVE) { return (STAT_NOOP); } - if (mp_planner_is_full()) { + if (mp_planner_is_full(mp)) { return (STAT_EAGAIN); } - arc.theta += arc.segment_theta; - arc.gm.target[arc.plane_axis_0] = arc.center_0 + sin(arc.theta) * arc.radius; - arc.gm.target[arc.plane_axis_1] = arc.center_1 + cos(arc.theta) * arc.radius; - arc.gm.target[arc.linear_axis] += arc.segment_linear_travel; + _cm->arc.theta += _cm->arc.segment_theta; + _cm->arc.gm.target[_cm->arc.plane_axis_0] = _cm->arc.center_0 + sin(_cm->arc.theta) * _cm->arc.radius; + _cm->arc.gm.target[_cm->arc.plane_axis_1] = _cm->arc.center_1 + cos(_cm->arc.theta) * _cm->arc.radius; + _cm->arc.gm.target[_cm->arc.linear_axis] += _cm->arc.segment_linear_travel; - mp_aline(&arc.gm); // run the line - copy_vector(arc.position, arc.gm.target); // update arc current position + mp_aline(&(_cm->arc.gm)); // run the line + copy_vector(_cm->arc.position, _cm->arc.gm.target); // update arc current position - if (--arc.segment_count > 0) { + if (--(_cm->arc.segment_count) > 0) { return (STAT_EAGAIN); } - arc.run_state = BLOCK_INACTIVE; + _cm->arc.run_state = BLOCK_INACTIVE; return (STAT_OK); } @@ -111,7 +105,7 @@ stat_t cm_arc_feed(const float target[], const bool target_f[], // target en const cmMotionMode motion_mode) // defined motion mode { // Start setting up the arc and trapping arc specification errors - + // Trap some precursor cases. Since motion mode (MODAL_GROUP_G1) persists from the // previous move it's possible for non-modal commands such as F or P to arrive here // when no motion has actually been specified. It's also possible to run an arc as @@ -144,48 +138,48 @@ stat_t cm_arc_feed(const float target[], const bool target_f[], // target en } // trap missing feed rate - if (fp_ZERO(cm.gm.feed_rate)) { - return (STAT_GCODE_FEEDRATE_NOT_SPECIFIED); + if (fp_ZERO(cm->gm.feed_rate)) { + return (STAT_FEEDRATE_NOT_SPECIFIED); } // Set the arc plane for the current G17/G18/G19 setting and test arc specification // Plane axis 0 and 1 are the arc plane, the linear axis is normal to the arc plane. - if (cm.gm.select_plane == CANON_PLANE_XY) { // G17 - the vast majority of arcs are in the G17 (XY) plane - arc.plane_axis_0 = AXIS_X; - arc.plane_axis_1 = AXIS_Y; - arc.linear_axis = AXIS_Z; - } else if (cm.gm.select_plane == CANON_PLANE_XZ) { // G18 - arc.plane_axis_0 = AXIS_X; - arc.plane_axis_1 = AXIS_Z; - arc.linear_axis = AXIS_Y; - } else if (cm.gm.select_plane == CANON_PLANE_YZ) { // G19 - arc.plane_axis_0 = AXIS_Y; - arc.plane_axis_1 = AXIS_Z; - arc.linear_axis = AXIS_X; + if (cm->gm.select_plane == CANON_PLANE_XY) { // G17 - the vast majority of arcs are in the G17 (XY) plane + cm->arc.plane_axis_0 = AXIS_X; + cm->arc.plane_axis_1 = AXIS_Y; + cm->arc.linear_axis = AXIS_Z; + } else if (cm->gm.select_plane == CANON_PLANE_XZ) { // G18 + cm->arc.plane_axis_0 = AXIS_X; + cm->arc.plane_axis_1 = AXIS_Z; + cm->arc.linear_axis = AXIS_Y; + } else if (cm->gm.select_plane == CANON_PLANE_YZ) { // G19 + cm->arc.plane_axis_0 = AXIS_Y; + cm->arc.plane_axis_1 = AXIS_Z; + cm->arc.linear_axis = AXIS_X; } else { - return(cm_panic(STAT_GCODE_ACTIVE_PLANE_IS_MISSING, "cm_arc_feed() impossible value")); // plane axis has impossible value + return(cm_panic(STAT_ACTIVE_PLANE_IS_MISSING, "cm_arc_feed() impossible value")); // plane axis has impossible value } // test if no endpoints are specified in the selected plane - arc.full_circle = false; // initial condition - if (!(target_f[arc.plane_axis_0] || target_f[arc.plane_axis_1])) { + cm->arc.full_circle = false; // initial condition + if (!(target_f[cm->arc.plane_axis_0] || target_f[cm->arc.plane_axis_1])) { if (radius_f) { // in radius mode arcs missing both endpoints is an error return (STAT_ARC_AXIS_MISSING_FOR_SELECTED_PLANE); } else { - arc.full_circle = true; // in center format arc this specifies a full circle + cm->arc.full_circle = true; // in center format arc this specifies a full circle } } // test radius arcs for radius tolerance if (radius_f) { - arc.radius = _to_millimeters(radius); // set radius to internal format (mm) - if (std::abs(arc.radius) < MIN_ARC_RADIUS) { // radius value must be > minimum radius + cm->arc.radius = _to_millimeters(radius); // set radius to internal format (mm) + if (std::abs(cm->arc.radius) < MIN_ARC_RADIUS) { // radius value must be > minimum radius return (STAT_ARC_RADIUS_OUT_OF_TOLERANCE); } } else { // test that center format absolute distance mode arcs have both offsets specified - if (cm.gm.arc_distance_mode == ABSOLUTE_DISTANCE_MODE) { - if (!(offset_f[arc.plane_axis_0] && offset_f[arc.plane_axis_1])) { // if one or both offsets are missing + if (cm->gm.arc_distance_mode == ABSOLUTE_DISTANCE_MODE) { + if (!(offset_f[cm->arc.plane_axis_0] && offset_f[cm->arc.plane_axis_1])) { // if one or both offsets are missing return (STAT_ARC_OFFSETS_MISSING_FOR_SELECTED_PLANE); } } @@ -199,12 +193,12 @@ stat_t cm_arc_feed(const float target[], const bool target_f[], // target en if (floor(P_word) - (P_word) > 0) { return (STAT_P_WORD_IS_NOT_AN_INTEGER); } - arc.rotations = P_word; + cm->arc.rotations = P_word; } else { - if (arc.full_circle) { // arc rotations default to 1 for full circles - arc.rotations = 1; + if (cm->arc.full_circle) { // arc rotations default to 1 for full circles + cm->arc.rotations = 1; } else { - arc.rotations = 0; // no rotations + cm->arc.rotations = 0; // no rotations } } @@ -213,34 +207,34 @@ stat_t cm_arc_feed(const float target[], const bool target_f[], // target en // in radius mode it's an error for start == end if (radius_f) { - if ((fp_EQ(cm.gmx.position[AXIS_X], cm.gm.target[AXIS_X])) && - (fp_EQ(cm.gmx.position[AXIS_Y], cm.gm.target[AXIS_Y])) && - (fp_EQ(cm.gmx.position[AXIS_Z], cm.gm.target[AXIS_Z]))) { + if ((fp_EQ(cm->gmx.position[AXIS_X], cm->gm.target[AXIS_X])) && + (fp_EQ(cm->gmx.position[AXIS_Y], cm->gm.target[AXIS_Y])) && + (fp_EQ(cm->gmx.position[AXIS_Z], cm->gm.target[AXIS_Z]))) { return (STAT_ARC_ENDPOINT_IS_STARTING_POINT); } } // *** now get down to the rest of the work setting up the arc for execution *** - cm.gm.motion_mode = motion_mode; - cm_set_work_offsets(&cm.gm); // capture the fully resolved offsets to gm - memcpy(&arc.gm, &cm.gm, sizeof(GCodeState_t)); // copy GCode context to arc singleton - some will be overwritten to run segments - copy_vector(arc.position, cm.gmx.position); // set initial arc position from gcode model + cm->gm.motion_mode = motion_mode; + cm_set_display_offsets(&cm->gm); // capture the fully resolved offsets to gm + memcpy(&(cm->arc.gm), &cm->gm, sizeof(GCodeState_t)); // copy GCode context to arc singleton - some will be overwritten to run segments + copy_vector(cm->arc.position, cm->gmx.position); // set initial arc position from gcode model // setup offsets if in center format mode if (!radius_f) { - arc.offset[OFS_I] = _to_millimeters(offset[OFS_I]); // copy offsets with conversion to canonical form (mm) - arc.offset[OFS_J] = _to_millimeters(offset[OFS_J]); - arc.offset[OFS_K] = _to_millimeters(offset[OFS_K]); + cm->arc.ijk_offset[OFS_I] = _to_millimeters(offset[OFS_I]); // copy offsets with conversion to canonical form (mm) + cm->arc.ijk_offset[OFS_J] = _to_millimeters(offset[OFS_J]); + cm->arc.ijk_offset[OFS_K] = _to_millimeters(offset[OFS_K]); - if (arc.gm.arc_distance_mode == ABSOLUTE_DISTANCE_MODE) { // adjust offsets if in absolute mode - arc.offset[OFS_I] -= arc.position[AXIS_X]; - arc.offset[OFS_J] -= arc.position[AXIS_Y]; - arc.offset[OFS_K] -= arc.position[AXIS_Z]; + if (cm->arc.gm.arc_distance_mode == ABSOLUTE_DISTANCE_MODE) { // adjust offsets if in absolute mode + cm->arc.ijk_offset[OFS_I] -= cm->arc.position[AXIS_X]; + cm->arc.ijk_offset[OFS_J] -= cm->arc.position[AXIS_Y]; + cm->arc.ijk_offset[OFS_K] -= cm->arc.position[AXIS_Z]; } - if ((fp_ZERO(arc.offset[OFS_I])) && // error if no offsets provided in center format mode - (fp_ZERO(arc.offset[OFS_J])) && - (fp_ZERO(arc.offset[OFS_K]))) { + if ((fp_ZERO(cm->arc.ijk_offset[OFS_I])) && // error if no offsets provided in center format mode + (fp_ZERO(cm->arc.ijk_offset[OFS_J])) && + (fp_ZERO(cm->arc.ijk_offset[OFS_K]))) { return (cm_alarm(STAT_ARC_OFFSETS_MISSING_FOR_SELECTED_PLANE, "arc offsets missing or zero")); } } @@ -251,14 +245,14 @@ stat_t cm_arc_feed(const float target[], const bool target_f[], // target en // test arc soft limits stat_t status = _test_arc_soft_limits(); if (status != STAT_OK) { - cm.gm.motion_mode = MOTION_MODE_CANCEL_MOTION_MODE; - copy_vector(cm.gm.target, arc.position); // reset model position - return (cm_alarm(status, "arc soft_limits")); // throw an alarm + cm->gm.motion_mode = MOTION_MODE_CANCEL_MOTION_MODE; + copy_vector(cm->gm.target, cm->arc.position); // reset model position + return (cm_alarm(status, "arc soft_limits")); // throw an alarm } - cm_cycle_start(); // if not already started - arc.run_state = BLOCK_ACTIVE; // enable arc to be run from the callback - cm_finalize_move(); + cm_cycle_start(); // if not already started + cm->arc.run_state = BLOCK_ACTIVE; // enable arc to be run from the callback + cm_update_model_position(); return (STAT_OK); } @@ -286,7 +280,7 @@ static stat_t _compute_arc(const bool radius_f) if (radius_f) { // indicates a radius arc _compute_arc_offsets_from_radius(); } else { // compute start radius - arc.radius = hypotf(-arc.offset[arc.plane_axis_0], -arc.offset[arc.plane_axis_1]); + cm->arc.radius = hypotf(-cm->arc.ijk_offset[cm->arc.plane_axis_0], -cm->arc.ijk_offset[cm->arc.plane_axis_1]); } // Test arc specification for correctness according to: @@ -296,68 +290,74 @@ static stat_t _compute_arc(const bool radius_f) // center by more than (.05 inch/.5 mm) OR ((.0005 inch/.005mm) AND .1% of radius)." // Compute end radius from the center of circle (offsets) to target endpoint - float end_0 = arc.gm.target[arc.plane_axis_0] - arc.position[arc.plane_axis_0] - arc.offset[arc.plane_axis_0]; - float end_1 = arc.gm.target[arc.plane_axis_1] - arc.position[arc.plane_axis_1] - arc.offset[arc.plane_axis_1]; - float err = std::abs(hypotf(end_0, end_1) - arc.radius); // end radius - start radius + float end_0 = cm->arc.gm.target[cm->arc.plane_axis_0] - + cm->arc.position[cm->arc.plane_axis_0] - + cm->arc.ijk_offset[cm->arc.plane_axis_0]; + + float end_1 = cm->arc.gm.target[cm->arc.plane_axis_1] - + cm->arc.position[cm->arc.plane_axis_1] - + cm->arc.ijk_offset[cm->arc.plane_axis_1]; + + float err = std::abs(hypotf(end_0, end_1) - cm->arc.radius); // end radius - start radius if ((err > ARC_RADIUS_ERROR_MAX) || - ((err > ARC_RADIUS_ERROR_MIN) && (err > arc.radius * ARC_RADIUS_TOLERANCE))) { + ((err > ARC_RADIUS_ERROR_MIN) && (err > cm->arc.radius * ARC_RADIUS_TOLERANCE))) { return (STAT_ARC_HAS_IMPOSSIBLE_CENTER_POINT); } // Compute the angular travel // Calculate the theta angle of the current position (theta is also needed for calculating center point) // Note: gcc atan2 reverses args, i.e.: atan2(Y,X) - arc.theta = atan2(-arc.offset[arc.plane_axis_0], -arc.offset[arc.plane_axis_1]); + cm->arc.theta = atan2(-cm->arc.ijk_offset[cm->arc.plane_axis_0], -cm->arc.ijk_offset[cm->arc.plane_axis_1]); // Compute angular travel if not a full circle arc - if (!arc.full_circle) { - arc.angular_travel = atan2(end_0, end_1) - arc.theta; // travel = theta_end - theta_start + if (!cm->arc.full_circle) { + cm->arc.angular_travel = atan2(end_0, end_1) - cm->arc.theta; // travel = theta_end - theta_start // correct for atan2 output quadrants - if (arc.gm.motion_mode == MOTION_MODE_CW_ARC) { - if (arc.angular_travel <= 0) { arc.angular_travel += 2*M_PI; } + if (cm->arc.gm.motion_mode == MOTION_MODE_CW_ARC) { + if (cm->arc.angular_travel <= 0) { cm->arc.angular_travel += 2*M_PI; } } else { - if (arc.angular_travel > 0) { arc.angular_travel -= 2*M_PI; } + if (cm->arc.angular_travel > 0) { cm->arc.angular_travel -= 2*M_PI; } } // add in travel for rotations - if (arc.angular_travel >= 0) { arc.angular_travel += 2*M_PI * arc.rotations; } - else { arc.angular_travel -= 2*M_PI * arc.rotations; } + if (cm->arc.angular_travel >= 0) { cm->arc.angular_travel += 2*M_PI * cm->arc.rotations; } + else { cm->arc.angular_travel -= 2*M_PI * cm->arc.rotations; } } // Compute full-circle arcs else { - if (arc.gm.motion_mode == MOTION_MODE_CCW_ARC) { arc.rotations *= -1; } - arc.angular_travel = 2 * M_PI * arc.rotations; + if (cm->arc.gm.motion_mode == MOTION_MODE_CCW_ARC) { cm->arc.rotations *= -1; } + cm->arc.angular_travel = 2 * M_PI * cm->arc.rotations; } // Trap zero movement arcs - if (fp_ZERO(arc.angular_travel)) { + if (fp_ZERO(cm->arc.angular_travel)) { return (STAT_ARC_ENDPOINT_IS_STARTING_POINT); } // Calculate travel in the plane and the depth axis of the helix // Length is the total mm of travel of the helix (or just the planar arc) - arc.linear_travel = arc.gm.target[arc.linear_axis] - arc.position[arc.linear_axis]; - arc.planar_travel = arc.angular_travel * arc.radius; - arc.length = hypotf(arc.planar_travel, std::abs(arc.linear_travel)); + cm->arc.linear_travel = cm->arc.gm.target[cm->arc.linear_axis] - cm->arc.position[cm->arc.linear_axis]; + cm->arc.planar_travel = cm->arc.angular_travel * cm->arc.radius; + cm->arc.length = hypotf(cm->arc.planar_travel, std::abs(cm->arc.linear_travel)); // Find the minimum number of segments that meet accuracy and time constraints... // Note: removed segment_length test as segment_time accounts for this (build 083.37) float arc_time; float segments_for_minimum_time = _estimate_arc_time(arc_time) * (MICROSECONDS_PER_MINUTE / MIN_ARC_SEGMENT_USEC); - float segments_for_chordal_accuracy = arc.length / sqrt(4*cm.chordal_tolerance * (2 * arc.radius - cm.chordal_tolerance)); - arc.segments = floor(min(segments_for_chordal_accuracy, segments_for_minimum_time)); - arc.segments = max(arc.segments, (float)1.0); //...but is at least 1 segment + float segments_for_chordal_accuracy = cm->arc.length / sqrt(4*cm->chordal_tolerance * (2 * cm->arc.radius - cm->chordal_tolerance)); + cm->arc.segments = floor(min(segments_for_chordal_accuracy, segments_for_minimum_time)); + cm->arc.segments = max(cm->arc.segments, (float)1.0); //...but is at least 1 segment - if (arc.gm.feed_rate_mode == INVERSE_TIME_MODE) { - arc.gm.feed_rate /= arc.segments; + if (cm->arc.gm.feed_rate_mode == INVERSE_TIME_MODE) { + cm->arc.gm.feed_rate /= cm->arc.segments; } // setup the rest of the arc parameters - arc.segment_count = (int32_t)arc.segments; - arc.segment_theta = arc.angular_travel / arc.segments; - arc.segment_linear_travel = arc.linear_travel / arc.segments; - arc.center_0 = arc.position[arc.plane_axis_0] - sin(arc.theta) * arc.radius; - arc.center_1 = arc.position[arc.plane_axis_1] - cos(arc.theta) * arc.radius; - arc.gm.target[arc.linear_axis] = arc.position[arc.linear_axis]; // initialize the linear target + cm->arc.segment_count = (int32_t)cm->arc.segments; + cm->arc.segment_theta = cm->arc.angular_travel / cm->arc.segments; + cm->arc.segment_linear_travel = cm->arc.linear_travel / cm->arc.segments; + cm->arc.center_0 = cm->arc.position[cm->arc.plane_axis_0] - sin(cm->arc.theta) * cm->arc.radius; + cm->arc.center_1 = cm->arc.position[cm->arc.plane_axis_1] - cos(cm->arc.theta) * cm->arc.radius; + cm->arc.gm.target[cm->arc.linear_axis] = cm->arc.position[cm->arc.linear_axis]; // initialize the linear target return (STAT_OK); } @@ -439,8 +439,8 @@ static stat_t _compute_arc(const bool radius_f) static void _compute_arc_offsets_from_radius() { // Calculate the change in position along each selected axis - float x = arc.gm.target[arc.plane_axis_0] - arc.position[arc.plane_axis_0]; - float y = arc.gm.target[arc.plane_axis_1] - arc.position[arc.plane_axis_1]; + float x = cm->arc.gm.target[cm->arc.plane_axis_0] - cm->arc.position[cm->arc.plane_axis_0]; + float y = cm->arc.gm.target[cm->arc.plane_axis_1] - cm->arc.position[cm->arc.plane_axis_1]; // *** From Forrest Green - Other Machine Co, 3/27/14 // If the distance between endpoints is greater than the arc diameter, disc will be @@ -451,13 +451,13 @@ static void _compute_arc_offsets_from_radius() // risks obscuring g-code errors where the radius is actually too small (they will be // treated as half circles), but ensures that all valid arcs end up reasonably close // to their intended paths regardless of any numerical issues. - float disc = 4 * square(arc.radius) - (square(x) + square(y)); + float disc = 4 * square(cm->arc.radius) - (square(x) + square(y)); // h_x2_div_d == -(h * 2 / d) float h_x2_div_d = (disc > 0) ? -sqrt(disc) / hypotf(x,y) : 0; // Invert the sign of h_x2_div_d if circle is counter clockwise (see header notes) - if (arc.gm.motion_mode == MOTION_MODE_CCW_ARC) { + if (cm->arc.gm.motion_mode == MOTION_MODE_CCW_ARC) { h_x2_div_d = -h_x2_div_d; } @@ -466,15 +466,15 @@ static void _compute_arc_offsets_from_radius() // single Gcode block. 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 inadvisably // long arcs as prescribed. - if (arc.radius < 0) { + if (cm->arc.radius < 0) { h_x2_div_d = -h_x2_div_d; - arc.radius *= -1; // and flip the radius sign while you are at it + cm->arc.radius *= -1; // and flip the radius sign while you are at it } // Complete the operation by calculating the actual center of the arc - arc.offset[arc.plane_axis_0] = (x-(y*h_x2_div_d))/2; - arc.offset[arc.plane_axis_1] = (y+(x*h_x2_div_d))/2; - arc.offset[arc.linear_axis] = 0; + cm->arc.ijk_offset[cm->arc.plane_axis_0] = (x-(y*h_x2_div_d))/2; + cm->arc.ijk_offset[cm->arc.plane_axis_1] = (y+(x*h_x2_div_d))/2; + cm->arc.ijk_offset[cm->arc.linear_axis] = 0; } /* @@ -490,17 +490,17 @@ static void _compute_arc_offsets_from_radius() static float _estimate_arc_time (float arc_time) { // Determine move time at requested feed rate - if (arc.gm.feed_rate_mode == INVERSE_TIME_MODE) { - arc_time = arc.gm.feed_rate; // inverse feed rate has been normalized to minutes + if (cm->arc.gm.feed_rate_mode == INVERSE_TIME_MODE) { + arc_time = cm->arc.gm.feed_rate; // inverse feed rate has been normalized to minutes } else { - arc_time = arc.length / cm.gm.feed_rate; + arc_time = cm->arc.length / cm->gm.feed_rate; } // Downgrade the time if there is a rate-limiting axis - arc_time = max(arc_time, (float)std::abs(arc.planar_travel/cm.a[arc.plane_axis_0].feedrate_max)); - arc_time = max(arc_time, (float)std::abs(arc.planar_travel/cm.a[arc.plane_axis_1].feedrate_max)); - if (std::abs(arc.linear_travel) > 0) { - arc_time = max(arc_time, (float)std::abs(arc.linear_travel/cm.a[arc.linear_axis].feedrate_max)); + arc_time = max(arc_time, (float)std::abs(cm->arc.planar_travel/cm->a[cm->arc.plane_axis_0].feedrate_max)); + arc_time = max(arc_time, (float)std::abs(cm->arc.planar_travel/cm->a[cm->arc.plane_axis_1].feedrate_max)); + if (std::abs(cm->arc.linear_travel) > 0) { + arc_time = max(arc_time, (float)std::abs(cm->arc.linear_travel/cm->a[cm->arc.linear_axis].feedrate_max)); } return (arc_time); } @@ -566,11 +566,11 @@ static stat_t _test_arc_soft_limit_plane_axis(float center, uint8_t plane_axis) if (arc.angular_travel < M_PI) { // case (1) return (STAT_OK); } - if ((center - arc.radius) < cm.a[plane_axis].travel_min) { // case (2) + if ((center - arc.radius) < cm->a[plane_axis].travel_min) { // case (2) return (STAT_SOFT_LIMIT_EXCEEDED); } } - if ((center + arc.radius) > cm.a[plane_axis].travel_max) { // cases (3) and (4) + if ((center + arc.radius) > cm->a[plane_axis].travel_max) { // cases (3) and (4) return (STAT_SOFT_LIMIT_EXCEEDED); } return(STAT_OK); @@ -579,7 +579,7 @@ static stat_t _test_arc_soft_limit_plane_axis(float center, uint8_t plane_axis) static stat_t _test_arc_soft_limits() { /* - if (cm.soft_limit_enable == true) { + if (cm->soft_limit_enable == true) { // Test if target falls outside boundaries. This is a 3 dimensional test // so it also checks the linear axis of the arc (helix axis) diff --git a/g2core/plan_arc.h b/g2core/plan_arc.h old mode 100755 new mode 100644 index 77910258..9fa9715a --- a/g2core/plan_arc.h +++ b/g2core/plan_arc.h @@ -2,7 +2,7 @@ * plan_arc.h - arc planning and motion execution * This file is part of the g2core project * - * Copyright (c) 2010 - 2016 Alden S. Hart, Jr. + * Copyright (c) 2010 - 2018 Alden S. Hart, Jr. * * This file ("the software") is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License, version 2 as published by the @@ -30,48 +30,12 @@ #define ARC_RADIUS_ERROR_MIN ((float)0.005) // min mm where 1% rule applies #define ARC_RADIUS_TOLERANCE ((float)0.001) // 0.1% radius variance test -typedef struct arArcSingleton { // persistent planner and runtime variables - magic_t magic_start; - uint8_t run_state; // runtime state machine sequence - - float position[AXES]; // accumulating runtime position - float offset[3]; // arc IJK offsets - - float length; // length of line or helix in mm - float radius; // Raw R value, or computed via offsets - float theta; // starting angle of arc - float angular_travel; // travel along the arc in radians - float planar_travel; // travel in arc plane in mm - float linear_travel; // travel along linear axis of arc in mm - bool full_circle; // True if full circle arcs specified - float rotations; // number of full rotations to add (P value + sign) - - cmAxes plane_axis_0; // arc plane axis 0 - e.g. X for G17 - cmAxes plane_axis_1; // arc plane axis 1 - e.g. Y for G17 - cmAxes linear_axis; // linear axis (normal to plane) - - float segments; // number of segments in arc or blend - int32_t segment_count; // count of running segments - float segment_theta; // angular motion per segment - float segment_linear_travel; // linear motion per segment - float center_0; // center of circle at plane axis 0 (e.g. X for G17) - float center_1; // center of circle at plane axis 1 (e.g. Y for G17) - - GCodeState_t gm; // Gcode state struct is passed for each arc segment. - // Usage: - // uint32_t linenum; // line number of the arc feed move - same for each segment - // float target[AXES]; // arc segment target -// float work_offset[AXES]; // offset from machine coord system for reporting (same for each segment) -// float block_time; // segment_time: constant time per aline segment - - magic_t magic_end; -} arc_t; -extern arc_t arc; +#define CHORDAL_TOLERANCE_MIN (0.001) // values below this are not accepted /* arc function prototypes */ -void cm_arc_init(void); -void cm_abort_arc(void); -stat_t cm_arc_callback(void); +void cm_arc_init(cmMachine_t *_cm); +void cm_abort_arc(cmMachine_t *_cm); +stat_t cm_arc_callback(cmMachine_t *_cm); #endif // End of include guard: PLAN_ARC_H_ONCE diff --git a/g2core/plan_exec.cpp b/g2core/plan_exec.cpp index 5ad4aef5..32961165 100644 --- a/g2core/plan_exec.cpp +++ b/g2core/plan_exec.cpp @@ -2,8 +2,8 @@ * plan_exec.cpp - execution function for acceleration managed lines * This file is part of the g2core project * - * Copyright (c) 2010 - 2017 Alden S. Hart, Jr. - * Copyright (c) 2012 - 2017 Rob Giseburt + * Copyright (c) 2010 - 2019 Alden S. Hart, Jr. + * Copyright (c) 2012 - 2019 Rob Giseburt * * This file ("the software") is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License, version 2 as published by the @@ -36,36 +36,37 @@ #include "report.h" #include "util.h" #include "spindle.h" -#include "xio.h" //+++++DIAGNOSTIC +#include "xio.h" // DIAGNOSTIC // execute routines (NB: These are all called from the LO interrupt) static stat_t _exec_aline_head(mpBuf_t *bf); // passing bf because body might need it, and it might call body static stat_t _exec_aline_body(mpBuf_t *bf); // passing bf so that body can extend itself if the exit velocity rises. static stat_t _exec_aline_tail(mpBuf_t *bf); static stat_t _exec_aline_segment(void); +static void _exec_aline_normalize_block(mpBlockRuntimeBuf_t *b); +static stat_t _exec_aline_feedhold(mpBuf_t *bf); static void _init_forward_diffs(float v_0, float v_1); -/******************************************************************************* +/**************************************************************************************** * mp_forward_plan() - plan commands and moves ahead of exec; call ramping for moves * **** WARNING **** - * This function should NOT be called directly! - * Instead call st_request_forward_plan(), which mediates access. + * mp_forward_plan() should NOT be called directly! + * Instead call st_request_forward_plan(), which mediates access. * * mp_forward_plan() performs just-in-time forward planning immediately before - * lines and commands are queued to the move execution runtime (exec). - * Unlike backward planning, buffers are only forward planned once. + * moves and commands are queued to the move execution runtime (exec). + * Unlike back planning, buffers are only forward planned once. * * mp_forward_plan() is called aggressively via st_request_forward_plan(). * It has a relatively low interrupt level to call its own. - * See also: Planner Overview notes in planner.h + * See also: Planner Background and Overview notes in planner.h * * It examines the currently running buffer and its adjacent buffers to: - * * - Stop the system from re-planning or planning something that's not prepped * - Plan the next available ALINE (movement) block past the COMMAND blocks - * - Skip past/ or pre-plan COMMAND blocks while labeling them as PLANNED + * - Skip past/ or pre-plan COMMAND blocks while labeling them as FULLY_PLANNED * * Returns: * - STAT_OK if exec should be called to kickstart (or continue) movement @@ -85,110 +86,100 @@ static void _init_forward_diffs(float v_0, float v_1); * * See planner.h / bufferState enum for shorthand used in the descriptions. * All cases assume a mix of moves and commands, as noted in the shorthand. - * All cases assume 2 'blocks' - Run block & Plan block. Cases will need to be - * revisited and generalized if more blocks are used in the future (deeper - * forward planning). + * All cases assume 2 'blocks' - Run block (r) & Plan block (p). These cases + * will need to be revisited and generalized if more blocks are used in the future + * (i.e. deeper forward planning). * - * 'NOT_PREPPED' refers to any preliminary state below PREPPED, i.e. < PREPPED. - * ' NOT_PREPPED' can be either a move or command, we don't care so it's not specified. + * 'NOT_PLANNED' means the block has not been back planned or forward planned + * This refers to any state below BACK_PLANNED, i.e. < MP_BUFFER_BACK_PLANNED + * 'NOT_PLANNED' can be either a move or command, we don't care so it's not specified. + * + * 'BACK_PLANNED' means the block has been back planned but not forward planned + * 'FULLY_PLANNED' means the block is back planned and forward planned, is ready for execution + * 'RUNNING' means the move is executing in the runtime. The bf is "locked" during this phase * * 'COMMAND' or 'COMMAND(s)' refers to one command or a contiguous group of command buffers - * that may be in PREPPED or PLANNED states. Processing is always the same. Plan all - * PREPPED commands and skip past all PLANNED commands. + * that may be in BACK_PLANNED or FULLY_PLANNED states. Processing is always the same; + * plan all BACK_PLANNED commands and skip past all FULLY_PLANNED commands. * - * Note 1: For MOVEs use the exit velocity of the Run block (mr.r->exit_velocity) as the - * entry velocity of the next adjacent move. + * Note 1: For MOVEs use the exit velocity of the Run block (mr->r->exit_velocity) + * as the entry velocity of the next adjacent move. * - * Note 1a: In this special COMMAND case we trust mr.r->exit_velocity because the - * backplanner has already handled this case for us. + * Note 1a: In this special COMMAND case we trust mr->r->exit_velocity because the + * back planner has already handled this case for us. * - * Note 2: For COMMANDs use the entry velocity of the current runtime (mr.entry_velocity) - * as the entry velocity for the next adjacent move. mr.entry_velocity is almost always 0, + * Note 2: For COMMANDs use the entry velocity of the current runtime (mr->entry_velocity) + * as the entry velocity for the next adjacent move. mr->entry_velocity is almost always 0, * but could be non-0 in a race condition. - * FYI: mr.entry_velocity is set at the end of the last running block in mp_exec_aline(). - * + * FYI: mr->entry_velocity is set at the end of the last running block in mp_exec_aline(). * * CASE: - * 0. Nothing to do + * 0. Nothing to do * - * run_buffer - * ---------- - * a. Run buffer has not yet been initialized (prep null buffer and return NOOP) - * b. NOT_PREPPED No moves or commands in run buffer. Exit with no action + * run_buffer + * ---------- + * a. Run buffer has not yet been initialized (prep null buffer and return NOOP) + * b. NOT_BACK_PLANNED No moves or commands in run buffer. Exit with no action * - * 1. Bootstrap cases (buffer state < RUNNING) + * 1. Bootstrap cases (buffer state < RUNNING) * - * run_buffer next N bufs terminal buf Actions - * ---------- ----------- ------------ ---------------------------------- - * a. PREPPED-MOVE plan move, exit OK - * b. PLANNED-MOVE NOT_PREPPED exit NOOP - * c. PLANNED-MOVE PREPPED-MOVE exit NOOP (don't plan past a PLANNED buffer) - * d. PLANNED-MOVE PLANNED-MOVE trap illegal condition, exit NOOP - * e. PLANNED-MOVE COMMAND(s) exit NOOP - * f. PREPPED-COMMAND NOT_PREPPED plan command, exit OK - * g. PREPPED-COMMAND PREPPED-MOVE plan command, plan move (Note 2), exit OK - * h. PREPPED-COMMAND PLANNED-MOVE trap illegal condition, exit NOOP - * i. PLANNED-COMMAND NOT_PREPPED skip command, exit OK - * j. PLANNED-COMMAND PREPPED-MOVE skip command, plan move (Note 2), exit OK - * k. PLANNED-COMMAND PLANNED-MOVE exit NOOP + * run_buffer next N bufs terminal buf Actions + * ---------- ----------- ------------ ---------------------------------- + * a. BACK_PLANNED/MOVE plan move, exit OK + * b. FULLY_PLANNED/MOVE NOT_PLANNED exit NOOP + * c. FULLY_PLANNED/MOVE BACK_PLANNED/MOVE exit NOOP (don't plan past a PLANNED buffer) + * d. FULLY_PLANNED/MOVE FULLY_PLANNED/MOVE trap illegal condition, exit NOOP + * e. FULLY_PLANNED/MOVE COMMAND(s) exit NOOP + * f. BACK_PLANNED/COMMAND NOT_PREPPED plan command, exit OK + * g. BACK_PLANNED/COMMAND BACK_PLANNED/MOVE plan command, plan move (Note 2), exit OK + * h. BACK_PLANNED/COMMAND FULLY_PLANNED/MOVE trap illegal condition, exit NOOP + * i. BACK_PLANNED/COMMAND NOT_PLANNED skip command, exit OK + * j. BACK_PLANNED/COMMAND BACK_PLANNED/MOVE skip command, plan move (Note 2), exit OK + * k. BACK_PLANNED/COMMAND FULLY_PLANNED/MOVE exit NOOP * - * 2. Running cases (buffer state == RUNNING) + * 2. Running cases (buffer state == RUNNING) * - * run_buffer next N bufs terminal buf Actions - * ---------- ----------- ------------ ---------------------------------- - * a. RUNNING-MOVE PREPPED-MOVE plan move, exit OK - * b. RUNNING-MOVE PLANNED-MOVE exit NOOP - * c. RUNNING-MOVE COMMAND(s) NOT_PREPPED skip/plan command(s), exit OK - * d. RUNNING-MOVE COMMAND(s) PREPPED-MOVE skip/plan command(s), plan move, exit OK - * e. RUNNING-MOVE COMMAND(s) PLANNED-MOVE exit NOOP - * f. RUNNING-COMMAND PREPPED-MOVE plan move, exit OK - * g. RUNNING-COMMAND PLANNED-MOVE exit NOOP - * h. RUNNING-COMMAND COMMAND(s) NOT_PREPPED skip/plan command(s), exit OK - * i. RUNNING-COMMAND COMMAND(s) PREPPED-MOVE skip/plan command(s), plan move (Note 1a), exit OK - * j. RUNNING-COMMAND COMMAND(s) PLANNED-MOVE skip command(s), exit NOOP - * (Note: all COMMAND(s) in j. should be in PLANNED state) + * run_buffer next N bufs terminal buf Actions + * ---------- ----------- ------------ ---------------------------------- + * a. RUNNING/MOVE BACK_PLANNED/MOVE plan move, exit OK + * b. RUNNING/MOVE FULLY_PLANNED/MOVE exit NOOP + * c. RUNNING/MOVE COMMAND(s) NOT_PLANNED skip/plan command(s), exit OK + * d. RUNNING/MOVE COMMAND(s) BACK_PLANNED/MOVE skip/plan command(s), plan move, exit OK + * e. RUNNING/MOVE BACK_PLANNED(s) FULLY_PLANNED-MOVE exit NOOP + * f. RUNNING/COMMAND BACK_PLANNED/MOVE plan move, exit OK + * g. RUNNING/COMMAND FULLY_PLANNED/MOVE exit NOOP + * h. RUNNING/COMMAND COMMAND(s) NOT_PLANNED skip/plan command(s), exit OK + * i. RUNNING/COMMAND COMMAND(s) BACK_PLANNED/MOVE skip/plan command(s), plan move (Note 1a), exit OK + * j. RUNNING/COMMAND COMMAND(s) FULLY_PLANNED/MOVE skip command(s), exit NOOP + * + * (Note: all COMMAND(s) in 2j. should be in PLANNED state) */ - -static mpBuf_t *_plan_commands(mpBuf_t *bf) // plan or skip commands; return bf past last command +/* + * _plan_aline() - mp_forward_plan() helper + * + * Calculate ramps for the current planning block and the next PREPPED buffer + * The PREPPED buffer will be set to PLANNED later... + * + * Pass in the bf buffer that will "link" with the planned block + * The block and the buffer are implicitly linked for exec_aline() + * + * Note that that can only be one PLANNED move at a time. + * This is to help sync mr->p to point to the next planned mr->bf + * mr->p is only advanced in mp_exec_aline(), after mp->r = mr->p. + * This code aligns the buffers and the blocks for exec_aline(). + */ +static stat_t _plan_aline(mpBuf_t *bf, float entry_velocity) { - // must test for buffer state first as the buffer is only "safe" once it's >= PREPPED - while ((bf->buffer_state >= MP_BUFFER_PREPPED) && (bf->block_type >= BLOCK_TYPE_COMMAND)) { - if (bf->buffer_state != MP_BUFFER_PLANNED) { // skip already planned buffers - bf->buffer_state = MP_BUFFER_PLANNED; // "planning" is just setting the state (for now) - } - bf = bf->nx; - } - return (bf); -} - -static stat_t _plan_move(mpBuf_t *bf, float entry_velocity) -{ - // Calculate ramps for the current planning block and the next PREPPED buffer - // The PREPPED buffer will be set to PLANNED later... - // - // Pass in the bf buffer that will "link" with the planned block - // The block and the buffer are implicitly linked for exec_aline() - // - // Note that that can only be one PLANNED move at a time. - // This is to help sync mr.p to point to the next planned mr.bf - // mr.p is only advanced in mp_exec_aline(), after mp.r = mr.p. - // This code aligns the buffers and the blocks for exec_aline(). - - mpBlockRuntimeBuf_t* block = mr.p; // set a local planning block so it doesn't change on you + mpBlockRuntimeBuf_t* block = mr->p; // set a local planning block so pointer doesn't change on you mp_calculate_ramps(block, bf, entry_velocity); // (which it will if you don't do this) - // diagnostic traps + debug_trap_if_true((block->exit_velocity > block->cruise_velocity), + "_plan_line() exit velocity > cruise velocity after calculate_ramps()"); -#if IN_DEBUGGER == 1 - if (block->exit_velocity > block->cruise_velocity) { - __asm__("BKPT"); // exit > cruise after calculate_block - } - if (block->head_length < 0.00001 && block->body_length < 0.00001 && block->tail_length < 0.00001) { - __asm__("BKPT"); // zero or negative length block - } -#endif + debug_trap_if_true((block->head_length < 0.00001 && block->body_length < 0.00001 && block->tail_length < 0.00001), + "_plan_line() zero or negative length block after calculate_ramps()"); - bf->buffer_state = MP_BUFFER_PLANNED; //...here + bf->buffer_state = MP_BUFFER_FULLY_PLANNED; //...here bf->plannable = false; return (STAT_OK); // report that we planned something... } @@ -203,37 +194,44 @@ stat_t mp_forward_plan() st_prep_null(); return (STAT_NOOP); } - if (bf->buffer_state < MP_BUFFER_PREPPED) { // case 0b: nothing to do. get outta here. + if (bf->buffer_state < MP_BUFFER_BACK_PLANNED) { // case 0b: nothing to do. get outta here. return (STAT_NOOP); } // Case 2: Running cases - move bf past run buffer so it acts like case 1 if (bf->buffer_state == MP_BUFFER_RUNNING) { bf = bf->nx; - entry_velocity = mr.r->exit_velocity; // set Note 1 entry_velocity (move cases) + entry_velocity = mr->r->exit_velocity; // set Note 1 entry_velocity (move cases) } else { - entry_velocity = mr.entry_velocity; // set Note 2 entry velocity (command cases) + entry_velocity = mr->entry_velocity; // set Note 2 entry velocity (command cases) } - // bf points to command; start cases 1f, 1g, 1h, 1i, 1j, 1k, 2c, 2d, 2e, 2h, 2i, 2j + // bf points to a command block; start cases 1f, 1g, 1h, 1i, 1j, 1k, 2c, 2d, 2e, 2h, 2i, 2j + bool planned_something = false; + if (bf->block_type != BLOCK_TYPE_ALINE) { // meaning it's a COMMAND - bf = _plan_commands(bf); // plan commands or skip past already planned commands - // bf now points to the first non-command buffer past the command(s) - if ((bf->block_type == BLOCK_TYPE_ALINE) && (bf->buffer_state > MP_BUFFER_PREPPED )) { // case 1i - entry_velocity = mr.r->exit_velocity; // set entry_velocity for Note 1a + while (bf->block_type >= BLOCK_TYPE_COMMAND) { + if (bf->buffer_state == MP_BUFFER_BACK_PLANNED) { + bf->buffer_state = MP_BUFFER_FULLY_PLANNED; // "planning" is just setting the state (for now) + planned_something = true; + } + bf = bf->nx; + } + // Note: bf now points to the first non-command buffer past the command(s) + if ((bf->block_type == BLOCK_TYPE_ALINE) && (bf->buffer_state > MP_BUFFER_BACK_PLANNED )) { // case 1i + entry_velocity = mr->r->exit_velocity; // set entry_velocity for Note 1a } } // bf will always be on a non-command at this point - either a move or empty buffer // process move - if (bf->block_type == BLOCK_TYPE_ALINE) { // do cases 1a - 1e; finish cases 1f - 1k - if (bf->buffer_state == MP_BUFFER_PREPPED) {// do 1a; finish 1f, 1j, 2d, 2i - return (_plan_move(bf, entry_velocity)); - } else { - return (STAT_NOOP); // do 1b, 1c, 1d, 1e; finish 1g, 1h, 1j, 1k, 2e, 2j + if (bf->block_type == BLOCK_TYPE_ALINE) { // do cases 1a - 1e; finish cases 1f - 1k + if (bf->buffer_state == MP_BUFFER_BACK_PLANNED) {// do 1a; finish 1f, 1j, 2d, 2i + _plan_aline(bf, entry_velocity); + planned_something = true; } } - return (STAT_OK); // report that we planned something... + return (planned_something ? STAT_OK : STAT_NOOP); } /************************************************************************* @@ -247,8 +245,18 @@ stat_t mp_exec_move() { mpBuf_t *bf; + // Run an out of band dwell. It was probably set in the previous st_load_move() + // TODO: Find a better place for this - we shouldn't be concerned with dwells or othermove types here, + // MORE: Dwells shouldn't hold planning hostage. + if (mr->out_of_band_dwell_flag) { + mr->out_of_band_dwell_flag = false; + st_prep_out_of_band_dwell(mr->out_of_band_dwell_seconds * 1000); + return (STAT_OK); + } + // NULL means nothing's running - this is OK - if ((bf = mp_get_run_buffer()) == NULL || (bf->buffer_state < MP_BUFFER_PREPPED)) { + // If something is MP_BUFFER_BACK_PLANNED, we don't want to idle or prep_null() + if ((bf = mp_get_run_buffer()) == NULL || (bf->buffer_state < MP_BUFFER_BACK_PLANNED)) { if (kn->idle_task()) { return STAT_OK; // IOW: we need something loaded } @@ -256,56 +264,51 @@ stat_t mp_exec_move() return (STAT_NOOP); // IOW: exec is done, nothing to load here, move on } - if (bf->block_type == BLOCK_TYPE_ALINE) { // cycle auto-start for lines only - + if (bf->block_type == BLOCK_TYPE_ALINE) { // cycle auto-start for lines only // first-time operations + if (bf->buffer_state != MP_BUFFER_RUNNING) { - if ((bf->buffer_state < MP_BUFFER_PREPPED) && (cm.motion_state == MOTION_RUN)) { -#if IN_DEBUGGER == 1 - __asm__("BKPT"); // mp_exec_move() buffer is not prepped -#endif - // IMPORTANT: We can't rpt_exception from here! + if ((bf->buffer_state < MP_BUFFER_BACK_PLANNED) && (cm->motion_state == MOTION_RUN)) { + // IMPORTANT: can't rpt_exception from here! st_prep_null(); return (STAT_NOOP); } - if (bf->nx->buffer_state < MP_BUFFER_PREPPED) { + if ((bf->nx->buffer_state < MP_BUFFER_BACK_PLANNED) && (bf->nx->buffer_state > MP_BUFFER_EMPTY)) { // This detects buffer starvation, but also can be a single-line "jog" or command // rpt_exception(42, "mp_exec_move() next buffer is empty"); // ^^^ CAUSES A CRASH. We can't rpt_exception from here! + debug_trap("mp_exec_move() no buffer prepped - starvation"); } - if (bf->buffer_state == MP_BUFFER_PREPPED) { - // We need to have it planned. We don't want to do this here, as it - // might already be happening in a lower interrupt. + if (bf->buffer_state == MP_BUFFER_BACK_PLANNED) { + debug_trap_if_true((cm->motion_state == MOTION_RUN), "mp_exec_move() buffer prepped but not planned"); + // IMPORTANT: can't rpt_exception from here! + // We need to have it planned. We don't want to do this here, + // as it might already be happening in a lower interrupt. st_request_forward_plan(); return (STAT_NOOP); } - if (bf->buffer_state == MP_BUFFER_PLANNED) { - bf->buffer_state = MP_BUFFER_RUNNING; // must precede mp_planner_time_acccounting() + if (bf->buffer_state == MP_BUFFER_FULLY_PLANNED) { + bf->buffer_state = MP_BUFFER_RUNNING; // must precede mp_planner_time_acccounting() } else { return (STAT_NOOP); } mp_planner_time_accounting(); } - if (bf->nx->buffer_state >= MP_BUFFER_PREPPED) { - // We go ahead and *ask* for a forward planning of the next move. - // This won't call mp_plan_move until we leave this function - // (and have called mp_exec_aline via bf->bf_func). - // This also allows mp_exec_aline to advance mr.p first. + // Go ahead and *ask* for a forward planning of the next move. + // This won't call mp_plan_move until we leave this function + // (and have called mp_exec_aline via bf->bf_func). + // This also allows mp_exec_aline to advance mr->p first. + if (bf->nx->buffer_state >= MP_BUFFER_BACK_PLANNED) { st_request_forward_plan(); } - - // Manage motion state transitions - if ((cm.motion_state != MOTION_RUN) && (cm.motion_state != MOTION_HOLD)) { - cm_set_motion_state(MOTION_RUN); - } } if (bf->bf_func == NULL) { return(cm_panic(STAT_INTERNAL_ERROR, "mp_exec_move()")); // never supposed to get here } - return (bf->bf_func(bf)); // run the move callback in the planner buffer + return (bf->bf_func(bf)); // run the move callback in the planner buffer } /*************************************************************************/ @@ -313,33 +316,33 @@ stat_t mp_exec_move() /************************************************************************* * ---> Everything here fires from interrupts and must be interrupt safe * - * _exec_aline() - acceleration line main routine - * _exec_aline_head() - helper for acceleration section - * _exec_aline_body() - helper for cruise section - * _exec_aline_tail() - helper for deceleration section + * _exec_aline() - acceleration line main routine + * _exec_aline_head() - helper for acceleration section + * _exec_aline_body() - helper for cruise section + * _exec_aline_tail() - helper for deceleration section * _exec_aline_segment() - helper for running a segment * * Returns: * STAT_OK move is done * STAT_EAGAIN move is not finished - has more segments to run - * STAT_NOOP cause no operation from the steppers - do not load the move + * STAT_NOOP would cause no operation to the steppers - do not load the move * STAT_xxxxx fatal error. Ends the move and frees the bf buffer * * This routine is called from the (LO) interrupt level. The interrupt sequencing * relies on the behaviors of the routines being exactly correct. Each call to - * _exec_aline() must execute and prep *one and only one* segment. If the segment + * _exec_aline() must execute and prep **one and only one** segment. If the segment * is the not the last segment in the bf buffer the _aline() must return STAT_EAGAIN. * If it's the last segment it must return STAT_OK. If it encounters a fatal error * that would terminate the move it should return a valid error code. Failure to * obey this will introduce subtle and very difficult to diagnose bugs (trust us on this). * * Note 1: Returning STAT_OK ends the move and frees the bf buffer. - * Returning STAT_OK at this point does NOT advance position meaning any - * position error will be compensated by the next move. + * Returning STAT_OK at this point does NOT advance the position vector, + * meaning any position error will be compensated by the next move. * - * Note 2: Solves a potential race condition where the current move ends but the - * new move has not started because the previous move is still being run - * by the steppers. Planning can overwrite the new move. + * Note 2: BF/MR sequencing solves a potential race condition where the current move + * ends but the new move has not started because the previous move is still + * being run by the steppers. Planning can overwrite the new move. */ /* --- State transitions - hierarchical state machine --- * @@ -348,33 +351,33 @@ stat_t mp_exec_move() * from _RUN to _OFF on final call * or just remains _OFF * - * mr.block_state transitions on first call from _OFF to one of _HEAD, _BODY, _TAIL + * mr->block_state transitions on first call from _OFF to one of _HEAD, _BODY, _TAIL * Within each section state may be * _NEW - trigger initialization * _RUN1 - run the first part * _RUN2 - run the second part * * Important distinction to note: - * - mp_plan move() is called for every type of move + * - mp_plan move() is called for every type of move (bf block) * - mp_exec_move() is called for every type of move * - mp_exec_aline() is only called for alines */ /* Synchronization of run BUFFER and run BLOCK * * Note first: mp_exec_aline() makes a huge assumption: When it comes time to get a - * new run block (mr.r) it assumes the planner block (mr.p) has been fully planned + * new run block (mr->r) it assumes the planner block (mr->p) has been fully planned * via the JIT forward planning and is ready for use as the new run block. * * The runtime uses 2 structures for the current move or commend, the run BUFFER * from the planner queue (mb.r, aka bf), and the run BLOCK from the runtime - * singleton (mr.r). These structures are synchronized implicitly, but not + * singleton (mr->r). These structures are synchronized implicitly, but not * explicitly referenced, as pointers can lead to race conditions. * See plan_zoid.cpp / mp_calculate_ramps() for more details * * When mp_exec_aline() needs to grab a new planner buffer for a new move or command * (i.e. block state is inactive) it swaps (rolls) the run and planner BLOCKS so that - * mr.p (planner block) is now the mr.r (run block), and the old mr.r block becomes - * available for planning; it becomes mr.p block. + * mr->p (planner block) is now the mr->r (run block), and the old mr->r block becomes + * available for planning; it becomes mr->p block. * * At the same time, it's when finished with its current run buffer (mb.r), it has already * advanced to the next buffer. mp_exec_move() does this at the end of previous move. @@ -398,267 +401,158 @@ stat_t mp_exec_move() stat_t mp_exec_aline(mpBuf_t *bf) { + // don't run the block if the machine is not in cycle + if (cm_get_machine_state() != MACHINE_CYCLE) { + return (STAT_NOOP); + } + + // don't run the block if the block is inactive if (bf->block_state == BLOCK_INACTIVE) { return (STAT_NOOP); } - // Initialize all new blocks, regardless of normal or feedhold operation - if (mr.block_state == BLOCK_INACTIVE) { + stat_t status; - // too short lines have already been removed... - // so is the following code is no longer needed ++++ ash + // Initialize all new blocks, regardless of normal or feedhold operation + if (mr->block_state == BLOCK_INACTIVE) { + + // ASSERTIONS + + // Zero length moves (and other too-short moves) should have already been removed earlier // But let's still alert the condition should it ever occur - if (fp_ZERO(bf->length)) { // ...looks for an actual zero here - rpt_exception(STAT_PLANNER_ASSERTION_FAILURE, "mp_exec_aline() zero length move"); - } + debug_trap_if_zero(bf->length, "mp_exec_aline() zero length move"); + + // These equalities in the assertions must be true for this to work: + // entry_velocity <= cruise_velocity + // exit_velocity <= cruise_velocity + // + // NB: Even if the move is head or tail only, cruise velocity needs to be valid. + // This is because a "head" is *always* entry->cruise, and a "tail" is *always* cruise->exit, + // even if there are no other sections in the move. (This is a significant time savings.) + debug_trap_if_true((mr->entry_velocity > mr->r->cruise_velocity), + "mp_exec_aline() mr->entry_velocity > mr->r->cruise_velocity"); + + debug_trap_if_true((mr->r->exit_velocity > mr->r->cruise_velocity), + "mp_exec_aline() mr->exit_velocity > mr->r->cruise_velocity"); // Start a new move by setting up the runtime singleton (mr) - memcpy(&mr.gm, &(bf->gm), sizeof(GCodeState_t)); // copy in the gcode model state - bf->block_state = BLOCK_ACTIVE; // note that this buffer is running - // note the planner doesn't look at block_state - mr.block_state = BLOCK_INITIAL_ACTION; - mr.section = SECTION_HEAD; - mr.section_state = SECTION_NEW; + memcpy(&mr->gm, &(bf->gm), sizeof(GCodeState_t)); // copy in the gcode model state + bf->block_state = BLOCK_ACTIVE; // note that this buffer is running + mr->block_state = BLOCK_INITIAL_ACTION; // note the planner doesn't look at block_state - // This is the only place in the system where mr.r and mr.p are allowed to be changed - mr.r = mr.p; // we are now going to run the planning block - mr.p = mr.p->nx; // re-use the old running block as the new planning block + // !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! + // !!! THIS IS THE ONLY PLACE WHERE mr->r AND mr->p ARE ALLOWED TO BE CHANGED !!! + // !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! + // Swap P and R blocks + mr->r = mr->p; // we are now going to run the planning block + mr->p = mr->p->nx; // re-use the old running block as the new planning block - // Assumptions that are required for this to work: - // entry velocity <= cruise velocity && cruise velocity >= exit velocity - // Even if the move is head or tail only, cruise velocity needs to be valid. - // This is because a "head" is *always* entry->cruise, and a "tail" is *always* cruise->exit, - // even if there are not other sections int he move. (This is a significant time savings.) + // Check to make sure no sections are less than MIN_SEGMENT_TIME & adjust if necessary + _exec_aline_normalize_block(mr->r); - // Here we will check to make sure that the sections are longer than MIN_SEGMENT_TIME + // transfer move parameters from planner buffer to the runtime + copy_vector(mr->unit, bf->unit); + copy_vector(mr->target, bf->gm.target); + copy_vector(mr->axis_flags, bf->axis_flags); - if ((!fp_ZERO(mr.r->head_length)) && (mr.r->head_time < MIN_SEGMENT_TIME)) { - // head_time !== body_time - // We have to compute the new body time addition. - mr.r->body_length += mr.r->head_length; - mr.r->body_time = mr.r->body_length/mr.r->cruise_velocity; + mr->run_bf = bf; // DIAGNOSTIC: points to running bf + mr->plan_bf = bf->nx; // DIAGNOSTIC: points to next bf to forward plan - mr.r->head_length = 0; - mr.r->head_time = 0; - } - if ((!fp_ZERO(mr.r->tail_length)) && (mr.r->tail_time < MIN_SEGMENT_TIME)) { - // tail_time !== body_time - // We have to compute the new body time addition. - mr.r->body_length += mr.r->tail_length; - mr.r->body_time = mr.r->body_length/mr.r->cruise_velocity; - - mr.r->tail_length = 0; - mr.r->tail_time = 0; - } - - // At this point, we've already possibly merged head and/or tail into the body. - // If the body is too "short" (brief) still, we *might* be able to add it to a head or tail. - // If there's still a head or a tail, we will add the body to whichever there is, maybe both. - // We saved it for last since it's the most expensive. - if ((!fp_ZERO(mr.r->body_length)) && (mr.r->body_time < MIN_SEGMENT_TIME)) { - - // We'll add the time to either the head or the tail or split it - if (mr.r->tail_length > 0) { - if (mr.r->head_length > 0) { - // We'll split the body to the head and tail - float body_split = mr.r->body_length/2.0; - mr.r->body_length = 0; - mr.r->body_time = 0; - - mr.r->head_length += body_split; - mr.r->tail_length += body_split; - - mr.r->head_time = (2.0 * mr.r->head_length)/(mr.entry_velocity + mr.r->cruise_velocity); - mr.r->tail_time = (2.0 * mr.r->tail_length)/(mr.r->cruise_velocity + mr.r->exit_velocity); - } else { - // We'll put it all in the tail - mr.r->tail_length += mr.r->body_length; - mr.r->body_length = 0; - mr.r->body_time = 0; - - mr.r->tail_time = (2.0 * mr.r->tail_length)/(mr.r->cruise_velocity + mr.r->exit_velocity); - } - } - else if (mr.r->head_length > 0) { - // We'll put it all in the head - mr.r->head_length += mr.r->body_length; - mr.r->body_length = 0; - mr.r->body_time = 0; - - mr.r->head_time = (2.0 * mr.r->head_length)/(mr.entry_velocity + mr.r->cruise_velocity); - } - else { - // Uh oh! We have a move that's all body, and is still too short!! - // ++++ RG For now, we'll consider this impossible. - while (1); + // characterize the move for starting section - head/body/tail + mr->section_state = SECTION_NEW; + mr->section = SECTION_HEAD; + if (fp_ZERO(mr->r->head_length)) { + mr->section = SECTION_BODY; + if (fp_ZERO(mr->r->body_length)) { + mr->section = SECTION_TAIL; } } - copy_vector(mr.unit, bf->unit); - copy_vector(mr.target, bf->gm.target); // save the final target of the move - copy_vector(mr.axis_flags, bf->axis_flags); - // generate the way points for position correction at section ends for (uint8_t axis=0; axishead_length; - mr.waypoint[SECTION_BODY][axis] = mr.position[axis] + mr.unit[axis] * (mr.r->head_length + mr.r->body_length); - mr.waypoint[SECTION_TAIL][axis] = mr.position[axis] + mr.unit[axis] * (mr.r->head_length + mr.r->body_length + mr.r->tail_length); + mr->waypoint[SECTION_HEAD][axis] = mr->position[axis] + mr->unit[axis] * mr->r->head_length; + mr->waypoint[SECTION_BODY][axis] = mr->position[axis] + mr->unit[axis] * (mr->r->head_length + mr->r->body_length); + mr->waypoint[SECTION_TAIL][axis] = mr->position[axis] + mr->unit[axis] * (mr->r->head_length + mr->r->body_length + mr->r->tail_length); } } // Feed Override Processing - We need to handle the following cases (listed in rough sequence order): - // (1) - We've received a feed override request in the middle of a cycle // Feedhold Processing - We need to handle the following cases (listed in rough sequence order): - // (1) - We have a block midway through normal execution and a new feedhold request - // (1a) - The deceleration will fit in the length remaining in the running block (mr) - // (1b) - The deceleration will not fit in the running block - // (1c) - 1a, except the remaining length would be zero or EPSILON close to zero (unlikely) - // (2) - We have a new block and a new feedhold request that arrived at EXACTLY the same time (unlikely, but handled) - // (3) - We are in the middle of a block - // (3a) - The block is currently accelerating (we wait for the body to start) - // (3b) - The block is currently in the tail (we wait until the end of the block) - // (4) - We have decelerated a block to some velocity > zero (needs continuation in next block) - // (5) - We have decelerated a block to zero velocity - // (6) - We have finished all the runtime work now we have to wait for the steppers to stop - // (7) - The steppers have stopped. No motion should occur - // (8) - We are removing the hold state and there is queued motion (handled outside this routine) - // (9) - We are removing the hold state and there is no queued motion (also handled outside this routine) - - if (cm.motion_state == MOTION_HOLD) { - // Case (7) - all motion has ceased - if (cm.hold_state == FEEDHOLD_HOLD) { - return (STAT_NOOP); // VERY IMPORTANT to exit as a NOOP. No more movement + if (cm->hold_state != FEEDHOLD_OFF) { + // if running actions, or in HOLD state, or exiting with actions + if (cm->hold_state >= FEEDHOLD_MOTION_STOPPED) { // handles _exec_aline_feedhold_processing case (7) + return (STAT_NOOP); // VERY IMPORTANT to exit as a NOOP. Do not load another move } - - // Case (6) - wait for the steppers to stop - if (cm.hold_state == FEEDHOLD_PENDING) { - if (mp_runtime_is_idle()) { // wait for the steppers to actually clear out - if ((cm.cycle_state == CYCLE_HOMING) || (cm.cycle_state == CYCLE_PROBE)) { - // when homing, we don't need to stay in HOLD - cm.hold_state = FEEDHOLD_OFF; - } else { - cm.hold_state = FEEDHOLD_HOLD; - } - mp_zero_segment_velocity(); // for reporting purposes - sr_request_status_report(SR_REQUEST_IMMEDIATE); // was SR_REQUEST_TIMED - cs.controller_state = CONTROLLER_READY; // remove controller readline() PAUSE - } - return (STAT_OK); // hold here. No more movement - } - - // Case (5) - decelerated to zero - // Update the run buffer then force a replan of the whole planner queue - if (cm.hold_state == FEEDHOLD_DECEL_END) { - mr.block_state = BLOCK_INACTIVE; // invalidate mr buffer to reset the new move - bf->block_state = BLOCK_INITIAL_ACTION; // tell _exec to re-use the bf buffer - bf->length = get_axis_vector_length(mr.target, mr.position);// reset length - //bf->entry_vmax = 0; // set bp+0 as hold point - - cm.hold_state = FEEDHOLD_PENDING; - - // No point bothering with the rest of this move if homing or probing - if ((cm.cycle_state == CYCLE_HOMING) || (cm.cycle_state == CYCLE_PROBE)) { - mp_free_run_buffer(); - } - - mp_replan_queue(mb.r); // make it replan all the blocks - - return (STAT_OK); - } - - // Cases (1a, 1b), Case (2), Case (4) - // Build a tail-only move from here. Decelerate as fast as possible in the space we have. - if ((cm.hold_state == FEEDHOLD_SYNC) || - ((cm.hold_state == FEEDHOLD_DECEL_CONTINUE) && (mr.block_state == BLOCK_INITIAL_ACTION))) { - - // Case (3a) - already decelerating, continue the deceleration. - if (mr.section == SECTION_TAIL) { // if already in a tail don't decelerate. You already are - if (fp_ZERO(mr.r->exit_velocity)) { - cm.hold_state = FEEDHOLD_DECEL_TO_ZERO; - } else { - cm.hold_state = FEEDHOLD_DECEL_CONTINUE; - } - - // Case (3b) - currently accelerating - is simply skipped and waited for - // Small exception, if we *just started* the head, then we're not actually accelerating yet. - } else if ((mr.section != SECTION_HEAD) || (mr.section_state == SECTION_NEW)) { - mr.entry_velocity = mr.segment_velocity; - - mr.section = SECTION_TAIL; - mr.section_state = SECTION_NEW; - - mr.r->head_length = 0; - mr.r->body_length = 0; - - float available_length = get_axis_vector_length(mr.target, mr.position); - mr.r->tail_length = mp_get_target_length(0, mr.r->cruise_velocity, bf); // braking length - - if (fp_ZERO(available_length - mr.r->tail_length)) { // (1c) the deceleration time is almost exactly the remaining of the current move - cm.hold_state = FEEDHOLD_DECEL_TO_ZERO; - mr.r->tail_length = available_length; - mr.r->exit_velocity = 0; - - } else if (available_length < mr.r->tail_length) { // (1b) the deceleration has to span multiple moves - cm.hold_state = FEEDHOLD_DECEL_CONTINUE; - mr.r->tail_length = available_length; - mr.r->exit_velocity = mp_get_decel_velocity(mr.r->cruise_velocity, mr.r->tail_length, bf); - - } else { // (1a)the deceleration will fit into the current move - cm.hold_state = FEEDHOLD_DECEL_TO_ZERO; - mr.r->exit_velocity = 0; - } - mr.r->tail_time = mr.r->tail_length*2 / (mr.r->exit_velocity + mr.r->cruise_velocity); - } + // STAT_OK terminates aline execution for this move + // STAT_NOOP terminates execution and does not load another move + status = _exec_aline_feedhold(bf); + if ((status == STAT_OK) || (status == STAT_NOOP)) { + return (status); } } - mr.block_state = BLOCK_ACTIVE; + mr->block_state = BLOCK_ACTIVE; // NB: from this point on the contents of the bf buffer do not affect execution //**** main dispatcher to process segments *** - stat_t status = STAT_OK; - if (mr.section == SECTION_HEAD) { status = _exec_aline_head(bf);} - else if (mr.section == SECTION_BODY) { status = _exec_aline_body(bf);} - else if (mr.section == SECTION_TAIL) { status = _exec_aline_tail(bf);} + status = STAT_OK; + if (mr->section == SECTION_HEAD) { status = _exec_aline_head(bf); } + else if (mr->section == SECTION_BODY) { status = _exec_aline_body(bf); } + else if (mr->section == SECTION_TAIL) { status = _exec_aline_tail(bf); } else { return(cm_panic(STAT_INTERNAL_ERROR, "exec_aline()"));} // never supposed to get here - // We can't use the if/else block above, since the head may call body, and body call tail, so we wait till after - if ((mr.section == SECTION_TAIL) // Once we're in the tail, we can't plan the block anymore - || ((mr.section == SECTION_BODY) && (mr.segment_count < 3))) { // or are too close to the end of the body - + // Conditionally set the move to be unplannable. We can't use the if/else block above, + // since the head may call a body or a tail, and a body call tail, so we wait till after. + // + // Conditions are: + // - Allow 3 segments: 1 segment isn't enough, because there's one running as we execute, + // so it has to be the next one. There's a slight possibility we'll miss that, since we + // didn't necessarily start at the beginning, so three. + // - If it's a head/tail move and we've started the head we can't replan it anyway as + // the head can't be interrupted, and the tail is already as sharp as it can be (or there'd be a body) + // - ...so if you are in a body mark the body unplannable if we are too close to its end. + if ((mr->section == SECTION_TAIL) || ((mr->section == SECTION_BODY) && (mr->segment_count < 3))) { bf->plannable = false; } - // Feedhold Case (5): Look for the end of the deceleration to go into HOLD state - if ((cm.hold_state == FEEDHOLD_DECEL_TO_ZERO) && (status == STAT_OK)) { - cm.hold_state = FEEDHOLD_DECEL_END; - bf->block_state = BLOCK_INITIAL_ACTION; // reset bf so it can restart the rest of the move + // Feedhold Case (3): Look for the end of the deceleration to transition HOLD states + // This code sets states used by _exec_feedhold_processing() helper. + if (cm->hold_state == FEEDHOLD_DECEL_TO_ZERO) { + if ((status == STAT_OK) || (status == STAT_NOOP)) { + cm->hold_state = FEEDHOLD_DECEL_COMPLETE; + bf->block_state = BLOCK_INITIAL_ACTION; // reset bf so it can restart the rest of the move + } + } + + // Perform motion state transition. Also sets active model to RUNTIME + if (cm->motion_state != MOTION_RUN) { + cm_set_motion_state(MOTION_RUN); } // There are 4 things that can happen here depending on return conditions: - // status bf->block_state Description - // ----------- -------------- ---------------------------------------- + // status bf->block_state Description + // ----------- -------------- ---------------------------------------- // STAT_EAGAIN mr buffer has more segments to run // STAT_OK BLOCK_ACTIVE mr and bf buffers are done // STAT_OK BLOCK_INITIAL_ACTION mr done; bf must be run again (it's been reused) - // There is no fourth thing. Nobody expects the Spanish Inquisition + // STAT_NOOP treated as a STAT_OK if (status == STAT_EAGAIN) { - sr_request_status_report(SR_REQUEST_TIMED); // continue reporting mr buffer - // Note that tha'll happen in a lower interrupt level. + sr_request_status_report(SR_REQUEST_TIMED); // continue reporting mr buffer + // Note that that'll happen in a lower interrupt level. } else { - mr.block_state = BLOCK_INACTIVE; // invalidate mr buffer (reset) - mr.section_state = SECTION_OFF; - mp.run_time_remaining = 0.0; // it's done, so time goes to zero - - mr.entry_velocity = mr.r->exit_velocity; // feed the old exit into the entry. + mr->block_state = BLOCK_INACTIVE; // invalidate mr buffer (reset) + mr->section_state = SECTION_OFF; + mp->run_time_remaining = 0.0; // it's done, so time goes to zero + mr->entry_velocity = mr->r->exit_velocity; // feed the old exit into the entry. if (bf->block_state == BLOCK_ACTIVE) { - if (mp_free_run_buffer()) { // returns true of the buffer is empty - if (cm.hold_state == FEEDHOLD_OFF) { - cm_cycle_end(); // free buffer & end cycle if planner is empty + if (mp_free_run_buffer()) { // returns true of the buffer is empty + if (cm->hold_state == FEEDHOLD_OFF) { + cm_set_motion_state(MOTION_STOP); // also sets active model to RUNTIME + cm_cycle_end(); // free buffer & end cycle if planner is empty } } else { st_request_forward_plan(); @@ -668,26 +562,6 @@ stat_t mp_exec_aline(mpBuf_t *bf) return (status); } -/* - * mp_exit_hold_state() - end a feedhold - * - * Feedhold is executed as cm.hold_state transitions executed inside _exec_aline() - * Invoke a feedhold by calling cm_request_hold() or cm_start_hold() directly - * Return from feedhold by calling cm_request_end_hold() or cm_end_hold directly. - * See canonical_macine.c for a more detailed explanation of feedhold operation. - */ - -void mp_exit_hold_state() -{ - cm.hold_state = FEEDHOLD_OFF; - if (mp_has_runnable_buffer()) { - cm_set_motion_state(MOTION_RUN); - sr_request_status_report(SR_REQUEST_IMMEDIATE); - } else { - cm_set_motion_state(MOTION_STOP); - } -} - /* * Forward difference math explained: * @@ -831,8 +705,7 @@ static void _init_forward_diffs(const float v_0, const float v_1) // E = 0 // F = Vi - - const float h = 1/(mr.segments); + const float h = 1/(mr->segments); const float h_2 = h * h; const float h_3 = h_2 * h; const float h_4 = h_3 * h; @@ -850,14 +723,14 @@ static void _init_forward_diffs(const float v_0, const float v_1) * F_1 = 120 A h^5 */ - mr.forward_diff_5 = Ah_5 + Bh_4 + Ch_3; - mr.forward_diff_4 = 30.0*Ah_5 + 14.0*Bh_4 + 6.0*Ch_3; - mr.forward_diff_3 = 150.0*Ah_5 + 36.0*Bh_4 + 6.0*Ch_3; - mr.forward_diff_2 = 240.0*Ah_5 + 24.0*Bh_4; - mr.forward_diff_1 = 120.0*Ah_5; + mr->forward_diff_5 = Ah_5 + Bh_4 + Ch_3; + mr->forward_diff_4 = 30.0*Ah_5 + 14.0*Bh_4 + 6.0*Ch_3; + mr->forward_diff_3 = 150.0*Ah_5 + 36.0*Bh_4 + 6.0*Ch_3; + mr->forward_diff_2 = 240.0*Ah_5 + 24.0*Bh_4; + mr->forward_diff_1 = 120.0*Ah_5; - mr.segment_velocity = v_0; - mr.target_velocity = v_0 + mr.forward_diff_5; + mr->segment_velocity = v_0; + mr->target_velocity = v_0 + mr->forward_diff_5; } /********************************************************************************************* @@ -866,46 +739,45 @@ static void _init_forward_diffs(const float v_0, const float v_1) static stat_t _exec_aline_head(mpBuf_t *bf) { - bool first_pass = false; - if (mr.section_state == SECTION_NEW) { // INITIALIZATION - if (fp_ZERO(mr.r->head_length)) { - mr.section = SECTION_BODY; - return(_exec_aline_body(bf)); // skip ahead to the body generator + if (mr->section_state == SECTION_NEW) { // INITIALIZATION + if (fp_ZERO(mr->r->head_length)) { + mr->section = SECTION_BODY; + return(_exec_aline_body(bf)); // skip ahead to the body generator } - mr.segments = ceil(uSec(mr.r->head_time) / NOM_SEGMENT_USEC);// # of segments for the section - mr.segment_count = (uint32_t)mr.segments; - mr.segment_time = mr.r->head_time / mr.segments; // time to advance for each segment + mr->segments = ceil(uSec(mr->r->head_time) / NOM_SEGMENT_USEC);// # of segments for the section + mr->segment_count = (uint32_t)mr->segments; + mr->segment_time = mr->r->head_time / mr->segments; // time to advance for each segment - if (mr.segment_count == 1) { - // We will only have one segment, simply average the velocities - mr.segment_velocity = mr.entry_velocity; - mr.target_velocity = mr.r->cruise_velocity; + if (mr->segment_count == 1) { + // We will only have one segment, simply set the velocities + mr->segment_velocity = mr->entry_velocity; + mr->target_velocity = mr->r->cruise_velocity; } else { - _init_forward_diffs(mr.entry_velocity, mr.r->cruise_velocity); // <-- sets inital segment_velocity + _init_forward_diffs(mr->entry_velocity, mr->r->cruise_velocity); // <-- sets inital segment_velocity } - if (mr.segment_time < MIN_SEGMENT_TIME) { - _debug_trap("mr.segment_time < MIN_SEGMENT_TIME"); - return(STAT_OK); // exit without advancing position, say we're done + if (mr->segment_time < MIN_SEGMENT_TIME) { + debug_trap("mr->segment_time < MIN_SEGMENT_TIME (head)"); + return(STAT_OK); // exit without advancing position, say we're done } - mr.section = SECTION_HEAD; - mr.section_state = SECTION_RUNNING; + mr->section = SECTION_HEAD; // redundant, likely will be optimized out + mr->section_state = SECTION_RUNNING; } else { - mr.segment_velocity = mr.target_velocity; - mr.target_velocity += mr.forward_diff_5; + mr->segment_velocity = mr->target_velocity; + mr->target_velocity += mr->forward_diff_5; } if (_exec_aline_segment() == STAT_OK) { // set up for second half - if ((fp_ZERO(mr.r->body_length)) && (fp_ZERO(mr.r->tail_length))) { + if ((fp_ZERO(mr->r->body_length)) && (fp_ZERO(mr->r->tail_length))) { return(STAT_OK); // ends the move } - mr.section = SECTION_BODY; - mr.section_state = SECTION_NEW; - } else if (!first_pass) { - mr.forward_diff_5 += mr.forward_diff_4; - mr.forward_diff_4 += mr.forward_diff_3; - mr.forward_diff_3 += mr.forward_diff_2; - mr.forward_diff_2 += mr.forward_diff_1; + mr->section = SECTION_BODY; // advance to body + mr->section_state = SECTION_NEW; + } else { + mr->forward_diff_5 += mr->forward_diff_4; + mr->forward_diff_4 += mr->forward_diff_3; + mr->forward_diff_3 += mr->forward_diff_2; + mr->forward_diff_2 += mr->forward_diff_1; } return(STAT_EAGAIN); } @@ -913,39 +785,39 @@ static stat_t _exec_aline_head(mpBuf_t *bf) /********************************************************************************************* * _exec_aline_body() * - * The body is broken into little segments even though it is a straight line so that - * feed holds can happen in the middle of a line with a minimum of latency + * The body is broken into little segments even though it is a straight line + * so that feed holds can happen in the middle of a line with minimum latency */ static stat_t _exec_aline_body(mpBuf_t *bf) { - if (mr.section_state == SECTION_NEW) { - if (fp_ZERO(mr.r->body_length)) { - mr.section = SECTION_TAIL; + if (mr->section_state == SECTION_NEW) { + if (fp_ZERO(mr->r->body_length)) { + mr->section = SECTION_TAIL; return(_exec_aline_tail(bf)); // skip ahead to tail periods } - float body_time = mr.r->body_time; - mr.segments = ceil(uSec(body_time) / NOM_SEGMENT_USEC); - mr.segment_time = body_time / mr.segments; - mr.segment_velocity = mr.r->cruise_velocity; - mr.target_velocity = mr.segment_velocity; - mr.segment_count = (uint32_t)mr.segments; - if (mr.segment_time < MIN_SEGMENT_TIME) { - _debug_trap("mr.segment_time < MIN_SEGMENT_TIME"); + float body_time = mr->r->body_time; + mr->segments = ceil(uSec(body_time) / NOM_SEGMENT_USEC); + mr->segment_time = body_time / mr->segments; + mr->segment_velocity = mr->r->cruise_velocity; + mr->target_velocity = mr->segment_velocity; + mr->segment_count = (uint32_t)mr->segments; + if (mr->segment_time < MIN_SEGMENT_TIME) { + debug_trap("mr->segment_time < MIN_SEGMENT_TIME (body)"); return(STAT_OK); // exit without advancing position, say we're done } - mr.section = SECTION_BODY; - mr.section_state = SECTION_RUNNING; // uses PERIOD_2 so last segment detection works + mr->section = SECTION_BODY; + mr->section_state = SECTION_RUNNING; // uses PERIOD_2 so last segment detection works } if (_exec_aline_segment() == STAT_OK) { // OK means this section is done - if (fp_ZERO(mr.r->tail_length)) { - return(STAT_OK); // ends the move + if (fp_ZERO(mr->r->tail_length)) { + return (STAT_OK); // ends the move } - mr.section = SECTION_TAIL; - mr.section_state = SECTION_NEW; + mr->section = SECTION_TAIL; // advance to tail + mr->section_state = SECTION_NEW; } - return(STAT_EAGAIN); + return (STAT_EAGAIN); } /********************************************************************************************* @@ -954,43 +826,42 @@ static stat_t _exec_aline_body(mpBuf_t *bf) static stat_t _exec_aline_tail(mpBuf_t *bf) { - bool first_pass = false; - if (mr.section_state == SECTION_NEW) { // INITIALIZATION + if (mr->section_state == SECTION_NEW) { // INITIALIZATION // Mark the block as unplannable bf->plannable = false; - if (fp_ZERO(mr.r->tail_length)) { return(STAT_OK);} // end the move - mr.segments = ceil(uSec(mr.r->tail_time) / NOM_SEGMENT_USEC);// # of segments for the section - mr.segment_count = (uint32_t)mr.segments; - mr.segment_time = mr.r->tail_time / mr.segments; // time to advance for each segment + if (fp_ZERO(mr->r->tail_length)) { return(STAT_OK);} // end the move + mr->segments = ceil(uSec(mr->r->tail_time) / NOM_SEGMENT_USEC);// # of segments for the section + mr->segment_count = (uint32_t)mr->segments; + mr->segment_time = mr->r->tail_time / mr->segments; // time to advance for each segment - if (mr.segment_count == 1) { - mr.segment_velocity = mr.r->cruise_velocity; - mr.target_velocity = mr.r->exit_velocity; + if (mr->segment_count == 1) { + mr->segment_velocity = mr->r->cruise_velocity; + mr->target_velocity = mr->r->exit_velocity; } else { - _init_forward_diffs(mr.r->cruise_velocity, mr.r->exit_velocity); // <-- sets inital segment_velocity + _init_forward_diffs(mr->r->cruise_velocity, mr->r->exit_velocity); // <-- sets inital segment_velocity } - if (mr.segment_time < MIN_SEGMENT_TIME) { - _debug_trap("mr.segment_time < MIN_SEGMENT_TIME"); + if (mr->segment_time < MIN_SEGMENT_TIME) { + debug_trap("mr->segment_time < MIN_SEGMENT_TIME (tail)"); return(STAT_OK); // exit without advancing position, say we're done // return(STAT_MINIMUM_TIME_MOVE); // exit without advancing position } - mr.section = SECTION_TAIL; - mr.section_state = SECTION_RUNNING; + mr->section = SECTION_TAIL; + mr->section_state = SECTION_RUNNING; } else { - mr.segment_velocity = mr.target_velocity; - mr.target_velocity += mr.forward_diff_5; + mr->segment_velocity = mr->target_velocity; + mr->target_velocity += mr->forward_diff_5; } if (_exec_aline_segment() == STAT_OK) { return(STAT_OK); // STAT_OK completes the move - } else if (!first_pass) { - mr.forward_diff_5 += mr.forward_diff_4; - mr.forward_diff_4 += mr.forward_diff_3; - mr.forward_diff_3 += mr.forward_diff_2; - mr.forward_diff_2 += mr.forward_diff_1; + } else { + mr->forward_diff_5 += mr->forward_diff_4; + mr->forward_diff_4 += mr->forward_diff_3; + mr->forward_diff_3 += mr->forward_diff_2; + mr->forward_diff_2 += mr->forward_diff_1; } - return(STAT_EAGAIN); + return (STAT_EAGAIN); } /********************************************************************************************* @@ -1023,42 +894,235 @@ static stat_t _exec_aline_segment() // Otherwise if not at a section waypoint compute target from segment time and velocity // Don't do waypoint correction if you are going into a hold. - // See https://en.wikipedia.org/wiki/Kahan_summation_algorithm - // for the description of the summation compensation used. - - if ((--mr.segment_count == 0) && (cm.motion_state != MOTION_HOLD)) { - copy_vector(mr.gm.target, mr.waypoint[mr.section]); + if ((--mr->segment_count == 0) && (cm->hold_state == FEEDHOLD_OFF)) { + copy_vector(mr->gm.target, mr->waypoint[mr->section]); } else { - float segment_length = (mr.segment_velocity+mr.target_velocity) * 0.5 * mr.segment_time; - // see https://en.wikipedia.org/wiki/Kahan_summation_algorithm - // for the summation compensation description + float segment_length = (mr->segment_velocity+mr->target_velocity) * 0.5 * mr->segment_time; + // See https://en.wikipedia.org/wiki/Kahan_summation_algorithm + // for the summation compensation description for (uint8_t a=0; agm.target[a] = mr->position[a] + (mr->unit[a] * segment_length); - float to_add = (mr.unit[a] * segment_length) - mr.gm.target_comp[a]; - float target = mr.position[a] + to_add; - mr.gm.target_comp[a] = (target - mr.position[a]) - to_add; - mr.gm.target[a] = target; + float to_add = (mr->unit[a] * segment_length) - mr->gm.target_comp[a]; + float target = mr->position[a] + to_add; + mr->gm.target_comp[a] = (target - mr->position[a]) - to_add; + mr->gm.target[a] = target; } } // Convert target position to steps - // kn->inverse_kinematics(mr.gm.target, exec_target_steps); // now determine the target steps... - kn->inverse_kinematics(mr.gm.target, mr.position, mr.segment_velocity, mr.target_velocity, mr.segment_time, exec_target_steps); + // kn->inverse_kinematics(mr->gm.target, exec_target_steps); // now determine the target steps... + kn->inverse_kinematics(mr->gm.target, mr->position, mr->segment_velocity, mr->target_velocity, mr->segment_time, exec_target_steps); // Update the mb->run_time_remaining -- we know it's missing the current segment's time before it's loaded, that's ok. - mp.run_time_remaining -= mr.segment_time; - if (mp.run_time_remaining < 0) { - mp.run_time_remaining = 0.0; + mp->run_time_remaining -= mr->segment_time; + if (mp->run_time_remaining < 0) { + mp->run_time_remaining = 0.0; } // Set the target steps and call the stepper prep function ritorno(mp_set_target_steps(exec_target_steps)); - copy_vector(mr.position, mr.gm.target); // update position from target - if (mr.segment_count == 0) { + copy_vector(mr->position, mr->gm.target); // update position from target + if (mr->segment_count == 0) { return (STAT_OK); // this section has run all its segments } return (STAT_EAGAIN); // this section still has more segments to run } + +/********************************************************************************************* + * _exec_aline_normalize_block() - re-organize block to eliminate minimum time segments + * + * Check to make sure no sections are less than MIN_SEGMENT_TIME & adjust if necessary + */ + +static void _exec_aline_normalize_block(mpBlockRuntimeBuf_t *b) +{ + if ((b->head_length > 0) && (b->head_time < MIN_SEGMENT_TIME)) { + // Compute the new body time. head_time !== body_time + b->body_length += b->head_length; + b->body_time = b->body_length / b->cruise_velocity; + b->head_length = 0; + b->head_time = 0; + } + if ((b->tail_length > 0) && (b->tail_time < MIN_SEGMENT_TIME)) { + // Compute the new body time. tail_time !== body_time + b->body_length += b->tail_length; + b->body_time = b->body_length / b->cruise_velocity; + b->tail_length = 0; + b->tail_time = 0; + } + + // At this point, we've already possibly merged head and/or tail into the body. + // If the body is still too "short" (brief) we *might* be able to add it to a head or tail. + // If there's still a head or a tail, we will add the body to whichever there is, maybe both. + // We saved it for last since it's the most expensive. + if ((b->body_length > 0) && (b->body_time < MIN_SEGMENT_TIME)) { + + // We'll add the time to either the head or the tail or split it + if (b->tail_length > 0) { + if (b->head_length > 0) { // Split the body to the head and tail + b->head_length += b->body_length * 0.5; + b->tail_length += b->body_length * 0.5; // let the compiler optimize out one of these * + b->head_time = (2.0 * b->head_length) / (mr->entry_velocity + b->cruise_velocity); + b->tail_time = (2.0 * b->tail_length) / (b->cruise_velocity + b->exit_velocity); + b->body_length = 0; + b->body_time = 0; + } else { // Put it all in the tail + b->tail_length += b->body_length; + b->tail_time = (2.0 * b->tail_length) / (b->cruise_velocity + b->exit_velocity); + b->body_length = 0; + b->body_time = 0; + } + } + else if (b->head_length > 0) { // Put it all in the head + b->head_length += b->body_length; + b->head_time = (2.0 * b->head_length) / (mr->entry_velocity + b->cruise_velocity); + b->body_length = 0; + b->body_time = 0; + } + else { // Uh oh! We have a move that's all body, and is still too short!! + debug_trap("_exec_aline_normalize_block() - found a move that is too short"); + } + } +} + +/********************************************************************************************* + * _exec_aline_feedhold() - feedhold helper for mp_exec_aline() + * + * This function performs the bulk of the feedhold state machine processing from within + * mp_exec_aline(). There is also a little chunk labeled "Feedhold Case (3-continued)". + * Feedhold processing mostly manages the deceleration phase into the hold, and sets + * state variables used in cycle_feedhold.cpp + * + * Returns: + * + * STAT_OK exits from mp_exec_aline() but allows another segment to be loaded + * and executed. This is used when the hold is still in continuous motion. + * + * STAT_NOOP exits from mp_exec_aline() and prevents another segment loading and + * executing. This is used when the hold has stopped at the hold point. + * + * STAT_EAGAIN allows mp_exec_aline() to continue execution, playing out a head, body + * or tail. + */ + +static stat_t _exec_aline_feedhold(mpBuf_t *bf) +{ + // Case (4) - Wait for the steppers to stop and complete the feedhold + if (cm->hold_state == FEEDHOLD_MOTION_STOPPING) { + if (mp_runtime_is_idle()) { // wait for steppers to actually finish + + // Motion has stopped, so we can rely on positions and other values to be stable + + // If hold was SKIP type, discard the remainder of the block and position to the next block + if (cm->hold_type == FEEDHOLD_TYPE_SKIP) { + copy_vector(mp->position, mr->position); // update planner position to the final runtime position + mp_free_run_buffer(); // advance to next block, discarding the rest of the move + } + + // Otherwise setup the block to complete motion (regardless of how hold will ultimately be exited) + else { + bf->length = get_axis_vector_length(mr->position, mr->target); // update bf w/remaining length in move + + // If length ~= 0 it's because the deceleration was exact. Handle this exception to avoid planning errors + if (bf->length < EPSILON4) { + copy_vector(mp->position, mr->position);// update planner position to the final runtime position + mp_free_run_buffer(); // advance to next block, discarding the zero-length move + } else { + bf->block_state = BLOCK_INITIAL_ACTION; // tell _exec to re-use the bf buffer + while (bf->buffer_state > MP_BUFFER_BACK_PLANNED) { + bf->buffer_state = MP_BUFFER_BACK_PLANNED;// revert from RUNNING so it can be forward planned again + bf->plannable = true; // needed so block can be re-planned + bf = mp_get_next_buffer(bf); + } + } + } + mr->reset(); // reset MR for next use and for forward planning + cm_set_motion_state(MOTION_STOP); + cm->hold_state = FEEDHOLD_MOTION_STOPPED; + sr_request_status_report(SR_REQUEST_IMMEDIATE); + } + return (STAT_NOOP); // hold here. leave with a NOOP so it does not attempt another load and exec. + } + + // Case (3') - Decelerated to zero. See also Feedhold Case (3) in mp_exec_aline() + // This state is needed to return an OK to complete the aline exec before transitioning to case (4). + if (cm->hold_state == FEEDHOLD_DECEL_COMPLETE) { + cm->hold_state = FEEDHOLD_MOTION_STOPPING; // wait for motion to come to a complete stop + return (STAT_OK); // exit from mp_exec_aline() + } + + // Cases (1x), Case (2) + // Build a tail-only move from here. Decelerate as fast as possible in the space available. + if ((cm->hold_state == FEEDHOLD_SYNC) || + ((cm->hold_state == FEEDHOLD_DECEL_CONTINUE) && (mr->block_state == BLOCK_INITIAL_ACTION))) { + + // Case (1d) - Already decelerating (in a tail), continue the deceleration. + if (mr->section == SECTION_TAIL) { // if already in a tail don't decelerate. You already are + if (mr->r->exit_velocity < EPSILON2) { // allow near-zero velocities to be treated as zero + cm->hold_state = FEEDHOLD_DECEL_TO_ZERO; + } else { + cm->hold_state = FEEDHOLD_DECEL_CONTINUE; + } + return (STAT_EAGAIN); // exiting with EAGAIN will continue exec_aline() execution + } + + // Case (1a) - Currently accelerating (in a head), skip and waited for body or tail + // This is true because to do otherwise the jerk would not have returned to zero. + // Small exception, if we *just started* the head, then we're not actually accelerating yet. + if ((mr->section == SECTION_HEAD) && (mr->section_state != SECTION_NEW)) { + return (STAT_EAGAIN); + } + + // Case (1b, 1c) - Block is in a body or about to start a new head. Turn it into a new tail. + // In the new_head case plan deceleration move (tail) starting at the at the entry velocity + mr->section = SECTION_TAIL; + mr->section_state = SECTION_NEW; + mr->entry_velocity = mr->segment_velocity; + mr->r->cruise_velocity = mr->entry_velocity; // cruise velocity must be set even if there's no body + mr->r->tail_length = mp_get_target_length(0, mr->r->cruise_velocity, bf); // braking length + mr->r->head_length = 0; + mr->r->body_length = 0; + mr->r->head_time = 0; + mr->r->body_time = 0; + + // The deceleration distance either fits in the available length or fits exactly or close + // enough (to EPSILON2) (1e). Case 1e happens frequently when the tail in the move was + // already planned to zero. EPSILON2 deals with floating point rounding errors that can + // mis-classify this case. EPSILON2 is 0.0001, which is 0.1 microns in length. + float available_length = get_axis_vector_length(mr->target, mr->position); + + // Cases (1b1, 1c1) deceleration will fit in the block + if ((available_length + EPSILON2 - mr->r->tail_length) > 0) { + cm->hold_state = FEEDHOLD_DECEL_TO_ZERO; + mr->r->exit_velocity = 0; + mr->r->tail_time = mr->r->tail_length*2 / (mr->r->exit_velocity + mr->r->cruise_velocity); + bf->block_time = mr->r->tail_time; + } + // Cases (1b2, 1c2) deceleration will not fit in the block + else { + cm->hold_state = FEEDHOLD_DECEL_CONTINUE; + mr->r->tail_length = available_length; + mr->r->exit_velocity = mp_get_decel_velocity(mr->r->cruise_velocity, mr->r->tail_length, bf); + if (mr->r->exit_velocity >= 0) { + mr->r->tail_time = mr->r->tail_length*2 / (mr->r->exit_velocity + mr->r->cruise_velocity); + bf->block_time = mr->r->tail_time; + } + // The following branch is rarely if ever taken. It's possible for the deceleration calculation + // to return an error if the length is too short and other conditions exist. In that case + // make the block into a cruise (body) and push the deceleration to the next block. + else { + mr->section = SECTION_BODY; + mr->r->exit_velocity = mr->r->cruise_velocity; // both should be @ mr->segment_velocity + mr->r->body_length = available_length; + mr->r->body_time = mr->r->body_length / mr->r->cruise_velocity; + mr->r->tail_length = 0; + mr->r->tail_time = 0; + } + } + _exec_aline_normalize_block(mr->r); + } + return (STAT_EAGAIN); // exiting with EAGAIN will continue exec_aline() execution +} diff --git a/g2core/plan_line.cpp b/g2core/plan_line.cpp index 1912b2ee..8ca8028e 100644 --- a/g2core/plan_line.cpp +++ b/g2core/plan_line.cpp @@ -2,8 +2,8 @@ * plan_line.c - acceleration managed line planning and motion execution * This file is part of the g2core project * - * Copyright (c) 2010 - 2017 Alden S. Hart, Jr. - * Copyright (c) 2012 - 2017 Rob Giseburt + * Copyright (c) 2010 - 2019 Alden S. Hart, Jr. + * Copyright (c) 2012 - 2019 Rob Giseburt * * This file ("the software") is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License, version 2 as published by the @@ -36,7 +36,6 @@ #include "util.h" #include "spindle.h" #include "settings.h" - #include "xio.h" // using Motate::Timeout; @@ -55,64 +54,70 @@ static void _calculate_jerk(mpBuf_t* bf); static void _calculate_vmaxes(mpBuf_t* bf, const float axis_length[], const float axis_square[]); static void _calculate_junction_vmax(mpBuf_t* bf); -//+++++DIAGNOSTICS -//#pragma GCC optimize("O0") // this pragma is required to force the planner to actually set these unused values + +#ifdef __PLANNER_DIAGNOSTICS +#pragma GCC push_options +#pragma GCC optimize("O0") // this pragma is required to force the planner to actually set these unused values static void _set_bf_diagnostics(mpBuf_t* bf) { - bf->linenum = bf->gm.linenum; -// UPDATE_BF_DIAGNOSTICS(bf); //+++++ + UPDATE_BF_DIAGNOSTICS(bf); } -//#pragma GCC reset_options +#pragma GCC reset_options +#else +static void _set_bf_diagnostics(mpBuf_t* bf) {} +#endif /* Runtime-specific setters and getters * * mp_zero_segment_velocity() - correct velocity in last segment for reporting purposes * mp_get_runtime_velocity() - returns current velocity (aggregate) * mp_get_runtime_machine_position() - returns current axis position in machine coordinates - * mp_set_runtime_work_offset() - set offsets in the MR struct - * mp_get_runtime_work_position() - returns current axis position in work coordinates + * mp_set_runtime_display_offset() - set combined display offsets in the MR struct + * mp_get_runtime_display_position() - returns current axis position in work display coordinates * that were in effect at move planning time */ -void mp_zero_segment_velocity() { mr.segment_velocity = 0; } -float mp_get_runtime_velocity(void) { return (mr.segment_velocity); } -float mp_get_runtime_absolute_position(uint8_t axis) { return (mr.position[axis]); } -void mp_set_runtime_work_offset(float offset[]) { copy_vector(mr.gm.work_offset, offset); } +void mp_zero_segment_velocity() { mr->segment_velocity = 0; } +float mp_get_runtime_velocity(void) { return (mr->segment_velocity); } +float mp_get_runtime_absolute_position(mpPlannerRuntime_t *_mr, uint8_t axis) { return (_mr->position[axis]); } +void mp_set_runtime_display_offset(float offset[]) { copy_vector(mr->gm.display_offset, offset); } // We have to handle rotation - "rotate" by the transverse of the matrix to got "normal" coordinates -float mp_get_runtime_work_position(uint8_t axis) { +float mp_get_runtime_display_position(uint8_t axis) { // Shorthand: // target_rotated[0] = a x_1 + b x_2 + c x_3 // target_rotated[1] = a y_1 + b y_2 + c y_3 // target_rotated[2] = a z_1 + b z_2 + c z_3 + z_offset if (axis == AXIS_X) { - return mr.position[0] * cm.rotation_matrix[0][0] + mr.position[1] * cm.rotation_matrix[1][0] + - mr.position[2] * cm.rotation_matrix[2][0] - mr.gm.work_offset[0]; + return mr->position[0] * cm->rotation_matrix[0][0] + mr->position[1] * cm->rotation_matrix[1][0] + + mr->position[2] * cm->rotation_matrix[2][0] - mr->gm.display_offset[0]; } else if (axis == AXIS_Y) { - return mr.position[0] * cm.rotation_matrix[0][1] + mr.position[1] * cm.rotation_matrix[1][1] + - mr.position[2] * cm.rotation_matrix[2][1] - mr.gm.work_offset[1]; + return mr->position[0] * cm->rotation_matrix[0][1] + mr->position[1] * cm->rotation_matrix[1][1] + + mr->position[2] * cm->rotation_matrix[2][1] - mr->gm.display_offset[1]; } else if (axis == AXIS_Z) { - return mr.position[0] * cm.rotation_matrix[0][2] + mr.position[1] * cm.rotation_matrix[1][2] + - mr.position[2] * cm.rotation_matrix[2][2] - cm.rotation_z_offset - mr.gm.work_offset[2]; + return mr->position[0] * cm->rotation_matrix[0][2] + mr->position[1] * cm->rotation_matrix[1][2] + + mr->position[2] * cm->rotation_matrix[2][2] - cm->rotation_z_offset - mr->gm.display_offset[2]; } else { // ABC, UVW, we don't rotate them - return (mr.position[axis] - mr.gm.work_offset[axis]); + return (mr->position[axis] - mr->gm.display_offset[axis]); } } -/* +/**************************************************************************************** * mp_get_runtime_busy() - returns TRUE if motion control busy (i.e. robot is moving) - * mp_runtime_is_idle() - returns TRUE is steppers are not actively moving + * mp_runtime_is_idle() - returns TRUE if steppers are not actively moving * * Use mp_get_runtime_busy() to sync to the queue. If you wait until it returns * FALSE you know the queue is empty and the motors have stopped. */ bool mp_get_runtime_busy() { - if (cm.cycle_state == CYCLE_OFF) { + if (cm->cycle_type == CYCLE_NONE) { return (false); } - if ((st_runtime_isbusy() == true) || (mr.block_state == BLOCK_ACTIVE) || (mb.r->buffer_state > MP_BUFFER_EMPTY)) { + if ((st_runtime_isbusy() == true) || + (mr->block_state == BLOCK_ACTIVE) || + (mp_get_r()->buffer_state > MP_BUFFER_EMPTY)) { return (true); } return (false); @@ -129,26 +134,26 @@ bool mp_runtime_is_idle() { return (!st_runtime_isbusy()); } * Controlling jerk smooths transitions between moves and allows for faster feeds while * controlling machine oscillations and other undesirable side-effects. * - * Note All math is done in absolute coordinates using single precision floating point (float). + * Note: All math is done in absolute coordinates using single precision floating point (float). * * Note: Returning a status that is not STAT_OK means the endpoint is NOT advanced. So lines * that are too short to move will accumulate and get executed once the accumulated error * exceeds the minimums. */ -stat_t mp_aline(GCodeState_t* gm_in) +stat_t mp_aline(GCodeState_t* _gm) { - mpBuf_t* bf; // current move pointer - float target_rotated[AXES] = {0, 0, 0, 0, 0, 0}; - float axis_square[AXES] = {0, 0, 0, 0, 0, 0}; - float axis_length[AXES]; - bool flags[AXES]; + float target_rotated[] = INIT_AXES_ZEROES; + float axis_square[] = INIT_AXES_ZEROES; + float axis_length[] = INIT_AXES_ZEROES; + bool flags[] = INIT_AXES_FALSE; + float length_square = 0; float length; // A few notes about the rotated coordinate space: // These are positions PRE-rotation: - // gm_in.* (anything in gm_in) + // _gm.* (anything in _gm) // // These are positions POST-rotation: // target_rotated (after the rotation here, of course) @@ -163,70 +168,78 @@ stat_t mp_aline(GCodeState_t* gm_in) // a being target[0], // b being target[1], // c being target[2], - // x_1 being cm.rotation_matrix[1][0] + // x_1 being cm->rotation_matrix[1][0] - target_rotated[0] = gm_in->target[0] * cm.rotation_matrix[0][0] + - gm_in->target[1] * cm.rotation_matrix[0][1] + - gm_in->target[2] * cm.rotation_matrix[0][2]; + target_rotated[AXIS_X] = _gm->target[AXIS_X] * cm->rotation_matrix[0][0] + + _gm->target[AXIS_Y] * cm->rotation_matrix[0][1] + + _gm->target[AXIS_Z] * cm->rotation_matrix[0][2]; - target_rotated[1] = gm_in->target[0] * cm.rotation_matrix[1][0] + - gm_in->target[1] * cm.rotation_matrix[1][1] + - gm_in->target[2] * cm.rotation_matrix[1][2]; + target_rotated[AXIS_Y] = _gm->target[AXIS_X] * cm->rotation_matrix[1][0] + + _gm->target[AXIS_Y] * cm->rotation_matrix[1][1] + + _gm->target[AXIS_Z] * cm->rotation_matrix[1][2]; - target_rotated[2] = gm_in->target[0] * cm.rotation_matrix[2][0] + - gm_in->target[1] * cm.rotation_matrix[2][1] + - gm_in->target[2] * cm.rotation_matrix[2][2] + - cm.rotation_z_offset; + target_rotated[AXIS_Z] = _gm->target[AXIS_X] * cm->rotation_matrix[2][0] + + _gm->target[AXIS_Y] * cm->rotation_matrix[2][1] + + _gm->target[AXIS_Z] * cm->rotation_matrix[2][2] + + cm->rotation_z_offset; - // copy rotation axes ABC - target_rotated[3] = gm_in->target[3]; - target_rotated[4] = gm_in->target[4]; - target_rotated[5] = gm_in->target[5]; + // copy rotation axes for UVW (no changes) + target_rotated[AXIS_U] = _gm->target[AXIS_U]; + target_rotated[AXIS_V] = _gm->target[AXIS_V]; + target_rotated[AXIS_W] = _gm->target[AXIS_W]; + + // copy rotation axes for ABC (no changes) + target_rotated[AXIS_A] = _gm->target[AXIS_A]; + target_rotated[AXIS_B] = _gm->target[AXIS_B]; + target_rotated[AXIS_C] = _gm->target[AXIS_C]; for (uint8_t axis = 0; axis < AXES; axis++) { - axis_length[axis] = target_rotated[axis] - mp.position[axis]; + axis_length[axis] = target_rotated[axis] - mp->position[axis]; if ((flags[axis] = fp_NOT_ZERO(axis_length[axis]))) { // yes, this supposed to be = not == axis_square[axis] = square(axis_length[axis]); length_square += axis_square[axis]; } else { axis_length[axis] = 0; // make it truly zero if it was tiny + axis_square[axis] = 0; // Fix bug that can kill feedholds by corrupting block_time in _calculate_times } } length = sqrt(length_square); // exit if the move has zero movement. At all. - if (fp_ZERO(length)) { - sr_request_status_report(SR_REQUEST_TIMED_FULL);// Was SR_REQUEST_IMMEDIATE_FULL - return (STAT_MINIMUM_LENGTH_MOVE); // STAT_MINIMUM_LENGTH_MOVE needed to end cycle +// if (length < 0.00002) { // this value is 2x EPSILON and prevents trap failures in _plan_aline() + if (length < 0.0001) { // this value is 0.1 microns. Prevents planner trap failures + sr_request_status_report(SR_REQUEST_TIMED_FULL); // Was SR_REQUEST_IMMEDIATE_FULL + return (STAT_MINIMUM_LENGTH_MOVE); // STAT_MINIMUM_LENGTH_MOVE needed to end cycle } // get a cleared buffer and copy in the Gcode model state - if ((bf = mp_get_write_buffer()) == NULL) { // never supposed to fail + mpBuf_t* bf = mp_get_write_buffer(); + + if (bf == NULL) { // never supposed to fail return (cm_panic(STAT_FAILED_GET_PLANNER_BUFFER, "aline()")); } - memcpy(&bf->gm, gm_in, sizeof(GCodeState_t)); - // Since bf->gm.target is being used all over the place, we'll make it the rotated target - copy_vector(bf->gm.target, target_rotated); // copy the rotated taget in place + memcpy(&bf->gm, _gm, sizeof(GCodeState_t)); + copy_vector(bf->gm.target, target_rotated); // copy the rotated target in place // setup the buffer bf->bf_func = mp_exec_aline; // register the callback to the exec function - bf->length = length; // record the length + bf->length = length; // record the length for (uint8_t axis = 0; axis < AXES; axis++) { // compute the unit vector and set flags if ((bf->axis_flags[axis] = flags[axis])) { // yes, this is supposed to be = and not == bf->unit[axis] = axis_length[axis] / length;// nb: bf-> unit was cleared by mp_get_write_buffer() } } - _calculate_jerk(bf); // compute bf->jerk values - _calculate_vmaxes(bf, axis_length, axis_square); // compute cruise_vmax and absolute_vmax - _set_bf_diagnostics(bf); //+++++DIAGNOSTIC + _calculate_jerk(bf); // compute bf->jerk values + _calculate_vmaxes(bf, axis_length, axis_square); // compute cruise_vmax and absolute_vmax + _set_bf_diagnostics(bf); // DIAGNOSTIC // Note: these next lines must remain in exact order. Position must update before committing the buffer. - copy_vector(mp.position, bf->gm.target); // set the planner position - mp_commit_write_buffer(BLOCK_TYPE_ALINE); // commit current block (must follow the position update) + copy_vector(mp->position, bf->gm.target); // update the planner position for the next move + mp_commit_write_buffer(BLOCK_TYPE_ALINE); // commit current block (must follow the position update) return (STAT_OK); } -/* +/**************************************************************************************** * mp_plan_block_list() - plan all the blocks in the list * * This parent function is just a dispatcher that reads forward in the list @@ -241,8 +254,8 @@ stat_t mp_aline(GCodeState_t* gm_in) void mp_plan_block_list() { - mpBuf_t* bf = mp.p; - bool planned_something = false; + mpBuf_t* bf = mp->p; + bool planned_something = false; while (true) { // unconditional exit condition @@ -251,36 +264,33 @@ void mp_plan_block_list() } // OK to replan running buffer during feedhold, but no other times (not supposed to happen) - if ((cm.hold_state == FEEDHOLD_OFF) && (bf->buffer_state == MP_BUFFER_RUNNING)) { - mp.p = mp.p->nx; + if ((cm->hold_state == FEEDHOLD_OFF) && (bf->buffer_state == MP_BUFFER_RUNNING)) { + mp->p = mp->p->nx; return; } - - bf = _plan_block(bf); // returns next block to plan - + bf = _plan_block(bf); // returns next block to plan planned_something = true; - mp.p = bf; //+++++ DIAGNOSTIC - this is not needed but is set here for debugging purposes + mp->p = bf; // DIAGNOSTIC - this is not needed but is set here for debugging purposes } - if (mp.planner_state > PLANNER_STARTUP) { - if (planned_something && (cm.hold_state != FEEDHOLD_HOLD)) { + if (mp->planner_state > PLANNER_STARTUP) { + if (planned_something && (cm->hold_state != FEEDHOLD_HOLD)) { st_request_forward_plan(); // start motion if runtime is not already busy } } - mp.p = bf; // update planner pointer + mp->p = bf; // update planner pointer } -/* - * _plan_block() - the block chain using pessimistic assumptions +/**************************************************************************************** + * _plan_block() - stitch and backplan a new block to the planner queue */ static mpBuf_t* _plan_block(mpBuf_t* bf) { // First time blocks - set vmaxes for as many blocks as possible (forward loading of priming blocks) // Note: cruise_vmax was computed in _calculate_vmaxes() in aline() - if (mp.planner_state == PLANNER_PRIMING) { + if (mp->planner_state == PLANNER_PRIMING) { // Timings from *here* - if (bf->pv->plannable) { // calculate junction with previous move _calculate_junction_vmax(bf->pv); // compute maximum junction velocity constraint @@ -290,26 +300,17 @@ static mpBuf_t* _plan_block(mpBuf_t* bf) bf->pv->exit_vmax = std::min(std::min(bf->pv->junction_vmax, bf->pv->cruise_vmax), bf->cruise_vmax); } } - - // calculate junction to stop - _calculate_junction_vmax(bf); // compute maximum junction velocity constraint - if (bf->gm.path_control == PATH_EXACT_STOP) { - bf->exit_vmax = 0; - } else { - bf->exit_vmax = std::min(bf->junction_vmax, bf->cruise_vmax); - } - - _calculate_override(bf); // adjust cruise_vmax for feed/traverse override + _calculate_override(bf); // adjust cruise_vmax for feed/traverse override // bf->plannable_time = bf->pv->plannable_time; // set plannable time - excluding current move - bf->buffer_state = MP_BUFFER_IN_PROCESS; + bf->buffer_state = MP_BUFFER_NOT_PLANNED; + bf->hint = NO_HINT; // ensure we've cleared the hints - bf->hint = NO_HINT; // ensure we've cleared the hints // Time: 12us-41us - if (bf->nx->plannable) { // read in new buffers until EMPTY + if (bf->nx->plannable) { // read in new buffers until EMPTY return (bf->nx); } - mp.planning_return = bf->nx; // where to return after planning is complete - mp.planner_state = PLANNER_BACK_PLANNING; // start backplanning + mp->planning_return = bf->nx; // where to return after planning is complete + mp->planner_state = PLANNER_BACK_PLANNING; // start backplanning } // Backward Planning Pass @@ -318,7 +319,7 @@ static mpBuf_t* _plan_block(mpBuf_t* bf) // Note: Vmax's are already set by the time you get here // Hint options from back-planning: COMMAND_BLOCK, PERFECT_DECELERATION, PERFECT_CRUISE, MIXED_DECELERATION - if (mp.planner_state == PLANNER_BACK_PLANNING) { + if (mp->planner_state == PLANNER_BACK_PLANNING) { // NOTE: We stop when the previous block is no longer plannable. // We will alter the previous block's exit_velocity. float braking_velocity = 0; // we use this to store the previous entry velocity, start at 0 @@ -328,7 +329,7 @@ static mpBuf_t* _plan_block(mpBuf_t* bf) for (; bf->plannable || (braking_velocity < bf->exit_velocity); bf = bf->pv) { // Timings from *here* - bf->iterations++; + INC_PLANNER_ITERATIONS // DIAGNOSTIC bf->plannable = bf->plannable && !optimal; // Don't accidentally enable plannable! // Let's be mindful that forward planning may change exit_vmax, and our exit velocity may be lowered @@ -422,24 +423,24 @@ static mpBuf_t* _plan_block(mpBuf_t* bf) optimal = true; // We can't improve this entry more } - // +++++ + // Exit the loop if we've hit and passed the running buffer. It can happen. if (bf->buffer_state == MP_BUFFER_EMPTY) { - // _debug_trap("Exec apparently cleared this block while we were planning it."); - break; // exit the loop, we've hit and passed the running buffer + break; } - // if (fp_ZERO(bf->exit_velocity) && !fp_ZERO(bf->exit_vmax)) { - // _debug_trap(); // why zero? + + // if (fp_ZERO(bf->exit_velocity) && !fp_ZERO(bf->exit_vmax)) { // DIAGNOSTIC + // debug_trap("_plan_block(): Why is exit velocity zero?"); // } // We might back plan into the running or planned buffer, so we have to check. - if (bf->buffer_state < MP_BUFFER_PREPPED) { - bf->buffer_state = MP_BUFFER_PREPPED; + if (bf->buffer_state < MP_BUFFER_BACK_PLANNED) { + bf->buffer_state = MP_BUFFER_BACK_PLANNED; } } // for loop } // exits with bf pointing to a locked or EMPTY block - mp.planner_state = PLANNER_PRIMING; // revert to initial state - return (mp.planning_return); + mp->planner_state = PLANNER_PRIMING; // revert to initial state + return (mp->planning_return); } /***** ALINE HELPERS ***** @@ -455,30 +456,30 @@ static void _calculate_override(mpBuf_t* bf) // execute ramp to adjust cruise v // TODO: Account for rapid overrides as well as feed overrides // pull in override factor from previous block or seed initial value from the system setting - bf->override_factor = fp_ZERO(bf->pv->override_factor) ? cm.gmx.mfo_factor : bf->pv->override_factor; + bf->override_factor = fp_ZERO(bf->pv->override_factor) ? cm->gmx.mfo_factor : bf->pv->override_factor; bf->cruise_vmax = bf->override_factor * bf->cruise_vset; // generate ramp term is a ramp is active - if (mp.ramp_active) { - bf->override_factor += mp.ramp_dvdt * bf->block_time; - if (mp.ramp_dvdt > 0) { // positive is an acceleration ramp - if (bf->override_factor > mp.ramp_target) { - bf->override_factor = mp.ramp_target; - mp.ramp_active = false; // detect end of ramp + if (mp->ramp_active) { + bf->override_factor += mp->ramp_dvdt * bf->block_time; + if (mp->ramp_dvdt > 0) { // positive is an acceleration ramp + if (bf->override_factor > mp->ramp_target) { + bf->override_factor = mp->ramp_target; + mp->ramp_active = false; // detect end of ramp } bf->cruise_velocity *= bf->override_factor; if (bf->cruise_velocity > bf->absolute_vmax) { // test max cruise_velocity bf->cruise_velocity = bf->absolute_vmax; - mp.ramp_active = false; // don't allow exceeding absolute_vmax + mp->ramp_active = false; // don't allow exceeding absolute_vmax } } else { // negative is deceleration ramp - if (bf->override_factor < mp.ramp_target) { - bf->override_factor = mp.ramp_target; - mp.ramp_active = false; + if (bf->override_factor < mp->ramp_target) { + bf->override_factor = mp->ramp_target; + mp->ramp_active = false; } - bf->cruise_velocity *= bf->override_factor; // +++++ this is probably wrong + bf->cruise_velocity *= bf->override_factor; // +++++ this is probably wrong // bf->exit_velocity *= bf->mfo_factor; //...but I'm not sure this is right, - // bf->cruise_velocity = bf->pv->exit_velocity; //...either + // bf->cruise_velocity = bf->pv->exit_velocity;//...either } } else { bf->cruise_velocity *= bf->override_factor; // apply original or changed factor @@ -497,7 +498,7 @@ static void _calculate_override(mpBuf_t* bf) // execute ramp to adjust cruise v // } } -/* +/**************************************************************************************** * _calculate_jerk() - calculate jerk given the dynamic state * * Set the jerk scaling to the lowest axis with a non-zero unit vector. @@ -513,13 +514,13 @@ static float _get_axis_jerk(mpBuf_t* bf, uint8_t axis) switch (bf->gm.motion_mode) { case MOTION_MODE_STRAIGHT_TRAVERSE: //case MOTION_MODE_STRAIGHT_PROBE: // <-- not sure on this one - return cm.a[axis].jerk_high; + return cm->a[axis].jerk_high; break; default: - return cm.a[axis].jerk_max; + return cm->a[axis].jerk_max; } #else - return cm.a[axis].jerk_max; + return cm->a[axis].jerk_max; #endif } @@ -550,7 +551,7 @@ static void _calculate_jerk(mpBuf_t* bf) bf->q_recip_2_sqrt_j = q / (2.0 * sqrt_j); } -/* +/**************************************************************************************** * _calculate_vmaxes() - compute cruise_vmax and absolute_vmax based on velocity constraints * * The following feeds and times are compared and the longest (slowest velocity) is returned: @@ -609,6 +610,24 @@ static void _calculate_jerk(mpBuf_t* bf) * so that the elapsed time from the start to the end of the motion is T plus * any time required for acceleration or deceleration. */ +/* + * Axis Decoupling Notes + * + * Use cases: + * - 3D mode (Machining) - All XYZABC axes must obey full jerk constraints + * - 2.5d mode (Contouring) - Treat as 3D mode (at least for now) + * - 2D mode (Routing) - Routing moves: XY obey jerk, Z movement does not affect move time + * + * - Case 1 - Z tab move w/XY movement - Z movement below 'delta' threshold + * Z is allowed to move at indicated velocity without slowing down XY (decoupled) + * + * - Case 2 - Z climb or plunge w/XY movement - Z movement above 'delta' threshold + * - Case 3 - Z climb or plunge w/o X or Y movement - Z must obey velocity/jerk constraints + * + * Z side loading factor - what's the maximum lateral force that be applied to the cutter? + * F = ma + * S(z) / V(xy) + */ static void _calculate_vmaxes(mpBuf_t* bf, const float axis_length[], const float axis_square[]) { float feed_time = 0; // one of: XYZ time, ABC time or inverse time. Mutually exclusive @@ -620,13 +639,13 @@ static void _calculate_vmaxes(mpBuf_t* bf, const float axis_length[], const floa // compute feed time for feeds and probe motion if (bf->gm.motion_mode != MOTION_MODE_STRAIGHT_TRAVERSE) { if (bf->gm.feed_rate_mode == INVERSE_TIME_MODE) { - feed_time = bf->gm.feed_rate; // NB: feed rate was un-inverted to minutes by cm_set_feed_rate() + feed_time = bf->gm.feed_rate; // NB: feed rate was un-inverted to minutes by cm_set_feed_rate() bf->gm.feed_rate_mode = UNITS_PER_MINUTE_MODE; } else { // compute length of linear move in millimeters. Feed rate is provided as mm/min feed_time = sqrt(axis_square[AXIS_X] + axis_square[AXIS_Y] + axis_square[AXIS_Z]) / bf->gm.feed_rate; - // if no linear axes, compute length of multi-axis rotary move in degrees. Feed rate is provided as - // degrees/min + // if no linear axes, compute length of multi-axis rotary move in degrees. + // Feed rate is provided as degrees/min if (fp_ZERO(feed_time)) { feed_time = sqrt(axis_square[AXIS_A] + axis_square[AXIS_B] + axis_square[AXIS_C]) / bf->gm.feed_rate; } @@ -636,9 +655,9 @@ static void _calculate_vmaxes(mpBuf_t* bf, const float axis_length[], const floa for (uint8_t axis = AXIS_X; axis < AXES; axis++) { if (bf->axis_flags[axis]) { if (bf->gm.motion_mode == MOTION_MODE_STRAIGHT_TRAVERSE) { - tmp_time = std::abs(axis_length[axis]) / cm.a[axis].velocity_max; - } else { // gm.motion_mode == MOTION_MODE_STRAIGHT_FEED - tmp_time = std::abs(axis_length[axis]) / cm.a[axis].feedrate_max; + tmp_time = std::abs(axis_length[axis]) / cm->a[axis].velocity_max; + } else {// gm.motion_mode == MOTION_MODE_STRAIGHT_FEED + tmp_time = std::abs(axis_length[axis]) / cm->a[axis].feedrate_max; } max_time = max(max_time, tmp_time); @@ -655,7 +674,7 @@ static void _calculate_vmaxes(mpBuf_t* bf, const float axis_length[], const floa bf->block_time = block_time; // initial estimate - used for ramp computations } -/* +/**************************************************************************************** * _calculate_junction_vmax() - Giseburt's Algorithm ;-) * * Computes the maximum allowable junction speed by finding the velocity that will not @@ -691,7 +710,7 @@ static void _calculate_vmaxes(mpBuf_t* bf, const float axis_length[], const floa * To deal with this, we keep track of the unit vector 0.5mm back, which may be in another move. We then use that * We will use the max delta between the current vector and that vector or that of the next move. * - * C) The last move (there is not "next move" yet) will have to compate to a unit vector of zero. + * C) The last move (there is not "next move" yet) will have to compute to a unit vector of zero. */ static void _calculate_junction_vmax(mpBuf_t* bf) @@ -707,7 +726,7 @@ static void _calculate_junction_vmax(mpBuf_t* bf) // if (delta > EPSILON) { - velocity = std::min(velocity, ((cm.a[axis].max_junction_accel * _get_axis_jerk(bf, axis)) / delta)); // formula (2) + velocity = std::min(velocity, ((cm->a[axis].max_junction_accel * _get_axis_jerk(bf, axis)) / delta)); // formula (2) } } } @@ -748,7 +767,7 @@ static void _calculate_junction_vmax(mpBuf_t* bf) // (A) special case handling if (delta > EPSILON) { - velocity = std::min(velocity, ((cm.a[axis].max_junction_accel * _get_axis_jerk(bf, axis)) / delta)); // formula (2) + velocity = std::min(velocity, ((cm->a[axis].max_junction_accel * _get_axis_jerk(bf, axis)) / delta)); // formula (2) } } } diff --git a/g2core/plan_zoid.cpp b/g2core/plan_zoid.cpp index 8bfe902a..889eeef9 100644 --- a/g2core/plan_zoid.cpp +++ b/g2core/plan_zoid.cpp @@ -2,8 +2,8 @@ * plan_zoid.cpp - acceleration managed line planning and motion execution - trapezoid planner * This file is part of the g2core project * - * Copyright (c) 2010 - 2016 Alden S. Hart, Jr. - * Copyright (c) 2012 - 2016 Rob Giseburt + * Copyright (c) 2010 - 2019 Alden S. Hart, Jr. + * Copyright (c) 2012 - 2019 Rob Giseburt * * This file ("the software") is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License, version 2 as published by the @@ -31,36 +31,37 @@ #include "planner.h" #include "report.h" #include "util.h" +//#include "xio.h" // only need for_ramp_exit_logger -//+++++ DIAGNOSTICS +// DIAGNOSTICS -//#if IN_DEBUGGER < 1 -#define LOG_RETURN(msg) // LOG_RETURN with no action (production) -//#else -//#include "xio.h" -//static char logbuf[128]; -//static void _logger(const char *msg, const mpBuf_t *bf) // LOG_RETURN with full state dump -//{ -// sprintf(logbuf, "[%2d] %s (%d) mt:%5.2f, L:%1.3f [%1.3f, %1.3f, %1.3f] V:[%1.2f, %1.2f, %1.2f]\n", -// bf->buffer_number, msg, bf->hint, (bf->block_time * 60000), -// bf->length, bf->head_length, bf->body_length, bf->tail_length, -// bf->pv->exit_velocity, bf->cruise_velocity, bf->exit_velocity); -// xio_writeline(logbuf); -//} -//#define LOG_RETURN(msg) { _logger(msg, bf); } -//#endif - -#if IN_DEBUGGER < 1 -#define TRAP_ZERO(t,m) -#else -#define TRAP_ZERO(t, m) \ - if (fp_ZERO(t)) { \ - rpt_exception(STAT_MINIMUM_LENGTH_MOVE, m); \ - _debug_trap(m); \ +stat_t _ramp_exit_logger(mpBuf_t* bf, const char *msg) +{ + #ifdef __PLANNER_DIAGNOSTICS + if (mp_runtime_is_idle()) { // normally the runtime keeps this value fresh +// bf->time_in_plan_ms += bf->block_time_ms; + bf->plannable_time_ms += bf->block_time_ms;\ } -#endif + #endif -//+++++ END DIAGNOSTICS +/* insert logger functions here if needed: + + // LOG_RETURN with full state dump + #if IN_DEBUGGER == 1 + + static char logbuf[128]; + + sprintf(logbuf, "[%2d] %s (%d) mt:%5.2f, L:%1.3f [%1.3f, %1.3f, %1.3f] V:[%1.2f, %1.2f, %1.2f]\n", + bf->buffer_number, msg, bf->hint, (bf->block_time * 60000), + bf->length, bf->head_length, bf->body_length, bf->tail_length, + bf->pv->exit_velocity, bf->cruise_velocity, bf->exit_velocity); + xio_writeline(logbuf); + #endif +*/ + return (STAT_OK); +} + +// END DIAGNOSTICS /* local functions */ @@ -114,32 +115,20 @@ static float _get_meet_velocity(const float v_0, * */ -void _zoid_exit(mpBuf_t* bf, zoidExitPoint exit_point) HOT_FUNC; -void _zoid_exit(mpBuf_t* bf, zoidExitPoint exit_point) -{ - //+++++ DIAGNOSTIC - // bf->zoid_exit = exit_point; - if (mp_runtime_is_idle()) { // normally the runtime keeps this value fresh - // bf->time_in_plan_ms += bf->block_time_ms; - bf->plannable_time_ms += bf->block_time_ms; - } -} - - // Hint will be one of these from back-planning: COMMAND_BLOCK, PERFECT_DECELERATION, PERFECT_CRUISE, // MIXED_DECELERATION, ASYMMETRIC_BUMP // We are incorporating both the forward planning and ramp-planning into one function, since we use the same data. -void mp_calculate_ramps(mpBlockRuntimeBuf_t* block, mpBuf_t* bf, const float entry_velocity) +stat_t mp_calculate_ramps(mpBlockRuntimeBuf_t* block, mpBuf_t* bf, const float entry_velocity) { // *** Skip non-move commands *** if (bf->block_type == BLOCK_TYPE_COMMAND) { bf->hint = COMMAND_BLOCK; - return; + return (STAT_NOOP); // NOOP status is informative, not actionable } - TRAP_ZERO(bf->length, "zoid() got L=0"); //+++++ Move this outside of zoid - TRAP_ZERO(bf->cruise_velocity, "zoid() got Vc=0"); // move this outside - - // Timing from *here* + debug_trap_if_zero(bf->length, "mp_calculate_ramps() - got L=0"); + debug_trap_if_zero(bf->cruise_velocity, "mp_calculate_ramps() - got Vc=0"); + + // Timings from *here* // initialize parameters to know values block->head_time = 0; @@ -150,12 +139,16 @@ void mp_calculate_ramps(mpBlockRuntimeBuf_t* block, mpBuf_t* bf, const float ent block->body_length = 0; block->tail_length = 0; - block->cruise_velocity = min(bf->cruise_velocity, bf->cruise_vmax); + // these conditions should have been met earlier, but if they are not trap and correct them + debug_trap_if_true((bf->exit_velocity > bf->exit_vmax), "mp_calculate_ramps() - Vexit > Vexit_max"); block->exit_velocity = min(bf->exit_velocity, bf->exit_vmax); + // +++++ THIS WILL NEED TO CHANGE TO SUPPORT OVERRIDES +// debug_trap_if_true((bf->cruise_velocity, bf->cruise_vmax), "mp_calculate_ramps() - Vcruise > Vcruise_max"); + block->cruise_velocity = min(bf->cruise_velocity, bf->cruise_vmax); - // We *might* do this exact computation later, so cache the value. - float test_velocity = 0; + // We *might* do this exact computation later, so cache the value + float test_velocity = 0; bool test_velocity_valid = false; // record if we have a validly cached value // *** Perfect-Fit Cases (1) *** Cases where curve fitting has already been done @@ -165,7 +158,7 @@ void mp_calculate_ramps(mpBlockRuntimeBuf_t* block, mpBuf_t* bf, const float ent // Here we verify it moving forward, checking to make sure it still is true. // If so, we plan the "ramp" as flat, body-only. if (bf->hint == PERFECT_CRUISE) { - if ((!mp.entry_changed) && fp_EQ(entry_velocity, bf->cruise_vmax)) { + if ((!mp->entry_changed) && fp_EQ(entry_velocity, bf->cruise_vmax)) { // We need to ensure that neither the entry or the exit velocities are // <= the cruise velocity even though there is tolerance in fp_EQ comparison. block->exit_velocity = entry_velocity; @@ -174,29 +167,25 @@ void mp_calculate_ramps(mpBlockRuntimeBuf_t* block, mpBuf_t* bf, const float ent block->body_length = bf->length; block->body_time = block->body_length / block->cruise_velocity; bf->block_time = block->body_time; - - LOG_RETURN("1c"); - return (_zoid_exit(bf, ZOID_EXIT_1c)); - } else { - // we need to degrade the hint to MIXED_ACCELERATION + return (_ramp_exit_logger(bf, "1c")); + } + else { // degrade the hint to MIXED_ACCELERATION bf->hint = MIXED_ACCELERATION; } } - // Quick test to ensure we haven't violated the hint - if (entry_velocity > block->exit_velocity) { - // We're in a deceleration. - if (mp.entry_changed) { - // If entry_changed, then entry_velocity is lower than the hints expect. - // A deceleration will never become an acceleration (post-hinting). - // If it is marked as MIXED_DECELERATION, it means the entry was CRUISE_VMAX. - // If it is marked as PERFECT_DECELERATION, it means the entry was as <= CRUISE_VMAX, - // but as high as possible. - // So, this move is and was a DECELERATION, meaning it *could* achieve the previous (higher) - // entry safely, it will likely get a head section, so we will now degrade the hint to an - // ASYMMETRIC_BUMP. + if (entry_velocity > block->exit_velocity) { // Means this is a deceleration + // If entry_changed then entry_velocity is lower than the hints expect + // A deceleration will never become an acceleration (post-hinting) + // If it is marked as MIXED_DECELERATION, it means the entry was CRUISE_VMAX + // If it is marked as PERFECT_DECELERATION, it means the entry was as <= CRUISE_VMAX, + // but as high as possible + // So, this move is and was a DECELERATION, meaning it *could* achieve the previous (higher) + // entry safely, it will likely get a head section, so we will now degrade the hint to an + // ASYMMETRIC_BUMP + if (mp->entry_changed) { bf->hint = ASYMMETRIC_BUMP; } @@ -210,8 +199,7 @@ void mp_calculate_ramps(mpBlockRuntimeBuf_t* block, mpBuf_t* bf, const float ent block->body_time = block->body_length / block->cruise_velocity; block->tail_time = block->tail_length * 2 / (block->exit_velocity + block->cruise_velocity); bf->block_time = block->body_time + block->tail_time; - LOG_RETURN("2d"); - return (_zoid_exit(bf, ZOID_EXIT_2d)); + return (_ramp_exit_logger(bf, "2d")); } // PERFECT_DECELERATION (1d) single tail segment (deltaV == delta_vmax) @@ -221,19 +209,16 @@ void mp_calculate_ramps(mpBlockRuntimeBuf_t* block, mpBuf_t* bf, const float ent block->cruise_velocity = entry_velocity; block->tail_time = block->tail_length * 2 / (block->exit_velocity + block->cruise_velocity); bf->block_time = block->tail_time; - LOG_RETURN("1d"); - return (_zoid_exit(bf, ZOID_EXIT_1d)); + return (_ramp_exit_logger(bf, "1d")); } - // Reset entry_changed. We won't likely be changing the next block's entry velocity. - mp.entry_changed = false; - + mp->entry_changed = false; // Since we are not generally decelerating, this is effectively all of forward planning that we need. } else { - // Note that the hints from back-planning are ignored in this section, since back-planing can only predict decel - // and cruise. + // Note that the hints from back-planning are ignored in this section, since back-planing can only + // predict decelerations and cruises float accel_velocity; if (test_velocity_valid) { @@ -245,7 +230,7 @@ void mp_calculate_ramps(mpBlockRuntimeBuf_t* block, mpBuf_t* bf, const float ent if (accel_velocity < block->exit_velocity) { // still accelerating - mp.entry_changed = true; // we are changing the *next* block's entry velocity + mp->entry_changed = true; // we are changing the *next* block's entry velocity block->exit_velocity = accel_velocity; block->cruise_velocity = accel_velocity; @@ -257,11 +242,11 @@ void mp_calculate_ramps(mpBlockRuntimeBuf_t* block, mpBuf_t* bf, const float ent block->cruise_velocity = block->exit_velocity; block->head_time = (block->head_length * 2.0) / (entry_velocity + block->cruise_velocity); bf->block_time = block->head_time; - LOG_RETURN("1a"); - return (_zoid_exit(bf, ZOID_EXIT_1a)); - } else { // it's hit the cusp + return (_ramp_exit_logger(bf, "1a")); + } + else { // it's hit the cusp - mp.entry_changed = false; // we are NOT changing the next block's entry velocity + mp->entry_changed = false; // we are NOT changing the next block's entry velocity block->cruise_velocity = bf->cruise_vmax; @@ -282,8 +267,7 @@ void mp_calculate_ramps(mpBlockRuntimeBuf_t* block, mpBuf_t* bf, const float ent block->head_time = (block->head_length * 2.0) / (entry_velocity + block->cruise_velocity); block->body_time = block->body_length / block->cruise_velocity; bf->block_time = block->head_time + block->body_time; - LOG_RETURN("2a"); - return (_zoid_exit(bf, ZOID_EXIT_2a)); + return (_ramp_exit_logger(bf, "2a")); } } } @@ -315,9 +299,7 @@ void mp_calculate_ramps(mpBlockRuntimeBuf_t* block, mpBuf_t* bf, const float ent bf->block_time = block->head_time + block->body_time + block->tail_time; bf->hint = ASYMMETRIC_BUMP; - - LOG_RETURN("2c"); - return (_zoid_exit(bf, ZOID_EXIT_2c)); + return (_ramp_exit_logger(bf, "2c")); } // *** Rate-Limited-Fit cases (3) *** @@ -326,7 +308,7 @@ void mp_calculate_ramps(mpBlockRuntimeBuf_t* block, mpBuf_t* bf, const float ent // Rate-limited asymmetric cases (3) // compute meet velocity to see if the cruise velocity rises above the entry and/or exit velocities block->cruise_velocity = _get_meet_velocity(entry_velocity, block->exit_velocity, bf->length, bf, block); - TRAP_ZERO(block->cruise_velocity, "zoid() Vc=0 asymmetric HT case"); + debug_trap_if_zero(block->cruise_velocity, "mp_calculate_ramps() Vc=0 asymmetric HT case"); // We now store the head/tail lengths we computed in _get_meet_velocity. // treat as a full up and down (head and tail) @@ -345,12 +327,9 @@ void mp_calculate_ramps(mpBlockRuntimeBuf_t* block, mpBuf_t* bf, const float ent block->tail_time = (block->tail_length * 2.0) / (block->exit_velocity + block->cruise_velocity); } bf->block_time = block->head_time + block->body_time + block->tail_time; - - LOG_RETURN("3c"); - return (_zoid_exit(bf, ZOID_EXIT_3c)); // 550us worst case + return (_ramp_exit_logger(bf, "3c")); // 550us worst case } - /**** Planner helpers **** * * mp_get_target_length() - find accel/decel length from delta V and jerk @@ -370,7 +349,7 @@ void mp_calculate_ramps(mpBlockRuntimeBuf_t* block, mpBuf_t* bf, const float ent */ // Just calling this tl_constant. It's full name is: -// static const float tl_constant = 1.201405707067378; // sqrt(5)/( sqrt(2)pow(3,4) ) +// static const float tl_constant = 1.201405707067378; // sqrt(5)/( sqrt(2)pow(3,4) ) float mp_get_target_length(const float v_0, const float v_1, const mpBuf_t* bf) { @@ -379,10 +358,10 @@ float mp_get_target_length(const float v_0, const float v_1, const mpBuf_t* bf) } /* - * mp_get_target_velocity() - find the velocity we would achieve if we *accelerated* from v_0 + * mp_get_target_velocity() - find the velocity we would achieve if we accelerated from v_0 * - * Get "the velocity" that we would end up at if we *accelerated* from v_0 - * over the provided L (length) and J (jerk, provided in the bf structure). + * Get the velocity that we would end up at if we accelerated from v_0 + * over the provided L (length) and J (jerk, provided in the bf structure) */ // 14 *, 1 /, 1 sqrt, 1 cbrt @@ -395,16 +374,16 @@ float mp_get_target_velocity(const float v_0, const float L, const mpBuf_t* bf) const float j = bf->jerk; - const float a80 = 7.698003589195; // 80 * a - const float a_2 = 0.00925925925926; // a^2 + const float a80 = 7.698003589195; // 80 * a + const float a_2 = 0.00925925925926; // a^2 - const float v_0_2 = v_0 * v_0; // v_0^2 - const float v_0_3 = v_0_2 * v_0; // v_0^3 + const float v_0_2 = v_0 * v_0; // v_0^2 + const float v_0_3 = v_0_2 * v_0; // v_0^3 - const float L_2 = L * L; // L^2 + const float L_2 = L * L; // L^2 - const float b_part1 = 9 * j * L_2; // 9 j L^2 - const float b_part2 = a80 * v_0_3; // 80 a v_0^3 + const float b_part1 = 9 * j * L_2; // 9 j L^2 + const float b_part2 = a80 * v_0_3; // 80 a v_0^3 // b^3 = a^2 (3 L sqrt(j (2 b_part2 + b_part1)) + b_part2 + b_part1) const float b_cubed = a_2 * (3 * L * sqrt(j * (2 * b_part2 + b_part1)) + b_part2 + b_part1); @@ -420,54 +399,65 @@ float mp_get_target_velocity(const float v_0, const float L, const mpBuf_t* bf) return std::abs(v_1); } - /* * mp_get_decel_velocity() - mp_get_target_velocity but ONLY for deceleration * - * Get "the velocity" that we would end up at if we *decelerated* from v_0, - * over the provided L (length) and J (jerk, provided in the bf structure). + * Get the velocity that we would end up at if we decelerated from v_0, + * over the provided L (length) and J (jerk, provided in the bf structure). * - * We have to use a root finding solution, since there is actually three possible - * solutions. We can eliminate one quickly, since it's the acceleration case. + * We have to use a root finding solution, since there are actually three possible + * solutions. We can eliminate one quickly, since it's the acceleration case. * - * The other two cases are occasionally the same value, but this is rare. - * Otherwise there is a "high" value and a "low" value. The low value can be - * negative and then eliminated. + * The other two cases are occasionally the same value, but this is rare. + * Otherwise there is a "high" value and a "low" value. The low value can be + * negative and then eliminated. + * + * This function may generate minor errors in target velocity, and should only + * be used to compute feedholds or other cases where exact velocity is not mandatory. + * + * This function can fail if the length is too short to get a good answer. + * Failures return (float)-1.0 Negative velocities should never be returned. */ float mp_get_decel_velocity(const float v_0, const float L, const mpBuf_t* bf) { const float q_recip_2_sqrt_j = bf->q_recip_2_sqrt_j; + float v_1 = 0; // start the guess at zero - float v_1 = 0; // start the guess at zero - bool first_pass = true; // We may invert our guess based on the first pass results. + int i = 0; // limit the iterations + while (i++ < 20) { // If it fails after 20 iterations something's wrong - int i = 0; // limit the iterations - while (i++ < 10) { // If it fails after 10, something's wrong + // l_t is the difference in length between the L provided and the current guessed deceleration length const float sqrt_delta_v_0 = sqrt(v_0 - v_1); - const float l_t = q_recip_2_sqrt_j * (sqrt_delta_v_0 * (v_1 + v_0)) - L; + const float l_t = q_recip_2_sqrt_j * (sqrt_delta_v_0 * (v_1 + v_0)) - L; - if (std::abs(l_t) < 0.00001) { + // The return condition allows a minor error in length (in mm). + // Note: This comparison does NOT affect actual lengths or steps, which would be bad. + // The actual lengths traveled must be controlled by the caller. + if (std::abs(l_t) < 0.001) { break; } - // For the first pass, we tested velocity 0. - // If velocity 0 yields a l_t > zero, then we need to start searching - // at v_1 instead. (We can't start AT v_1, so we start at v_1 - 0.1.) - if (first_pass && (l_t > 0)) { - v_1 = v_0 - 0.001; - + // For the first pass we tested velocity 0. If velocity 0 yields a l_t > 0, + // then we need to start searching at v_1 instead. + // (We can't start AT v_1, so we start at v_1 - 0.1) + if (i==1 && (l_t > 0)) { + v_1 = v_0 - 0.1; continue; } - - const float v_1x3 = 3 * v_1; + const float v_1x3 = 3 * v_1; const float recip_l_t = (2 * sqrt_delta_v_0) / ((v_0 - v_1x3) * q_recip_2_sqrt_j); - v_1 = v_1 - (l_t * recip_l_t); + + // In some extreme cases there is no solution because the length is too short + if (v_1 > v_0) { + return (-1.0); // cannot decelerate. Return an error + } } - return v_1; } +//Is there a way to derive the average slope of a deceleration given the starting velocity, length and jerk? We don't need the + /* * _get_meet_velocity() - find intersection velocity * @@ -501,9 +491,7 @@ static float _get_meet_velocity(const float v_0, block->head_length = L / 2.0; block->body_length = 0; block->tail_length = L - block->head_length; - - bf->meet_iterations = -1; - + SET_PLANNER_ITERATIONS(-1); // DIAGNOSTIC return v_1; } @@ -525,7 +513,7 @@ static float _get_meet_velocity(const float v_0, if (block->head_length > L) { block->head_length = L; block->body_length = 0; - v_1 = mp_get_target_velocity(v_0, L, bf); + v_1 = mp_get_target_velocity(v_0, L, bf); } else { block->body_length = L - block->head_length; } @@ -537,7 +525,7 @@ static float _get_meet_velocity(const float v_0, if (block->tail_length > L) { block->tail_length = L; block->body_length = 0; - v_1 = mp_get_target_velocity(v_2, L, bf); + v_1 = mp_get_target_velocity(v_2, L, bf); } else { block->body_length = L - block->tail_length; } @@ -587,8 +575,6 @@ static float _get_meet_velocity(const float v_0, v_1 = v_1 - (l_c * recip_l_d); } - - bf->meet_iterations = i; - + SET_MEET_ITERATIONS(i); // DIAGNOSTIC return v_1; } diff --git a/g2core/planner.cpp b/g2core/planner.cpp index f99762b3..3563b49a 100644 --- a/g2core/planner.cpp +++ b/g2core/planner.cpp @@ -2,8 +2,8 @@ * planner.cpp - Cartesian trajectory planning and motion execution * This file is part of the g2core project * - * Copyright (c) 2010 - 2016 Alden S. Hart, Jr. - * Copyright (c) 2012 - 2016 Rob Giseburt + * Copyright (c) 2010 - 2019 Alden S. Hart, Jr. + * Copyright (c) 2012 - 2019 Rob Giseburt * * This file ("the software") is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License, version 2 as published by the @@ -65,9 +65,29 @@ #include "xio.h" //+++++ DIAGNOSTIC - only needed if xio_writeline() direct prints are used // Allocate planner structures -mpBufferPool_t mb; // buffer pool management -mpMotionPlannerSingleton_t mp; // context for block planning -mpMotionRuntimeSingleton_t mr; // context for block runtime + +mpPlanner_t *mp; // currently active planner (global variable) +mpPlanner_t mp1; // primary planning context +mpPlanner_t mp2; // secondary planning context + +mpPlannerRuntime_t *mr; // context for planner block runtime +mpPlannerRuntime_t mr1; // primary planner runtime context +mpPlannerRuntime_t mr2; // secondary planner runtime context + +mpBuf_t mp1_queue[PLANNER_QUEUE_SIZE]; // storage allocation for primary planner queue buffers +mpBuf_t mp2_queue[SECONDARY_QUEUE_SIZE]; // storage allocation for secondary planner queue buffers + +// Execution routines (NB: These are called from the LO interrupt) +static stat_t _exec_dwell(mpBuf_t *bf); +static stat_t _exec_command(mpBuf_t *bf); + +// DIAGNOSTICS +//static void _planner_time_accounting(); +static void _audit_buffers(); + +/**************************************************************************************** + * JSON planner objects + */ #define JSON_COMMAND_BUFFER_SIZE 3 @@ -85,7 +105,6 @@ struct _json_commands_t { int8_t available; // Constructor (initializer) - // Note, reset routines handle zeroing out all of the above _json_commands_t() { json_command_buffer_t *js_pv = &_json_bf[JSON_COMMAND_BUFFER_SIZE - 1]; for (uint8_t i=0; i < JSON_COMMAND_BUFFER_SIZE; i++) { @@ -93,9 +112,7 @@ struct _json_commands_t { _json_bf[i].pv = js_pv; js_pv = &_json_bf[i]; } - _json_r = &_json_bf[0]; - _json_w = _json_r; - available = JSON_COMMAND_BUFFER_SIZE; + reset(); }; // Write a json command to the buffer, using up one slot @@ -115,139 +132,150 @@ struct _json_commands_t { _json_r = _json_r->nx; available++; } -}; + // Reset the JSON command queue + void reset() { + _json_r = &_json_bf[0]; + _json_w = _json_r; + available = JSON_COMMAND_BUFFER_SIZE; + } +}; _json_commands_t jc; -// Local Scope Data and Functions -#define spindle_speed block_time // local alias for spindle_speed to the time variable -#define value_vector gm.target // alias for vector of values - -//static void _planner_time_accounting(); -static void _audit_buffers(); - -// Execution routines (NB: These are called from the LO interrupt) -static void _exec_json_command(float *value, bool *flag); -static stat_t _exec_dwell(mpBuf_t *bf); -static stat_t _exec_command(mpBuf_t *bf); -static stat_t _exec_json_wait(mpBuf_t *bf); - -/* - * planner_init() - * planner_reset() - */ -void planner_init() -{ -// If you know all memory has been zeroed by a hard reset you don't need these next 2 lines - memset(&mp, 0, sizeof(mp)); // clear all values, pointers and status - memset(&mr, 0, sizeof(mr)); // clear all values, pointers and status - planner_init_assertions(); - mp_init_buffers(); - mp.mfo_factor = 1.00; -} - -void planner_reset() -{ - planner_init(); -} - -/* - * planner_init_assertions() - * planner_test_assertions() - test assertions, PANIC if violation exists +/**************************************************************************************** + * planner_init() - initialize MP, MR and planner queue buffers + * planner_reset() - selective reset MP and MR structures + * planner_assert() - test planner assertions, PANIC if violation exists */ -void planner_init_assertions() +// initialize a planner queue +void _init_planner_queue(mpPlanner_t *_mp, mpBuf_t *queue, uint8_t size) { - // Note: mb magic numbers set up by mp_init_buffers() - mp.magic_start = MAGICNUM; - mp.magic_end = MAGICNUM; - mr.magic_start = MAGICNUM; - mr.magic_end = MAGICNUM; -} + mpBuf_t *pv, *nx; + uint8_t i, nx_i; + mpPlannerQueue_t *q = &(_mp->q); -stat_t planner_test_assertions() -{ - if ((BAD_MAGIC(mb.magic_start)) || (BAD_MAGIC(mb.magic_end)) || - (BAD_MAGIC(mp.magic_start)) || (BAD_MAGIC(mp.magic_end)) || - (BAD_MAGIC(mr.magic_start)) || (BAD_MAGIC(mr.magic_end))) { - return(cm_panic(STAT_PLANNER_ASSERTION_FAILURE, "planner_test_assertions()")); + memset(q, 0, sizeof(mpPlannerQueue_t)); // clear values, pointers and status + q->magic_start = MAGICNUM; + q->magic_end = MAGICNUM; + + memset(queue, 0, sizeof(mpBuf_t)*size); // clear all buffers in queue + q->bf = queue; // link the buffer pool first + q->w = queue; // init all buffer pointers + q->r = queue; + q->queue_size = size; + q->buffers_available = size; + + pv = &q->bf[size-1]; + for (i=0; i < size; i++) { + q->bf[i].buffer_number = i; // number is for diagnostics only (otherwise not used) + nx_i = ((ibf[nx_i]; + q->bf[i].nx = nx; // setup circular list pointers + q->bf[i].pv = pv; + pv = &q->bf[i]; + } + q->bf[size-1].nx = queue; +} + +void planner_init(mpPlanner_t *_mp, mpPlannerRuntime_t *_mr, mpBuf_t *queue, uint8_t queue_size) +{ + // init planner master structure + memset(_mp, 0, sizeof(mpPlanner_t)); // clear all values, pointers and status + _mp->magic_start = MAGICNUM; // set boundary condition assertions + _mp->magic_end = MAGICNUM; + _mp->mfo_factor = 1.00; + + // init planner queues + _mp->q.bf = queue; // assign puffer pool to queue manager structure + _init_planner_queue(_mp, queue, queue_size); + + // init runtime structs + _mp->mr = _mr; + memset(_mr, 0, sizeof(mpPlannerRuntime_t)); // clear all values, pointers and status + _mr->magic_start = MAGICNUM; // mr assertions + _mr->magic_end = MAGICNUM; + + _mr->block[0].nx = &_mr->block[1]; // Handle the two "stub blocks" in the runtime structure + _mr->block[1].nx = &_mr->block[0]; + _mr->r = &_mr->block[0]; + _mr->p = &_mr->block[1]; +} + +void planner_reset(mpPlanner_t *_mp) // reset planner queue, cease MR activity, but leave positions alone +{ + // selectively reset mpPlanner and mpPlannerRuntime w/o actually wiping them + _mp->reset(); + _mp->mr->reset(); + jc.reset(); + _init_planner_queue(_mp, _mp->q.bf, _mp->q.queue_size); // reset planner buffers +} + +stat_t planner_assert(const mpPlanner_t *_mp) +{ + if ((BAD_MAGIC(_mp->magic_start)) || (BAD_MAGIC(_mp->magic_end)) || + (BAD_MAGIC(_mp->mr->magic_start)) || (BAD_MAGIC(_mp->mr->magic_end))) { + return (cm_panic(STAT_PLANNER_ASSERTION_FAILURE, "planner_assert()")); + } + for (uint8_t i=0; i < _mp->q.queue_size; i++) { + if ((_mp->q.bf[i].nx == nullptr) || (_mp->q.bf[i].pv == nullptr)) { + return (cm_panic(STAT_PLANNER_ASSERTION_FAILURE, "planner buffer is corrupted")); + } } -// for (uint8_t i=0; i < PLANNER_BUFFER_POOL_SIZE; i++) { -// if (mb.bf[i].nx == nullptr) { -// _debug_trap("buffer has nullptr for nx"); -// } -// if (mb.bf[i].pv == nullptr) { -// _debug_trap("buffer has nullptr for pv"); -// } -// } return (STAT_OK); } -/* +/**************************************************************************************** * mp_halt_runtime() - stop runtime movement immediately */ void mp_halt_runtime() { stepper_reset(); // stop the steppers and dwells - planner_reset(); // reset the planner queues + planner_reset(mp); // reset the active planner } -/* - * mp_flush_planner() - flush all moves in the planner and all arcs - * - * Does not affect the move currently running in mr. - * Does not affect mm or gm model positions - * This function is designed to be called during a hold to reset the planner - * This function should not generally be called; call cm_queue_flush() instead - */ -void mp_flush_planner() -{ - cm_abort_arc(); - mp_init_buffers(); - mr.block_state = BLOCK_INACTIVE; // invalidate mr buffer to prevent subsequent motion -} - -/* +/**************************************************************************************** * mp_set_planner_position() - set planner position for a single axis * mp_set_runtime_position() - set runtime position for a single axis * mp_set_steps_to_runtime_position() - set encoder counts to the runtime position * * Since steps are in motor space you have to run the position vector through inverse - * kinematics to get the right numbers. This means that in a non-Cartesian robot changing - * any position can result in changes to multiple step values. So this operation is provided - * as a single function and always uses the new position vector as an input. + * kinematics to get the right numbers. This means that in a non-Cartesian robot + * changing any position can result in changes to multiple step values. So this operation + * is provided as a single function and always uses the new position vector as an input. * - * Keeping track of position is complicated by the fact that moves exist in several reference - * frames. The scheme to keep this straight is: + * Keeping track of position is complicated by the fact that moves exist in several + * reference frames. The scheme to keep this straight is: * - * - mm.position - start and end position for planning - * - mr.position - current position of runtime segment - * - mr.target - target position of runtime segment + * - mp->position - start and end position for planning + * - mr->position - current position of runtime segment + * - mr->target - target position of runtime segment * * The runtime keeps a lot more data, such as waypoints, step vectors, etc. * See struct mpMoveRuntimeSingleton for details. * - * Note that position is set immediately when called and may not be not an accurate representation - * of the tool position. The motors are still processing the action and the real tool position is - * still close to the starting point. + * Note that position is set immediately when called and may not be not an accurate + * representation of the tool position. The motors are still processing the action + * and the real tool position is still close to the starting point. */ -void mp_set_planner_position(uint8_t axis, const float position) { mp.position[axis] = position; } -void mp_set_runtime_position(uint8_t axis, const float position) { mr.position[axis] = position; } +void mp_set_planner_position(uint8_t axis, const float position) { mp->position[axis] = position; } +void mp_set_runtime_position(uint8_t axis, const float position) { mr->position[axis] = position; } void mp_set_steps_to_runtime_position() { float step_position[MOTORS]; - //kn_inverse_kinematics(mr.position, step_position); // convert lengths to steps in floating point + // There use to be a call to kn_inverse_kinematics here, but we don't do that now, instead we call kn->sync_encoders() + // after handling the steps, allowing the kinematics to intelligently handle the offset of step and position. for (uint8_t motor = MOTOR_1; motor < MOTORS; motor++) { - mr.target_steps[motor] = step_position[motor]; - mr.position_steps[motor] = step_position[motor]; - mr.commanded_steps[motor] = step_position[motor]; + mr->target_steps[motor] = step_position[motor]; + mr->position_steps[motor] = step_position[motor]; + mr->commanded_steps[motor] = step_position[motor]; en_set_encoder_steps(motor, step_position[motor]); // write steps to encoder register - mr.encoder_steps[motor] = en_read_encoder(motor); + mr->encoder_steps[motor] = en_read_encoder(motor); // These must be zero: - mr.following_error[motor] = 0; + mr->following_error[motor] = 0; st_pre.mot[motor].corrected_steps = 0; } kn->sync_encoders(); @@ -260,7 +288,7 @@ void mp_set_steps_to_runtime_position() * This function used to be a portion of _exec_aline_segment(). * It handles the step-count bucket brigade, and the call to st_prep_line. * Note that one of the goals is that stepper code and code that wants to set - * the stpeer count (i.e. kinematics idle) doesn't know or care about about mr. + * the stpeer count (i.e. kinematics idle) doesn't know or care about about mr-> */ // global, for use locally in various functions @@ -274,20 +302,20 @@ stat_t mp_set_target_steps(const float target_steps[MOTORS]) // Other kinematics may require transforming travel distance as opposed to simply subtracting steps. for (uint8_t m=0; mcommanded_steps[m] = mr->position_steps[m]; // previous segment's position, delayed by 1 segment + mr->position_steps[m] = mr->target_steps[m]; // previous segment's target becomes position + mr->target_steps[m] = target_steps[m]; // set the new target + mp_travel_steps[m] = mr->target_steps[m] - mr->position_steps[m]; + mr->encoder_steps[m] = en_read_encoder(m); // get current encoder position (time aligns to commanded_steps) + mr->following_error[m] = mr->encoder_steps[m] - mr->commanded_steps[m]; } - return st_prep_line(mr.segment_velocity, mr.target_velocity, mp_travel_steps, mr.following_error, mr.segment_time); + return st_prep_line(mr->segment_velocity, mr->target_velocity, mp_travel_steps, mr->following_error, mr->segment_time); } stat_t mp_set_target_steps(const float target_steps[MOTORS], const float start_velocities[MOTORS], const float end_velocities[MOTORS], const float segment_time) { - mr.segment_time = segment_time; + mr->segment_time = segment_time; // Bucket-brigade the old target down the chain before getting the new target from kinematics // @@ -295,19 +323,19 @@ stat_t mp_set_target_steps(const float target_steps[MOTORS], const float start_v // Other kinematics may require transforming travel distance as opposed to simply subtracting steps. for (uint8_t m=0; mcommanded_steps[m] = mr->position_steps[m]; // previous segment's position, delayed by 1 segment + mr->position_steps[m] = mr->target_steps[m]; // previous segment's target becomes position + mr->target_steps[m] = target_steps[m]; // set the new target + mp_travel_steps[m] = mr->target_steps[m] - mr->position_steps[m]; + mr->encoder_steps[m] = en_read_encoder(m); // get current encoder position (time aligns to commanded_steps) + mr->following_error[m] = mr->encoder_steps[m] - mr->commanded_steps[m]; } - return st_prep_line(start_velocities, end_velocities, mp_travel_steps, mr.following_error, mr.segment_time); + return st_prep_line(start_velocities, end_velocities, mp_travel_steps, mr->following_error, mr->segment_time); } -/************************************************************************************ +/**************************************************************************************** * mp_queue_command() - queue a synchronous Mcode, program control, or other command * _exec_command() - callback to execute command * @@ -328,7 +356,7 @@ stat_t mp_set_target_steps(const float target_steps[MOTORS], const float start_v * and makes keeping the queue full much easier - therefore avoiding Q starvation */ -void mp_queue_command(void(*cm_exec)(float[], bool[]), float *value, bool *flag) +void mp_queue_command(void(*cm_exec)(float *, bool *), float *value, bool *flag) { mpBuf_t *bf; @@ -342,7 +370,7 @@ void mp_queue_command(void(*cm_exec)(float[], bool[]), float *value, bool *flag) bf->cm_func = cm_exec; // callback to canonical machine exec function for (uint8_t axis = AXIS_X; axis < AXES; axis++) { - bf->value_vector[axis] = value[axis]; + bf->unit[axis] = value[axis]; // use the unit vector to store command values bf->axis_flags[axis] = flag[axis]; } mp_commit_write_buffer(BLOCK_TYPE_COMMAND); // must be final operation before exit @@ -356,53 +384,74 @@ static stat_t _exec_command(mpBuf_t *bf) stat_t mp_runtime_command(mpBuf_t *bf) { - bf->cm_func(bf->value_vector, bf->axis_flags); // 2 vectors used by callbacks + bf->cm_func(bf->unit, bf->axis_flags); // 2 vectors used by callbacks if (mp_free_run_buffer()) { cm_cycle_end(); // free buffer & perform cycle_end if planner is empty } return (STAT_OK); } - -/************************************************************************* - * mp_json_command() - queue a json command +/**************************************************************************************** * _exec_json_command() - execute json string (from exec system) + * mp_json_command() - queue a json command + * mp_json_command_immediate() - execute a json command with response suppressed */ +static void _exec_json_command(float *value, bool *flag) +{ + char *json_string = jc.read_buffer(); + json_parse_for_exec(json_string, true); // process it + jc.free_buffer(); +} + stat_t mp_json_command(char *json_string) { // Never supposed to fail, since we stopped parsing when we were full jc.write_buffer(json_string); - - // We don't actually use these... - float value[] = { 0,0,0,0,0,0 }; - bool flags[] = { 0,0,0,0,0,0 }; - - mp_queue_command(_exec_json_command, value, flags); + mp_queue_command(_exec_json_command, nullptr, nullptr); return (STAT_OK); } -static void _exec_json_command(float *value, bool *flag) -{ - char *json_string = jc.read_buffer(); - json_parse_for_exec(json_string, true); // process it - jc.free_buffer(); -} - -/************************************************************************* - * mp_json_command_immediate() - execute a json command with response suppressed - */ - stat_t mp_json_command_immediate(char *json_string) { return json_parser(json_string); } -/************************************************************************* - * mp_json_wait() - queue a json wait command +/**************************************************************************************** * _exec_json_wait() - execute json wait string + * mp_json_wait() - queue a json wait command */ +static stat_t _exec_json_wait(mpBuf_t *bf) +{ + char *json_string = jc.read_buffer(); + + // process it + json_parse_for_exec(json_string, false); // do NOT execute + + nvObj_t *nv = nv_exec; + while ((nv != NULL) && (nv->valuetype != TYPE_EMPTY)) { + // For now we ignore non-BOOL + if (nv->valuetype == TYPE_BOOLEAN) { + bool old_value = (bool)nv->value_int; // force it to bool + + nv_get_nvObj(nv); + bool new_value = (bool)nv->value_int; + if (old_value != new_value) { + st_prep_dwell(1.0); // 1ms exactly + return STAT_OK; + } + } + nv = nv->nx; + } + jc.free_buffer(); + + if (mp_free_run_buffer()) { + cm_cycle_end(); // free buffer & perform cycle_end if planner is empty + } + return (STAT_OK); +} + stat_t mp_json_wait(char *json_string) { // Never supposed to fail, since we stopped parsing when we were full @@ -421,38 +470,8 @@ stat_t mp_json_wait(char *json_string) return (STAT_OK); } -static stat_t _exec_json_wait(mpBuf_t *bf) -{ - char *json_string = jc.read_buffer(); - // process it - json_parse_for_exec(json_string, false); // do NOT execute - - nvObj_t *nv = nv_exec; - while ((nv != NULL) && (nv->valuetype != TYPE_EMPTY)) { - // For now we ignore non-BOOL - if (nv->valuetype == TYPE_BOOL) { - bool old_value = !fp_ZERO(nv->value); // force it to bool - - nv_get_nvObj(nv); - bool new_value = !fp_ZERO(nv->value); - if (old_value != new_value) { - st_prep_dwell(1.0); // 1ms exactly - return STAT_OK; - } - } - nv = nv->nx; - } - jc.free_buffer(); - - if (mp_free_run_buffer()) { - cm_cycle_end(); // free buffer & perform cycle_end if planner is empty - } - return (STAT_OK); -} - - -/************************************************************************* +/**************************************************************************************** * mp_dwell() - queue a dwell * _exec_dwell() - dwell execution * @@ -460,6 +479,7 @@ static stat_t _exec_json_wait(mpBuf_t *bf) * When the stepper driver sees a dwell it times the dwell on a separate * timer than the stepper pulse timer. */ + stat_t mp_dwell(float seconds) { mpBuf_t *bf; @@ -478,24 +498,30 @@ static stat_t _exec_dwell(mpBuf_t *bf) { st_prep_dwell(bf->block_time * 1000.0);// convert seconds to ms if (mp_free_run_buffer()) { - cm_cycle_end(); // free buffer & perform cycle_end if planner is empty + cm_cycle_end(); // free buffer & perform cycle_end if planner is empty } return (STAT_OK); } -//++++ stubbed ++++ +/**************************************************************************************** + * mp_request_out_of_band_dwell() - request a dwell outside of the planner queue + * + * This command is used to request that a dwell be run outside of the planner. + * The dwell will only be queued if the time is non-zero, and will only be executed + * if the runtime has been stopped. This function is typically called from an exec + * such as _exec_spindle_control(). The dwell move is executed from mp_exec_move(). + * This is useful for queuing a dwell after a spindle change. + */ + void mp_request_out_of_band_dwell(float seconds) { -// mr.out_of_band_dwell_time = seconds; -} -//++++ stubbed ++++ -stat_t mp_exec_out_of_band_dwell(void) -{ -// return _advance_dwell(mr.out_of_band_dwell_time); - return 0; + if (fp_NOT_ZERO(seconds)) { + mr->out_of_band_dwell_flag = true; + mr->out_of_band_dwell_seconds = seconds; + } } -/********************************************************************************** +/**************************************************************************************** * Planner helpers * * mp_get_planner_buffers() - return # of available planner buffers @@ -503,31 +529,32 @@ stat_t mp_exec_out_of_band_dwell(void) * mp_has_runnable_buffer() - true if next buffer is runnable, indicating motion has not stopped. * mp_is_it_phat_city_time() - test if there is time for non-essential processes */ -uint8_t mp_get_planner_buffers() + +uint8_t mp_get_planner_buffers(const mpPlanner_t *_mp) // which planner are you interested in? { - return (mb.buffers_available); + return (_mp->q.buffers_available); } -bool mp_planner_is_full() +bool mp_planner_is_full(const mpPlanner_t *_mp) // which planner are you interested in? { // We also need to ensure we have room for another JSON command - return ((mb.buffers_available < PLANNER_BUFFER_HEADROOM) || (jc.available == 0)); + return ((_mp->q.buffers_available < PLANNER_BUFFER_HEADROOM) || (jc.available == 0)); } -bool mp_has_runnable_buffer() +bool mp_has_runnable_buffer(const mpPlanner_t *_mp) // which planner are you interested in?) { - return (mb.r->buffer_state); // anything other than MP_BUFFER_EMPTY returns true + return (_mp->q.r->buffer_state); // anything other than MP_BUFFER_EMPTY returns true } bool mp_is_phat_city_time() { - if(cm.hold_state == FEEDHOLD_HOLD) { + if(cm->hold_state == FEEDHOLD_HOLD) { return true; } - return ((mp.plannable_time <= 0.0) || (PHAT_CITY_TIME < mp.plannable_time)); + return ((mp->plannable_time <= 0.0) || (PHAT_CITY_TIME < mp->plannable_time)); } -/* +/**************************************************************************************** * mp_planner_callback() * * mp_planner_callback()'s job is to invoke backward planning intelligently. @@ -545,7 +572,8 @@ bool mp_is_phat_city_time() * * - _plan_block() is the backward planning function for a single buffer. * - * - Just-in-time forward planning is performed by mp_plan_move() in the plan_exec.cpp runtime executive + * - Just-in-time forward planning is performed by mp_plan_move() in the + * plan_exec.cpp runtime executive * * Some Items to note: * @@ -564,36 +592,31 @@ bool mp_is_phat_city_time() stat_t mp_planner_callback() { // Test if the planner has transitioned to an IDLE state - if ((mb.buffers_available == PLANNER_BUFFER_POOL_SIZE) && // detect and set IDLE state - (cm.motion_state == MOTION_STOP) && - (cm.hold_state == FEEDHOLD_OFF)) { - mp.planner_state = PLANNER_IDLE; - - // request an empty exec - in case the kinemtics have an idle exec cycle - st_request_exec_move(); - + if ((mp_get_planner_buffers(mp) == mp->q.queue_size) && // detect and set IDLE state + (cm->motion_state == MOTION_STOP) && (cm->hold_state == FEEDHOLD_OFF)) { + mp->planner_state = PLANNER_IDLE; return (STAT_OK); } - bool _timed_out = mp.block_timeout.isPast(); + bool _timed_out = mp->block_timeout.isPast(); if (_timed_out) { - mp.block_timeout.clear(); // timer is set on commit_write_buffer() + mp->block_timeout.clear(); // timer is set on commit_write_buffer() } - if (!mp.request_planning && !_timed_out) { // Exit if no request or timeout + if (!mp->request_planning && !_timed_out) { // Exit if no request or timeout return (STAT_OK); } // Process a planner request or timeout - if (mp.planner_state == PLANNER_IDLE) { - mp.p = mb.r; // initialize planner pointer to run buffer - mp.planner_state = PLANNER_STARTUP; + if (mp->planner_state == PLANNER_IDLE) { + mp->p = mp_get_r(); // initialize planner pointer to run buffer + mp->planner_state = PLANNER_STARTUP; } - if (mp.planner_state == PLANNER_STARTUP) { - if (!mp_planner_is_full() && !_timed_out) { + if (mp->planner_state == PLANNER_STARTUP) { + if (!mp_planner_is_full(mp) && !_timed_out) { return (STAT_OK); // remain in STARTUP } - mp.planner_state = PLANNER_PRIMING; + mp->planner_state = PLANNER_PRIMING; } mp_plan_block_list(); return (STAT_OK); @@ -605,17 +628,18 @@ stat_t mp_planner_callback() * We don't actually need to invalidate back-planning. Only forward planning. * */ + void mp_replan_queue(mpBuf_t *bf) { do { - if (bf->buffer_state >= MP_BUFFER_PLANNED) { - bf->buffer_state = MP_BUFFER_PREPPED; // revert from PLANNED state - } else { // If it's not "planned" then it's either PREPPED or earlier. - break; // We don't need to adjust it. + if (bf->buffer_state >= MP_BUFFER_FULLY_PLANNED) { // revert from FULLY PLANNED state + bf->buffer_state = MP_BUFFER_BACK_PLANNED; + } else { // If it's not "planned" then it's either backplanned or earlier. + break; // We don't need to adjust it. } - } while ((bf = mp_get_next_buffer(bf)) != mb.r); + } while ((bf = mp_get_next_buffer(bf)) != mp_get_r()); - mp.request_planning = true; + mp->request_planning = true; } /* @@ -639,23 +663,23 @@ void mp_replan_queue(mpBuf_t *bf) void mp_start_feed_override(const float ramp_time, const float override_factor) { - cm.mfo_state = MFO_REQUESTED; + cm->mfo_state = MFO_REQUESTED; - if (mp.planner_state == PLANNER_IDLE) { - mp.mfo_factor = override_factor; // that was easy + if (mp->planner_state == PLANNER_IDLE) { + mp->mfo_factor = override_factor; // that was easy return; } // Assume that the min and max values for override_factor have been validated upstream // SUVAT: V = U+AT ==> A = (V-U)/T - mp.ramp_target = override_factor; - mp.ramp_dvdt = (override_factor - mp.c->override_factor) / ramp_time; - mp.mfo_active = true; + mp->ramp_target = override_factor; + mp->ramp_dvdt = (override_factor - mp->c->override_factor) / ramp_time; + mp->mfo_active = true; - if (fp_NOT_ZERO(mp.ramp_dvdt)) { // do these things only if you actually have a ramp to run - mp.p = mp.c; // re-position the planner pointer - mp.ramp_active = true; - mp.request_planning = true; + if (fp_NOT_ZERO(mp->ramp_dvdt)) { // do these things only if you actually have a ramp to run + mp->p = mp->c; // re-position the planner pointer + mp->ramp_active = true; + mp->request_planning = true; } } @@ -680,20 +704,20 @@ void mp_end_traverse_override(const float ramp_time) void mp_planner_time_accounting() { - mpBuf_t *bf = mb.r; // start with run buffer + mpBuf_t *bf = mp_get_r(); // start with run buffer // check the run buffer to see if anything is running. Might not be if (bf->buffer_state != MP_BUFFER_RUNNING) { // this is not an error condition return; } - mp.plannable_time = 0; //UPDATE_BF_MS(bf); //+++++ - while ((bf = bf->nx) != mb.r) { + mp->plannable_time = 0; //UPDATE_BF_MS(bf); // DIAGNOSTIC + while ((bf = bf->nx) != mp_get_r()) { if (bf->buffer_state == MP_BUFFER_EMPTY || bf->plannable == true) { break; } - mp.plannable_time += bf->block_time; + mp->plannable_time += bf->block_time; } - UPDATE_MP_DIAGNOSTICS //+++++ + UPDATE_MP_DIAGNOSTICS // DIAGNOSTIC } /**** PLANNER BUFFER PRIMITIVES ************************************************************ @@ -741,8 +765,6 @@ void mp_planner_time_accounting() * Functions Provided: * _clear_buffer(bf) Zero the contents of a buffer * - * mp_init_buffers() Initialize or reset buffers - * * mp_get_prev_buffer(bf) Return pointer to the previous buffer in the linked list * mp_get_next_buffer(bf) Return pointer to the next buffer in the linked list * @@ -775,67 +797,28 @@ void mp_planner_time_accounting() // Also clears unlocked, so the buffer cannot be used static inline void _clear_buffer(mpBuf_t *bf) { - // Note: bf->bf_func is the first address we wish to clear as we must preserve - // the pointers and buffer number during interrupts - - // We'll have to figure something else out for C, sorry. - bf->reset(); -} - -void mp_init_buffers(void) -{ - mpBuf_t *pv, *nx; - - //memset(&mb, 0, sizeof(mb)); // clear all values, pointers and status - mb.magic_start = MAGICNUM; - mb.magic_end = MAGICNUM; - - mb.w = &mb.bf[0]; // init all buffer pointers - mb.r = &mb.bf[0]; - pv = &mb.bf[PLANNER_BUFFER_POOL_SIZE-1]; - for (uint8_t i=0; i < PLANNER_BUFFER_POOL_SIZE; i++) { - _clear_buffer(&mb.bf[i]); - uint8_t nx_i = ((i<(PLANNER_BUFFER_POOL_SIZE-1))?(i+1):0); // buffer incr & wrap - - mb.bf[i].buffer_number = i; //+++++ number it for diagnostics only (otherwise not used) - - nx = &mb.bf[nx_i]; - mb.bf[i].nx = nx; // setup ring pointers - mb.bf[i].pv = pv; - - pv = &mb.bf[i]; - } - mb.buffers_available = PLANNER_BUFFER_POOL_SIZE; - -// mb.entry_changed = false; - - // Now handle the two "stub buffers" in the runtime structure. - mr.bf[0].nx = &mr.bf[1]; - mr.bf[1].nx = &mr.bf[0]; - mr.r = &mr.bf[0]; - mr.p = &mr.bf[1]; + bf->reset(); // Call a reset method on the buffer object. } /* * These GET functions are defined here but we use the macros in planner.h instead - * -mpBuf_t * mp_get_prev_buffer(const mpBuf_t *bf) -{ - return (bf->pv); -} -mpBuf_t * mp_get_next_buffer(const mpBuf_t *bf) -{ - return (bf->nx); -} -*/ +mpBuf_t * mp_get_prev_buffer(const mpBuf_t *bf) { return (bf->pv); } +mpBuf_t * mp_get_next_buffer(const mpBuf_t *bf) { return (bf->nx); } + */ + +mpBuf_t * mp_get_w() { return (mp->q.w); } +mpBuf_t * mp_get_r() { return (mp->q.r); } mpBuf_t * mp_get_write_buffer() // get & clear a buffer { - if (mb.w->buffer_state == MP_BUFFER_EMPTY) { - _clear_buffer(mb.w); // ++++RG this is redundant, it was just cleared in mp_free_run_buffer - mb.w->buffer_state = MP_BUFFER_INITIALIZING; - mb.buffers_available--; - return (mb.w); + + mpPlannerQueue_t *q = &(mp->q); + + if (q->w->buffer_state == MP_BUFFER_EMPTY) { + _clear_buffer(q->w); // NB: this is redundant if the buffer was cleared mp_free_run_buffer() + q->w->buffer_state = MP_BUFFER_INITIALIZING; + q->buffers_available--; + return (mp_get_w()); } // The no buffer condition always causes a panic - invoked by the caller rpt_exception(STAT_FAILED_TO_GET_PLANNER_BUFFER, "mp_get_write_buffer()"); @@ -844,9 +827,11 @@ mpBuf_t * mp_get_write_buffer() // get & clear a buffer void mp_unget_write_buffer() // mark buffer as empty and adjust free buffer count { - if (mb.w->buffer_state != MP_BUFFER_EMPTY) { // safety. Can't unget an empty buffer - mb.w->buffer_state = MP_BUFFER_EMPTY; - mb.buffers_available++; + mpPlannerQueue_t *q = &(mp->q); + + if (q->w->buffer_state != MP_BUFFER_EMPTY) { // safety. Can't unget an empty buffer + q->w->buffer_state = MP_BUFFER_EMPTY; + q->buffers_available++; } } @@ -857,69 +842,78 @@ void mp_unget_write_buffer() // mark buffer as empty and adjust free buff void mp_commit_write_buffer(const blockType block_type) { - mb.w->block_type = block_type; - mb.w->block_state = BLOCK_INITIAL_ACTION; + mpPlannerQueue_t *q = &(mp->q); - if (block_type == BLOCK_TYPE_ALINE) { - if (cm.motion_state == MOTION_STOP) { - cm_set_motion_state(MOTION_PLANNING); - } - } else { - if ((mp.planner_state > PLANNER_STARTUP) && (cm.hold_state == FEEDHOLD_OFF)) { - // NB: BEWARE! the exec may result in the planner buffer being + q->w->block_type = block_type; + q->w->block_state = BLOCK_INITIAL_ACTION; + + if (block_type != BLOCK_TYPE_ALINE) { + if ((mp->planner_state > PLANNER_STARTUP) && (cm->hold_state == FEEDHOLD_OFF)) { + // NB: BEWARE! the requested exec may result in the planner buffer being // processed IMMEDIATELY and then freed - invalidating the contents - st_request_forward_plan(); // request an exec if the runtime is not busy + st_request_forward_plan(); // request an exec if the runtime is not busy } } - mb.w->plannable = true; // enable block for planning - mp.request_planning = true; - mb.w = mb.w->nx; // advance write buffer pointer - mp.block_timeout.set(BLOCK_TIMEOUT_MS); // reset the block timer - qr_request_queue_report(+1); // request QR and add to "added buffers" count + q->w->plannable = true; // enable block for planning + mp->request_planning = true; + q->w = q->w->nx; // advance write buffer pointer + mp->block_timeout.set(BLOCK_TIMEOUT_MS);// reset the block timer + qr_request_queue_report(+1); // request QR and add to "added buffers" count } // Note: mp_get_run_buffer() is only called by mp_exec_move(), which is inside an interrupt +// EMPTY and INITALIZING are the two cases where nothing is returned. This is not an error +// Otherwise return the buffer. Let mp_exec_move() manage the state machine to sort out: +// (1) is the the first time the run buffer has been retrieved? +// (2) is the buffer in error - i.e. not yet ready for running? mpBuf_t * mp_get_run_buffer() { - // EMPTY is the one case where nothing is returned. This is not an error - if (mb.r->buffer_state == MP_BUFFER_EMPTY) { + mpBuf_t *r = mp->q.r; + + if (r->buffer_state == MP_BUFFER_EMPTY || r->buffer_state == MP_BUFFER_INITIALIZING) { return (NULL); } - // Otherwise return the buffer. Let mp_exec_move() manage the state machine to sort out: - // (1) is the the first time the run buffer has been retrieved? - // (2) is the buffer in error - i.e. not yet ready for running? - return (mb.r); + return (r); } // Note: mp_free_run_buffer() is only called from mp_exec_XXX, which are within an interrupt +// Clearing and advancing must be done atomically as other interrupts may be using the run buffer bool mp_free_run_buffer() // EMPTY current run buffer & advance to the next { - _audit_buffers(); // diagnostic audit for buffer chain integrity (only runs in DEBUG mode) + mpPlannerQueue_t *q = &(mp->q); + mpBuf_t *r_now = q->r; // save this pointer is to avoid a race condition when clearing the buffer - mpBuf_t *r = mb.r; - mb.r = mb.r->nx; // advance to next run buffer - _clear_buffer(r); // clear it out (& reset unlocked and set MP_BUFFER_EMPTY) - - mb.buffers_available++; + _audit_buffers(); // DIAGNOSTIC audit for buffer chain integrity (only runs in DEBUG mode) + q->r = q->r->nx; // advance to next run buffer first... + _clear_buffer(r_now); // ... then clear out the old buffer (& set MP_BUFFER_EMPTY) +// r_now->buffer_state = MP_BUFFER_EMPTY; //... then mark the buffer empty while preserving content for debug inspection + q->buffers_available++; qr_request_queue_report(-1); // request a QR and add to the "removed buffers" count - return (mb.w == mb.r); // return true if the queue emptied + return (q->w == q->r); // return true if the queue emptied } /* UNUSED FUNCTIONS - left in for completeness and for reference void mp_copy_buffer(mpBuf_t *bf, const mpBuf_t *bp) { - // copy contents of bp to by while preserving pointers in bp + // copy contents of bp to bf while preserving pointers in bp memcpy((void *)(&bf->bf_func), (&bp->bf_func), sizeof(mpBuf_t) - (sizeof(void *) * 2)); } */ + +/************************************************************************************ + *** DIAGNOSTICS ******************************************************************** + ************************************************************************************/ + /************************************************************************************ * mp_dump_planner * _planner_report() - * _audit_buffers() - a DEBUG diagnostic + * _audit_buffers() */ //#define __DUMP_PLANNER +//#define __PLANNER_REPORT_ENABLED +//#define __AUDIT_BUFFERS #ifdef __DUMP_PLANNER void mp_dump_planner(mpBuf_t *bf_start) // starting at bf @@ -955,49 +949,57 @@ void mp_dump_planner(mpBuf_t *bf_start) // starting at bf } #endif // __DUMP_PLANNER -#if 0 && defined(DEBUG) - -#warning DEBUG TRAPS ENABLED - -#pragma GCC optimize ("O0") - -//static void _planner_report(const char *msg) -//{ -// rpt_exception(STAT_PLANNER_ASSERTION_FAILURE, msg); -// -// for (uint8_t i=0; ibuffer_state != MP_BUFFER_RUNNING) { -// _planner_report("buffer audit1"); + _planner_report("buffer audit1"); __NOP(); -// _debug_trap(); + debug_trap("buffer audit1"); } // Check that the next from the previous is correct. if (mb.r->pv->nx != mb.r || mb.r->nx->pv != mb.r){ -// _planner_report("buffer audit2"); - _debug_trap("buffer audit2"); + _planner_report("buffer audit2"); + debug_trap("buffer audit2"); } // Now check every buffer, in order we would execute them. @@ -1005,8 +1007,8 @@ static void _audit_buffers() while (bf != mb.r) { // Check that the next from the previous is correct. if (bf->pv->nx != bf || bf->nx->pv != bf){ -// _planner_report("buffer audit3"); - _debug_trap("buffer audit3"); + _planner_report("buffer audit3"); + debug_trap("buffer audit3"); } // Order should be: @@ -1027,8 +1029,8 @@ static void _audit_buffers() if ((bf->buffer_state == MP_BUFFER_INITIALIZING) || (bf->buffer_state == MP_BUFFER_IN_PROCESS)) { __NOP(); } else { -// _planner_report("buffer audit4"); - _debug_trap("buffer audit4"); + _planner_report("buffer audit4"); + debug_trap("buffer audit4"); } } @@ -1038,8 +1040,8 @@ static void _audit_buffers() bf->buffer_state != MP_BUFFER_INITIALIZING && bf->buffer_state != MP_BUFFER_IN_PROCESS && bf->buffer_state != MP_BUFFER_EMPTY) { -// _planner_report("buffer audit5"); - _debug_trap("buffer audit5"); + _planner_report("buffer audit5"); + debug_trap("buffer audit5"); } // After PREPPED, we can see PREPPED, INITED, IN_PROCESS, or EMPTY @@ -1048,14 +1050,14 @@ static void _audit_buffers() bf->buffer_state != MP_BUFFER_INITIALIZING && bf->buffer_state != MP_BUFFER_IN_PROCESS && bf->buffer_state != MP_BUFFER_EMPTY) { -// _planner_report("buffer audit6"); - _debug_trap("buffer audit6"); + _planner_report("buffer audit6"); + debug_trap("buffer audit6"); } // After EMPTY, we should only see EMPTY if (bf->pv->buffer_state == MP_BUFFER_EMPTY && bf->buffer_state != MP_BUFFER_EMPTY) { -// _planner_report("buffer audit7"); - _debug_trap("buffer audit7"); + _planner_report("buffer audit7"); + debug_trap("buffer audit7"); } // Now look at the next one. bf = bf->nx; @@ -1063,17 +1065,7 @@ static void _audit_buffers() __enable_irq(); } -#pragma GCC reset_options - -#else - -static void _audit_buffers() -{ - // empty stub -} - -#endif // 0 - +#endif // __AUDIT_BUFFERS /**************************** * END OF PLANNER FUNCTIONS * diff --git a/g2core/planner.h b/g2core/planner.h index 6238ae46..8babfedf 100644 --- a/g2core/planner.h +++ b/g2core/planner.h @@ -2,8 +2,8 @@ * planner.h - cartesian trajectory planning and motion execution * This file is part of the g2core project * - * Copyright (c) 2013 - 2016 Alden S. Hart, Jr. - * Copyright (c) 2013 - 2016 Robert Giseburt + * Copyright (c) 2013 - 2019 Alden S. Hart, Jr. + * Copyright (c) 2013 - 2019 Robert Giseburt * * This file ("the software") is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License, version 2 as published by the @@ -156,8 +156,7 @@ using Motate::Timeout; /* * Enums and other type definitions - * - * All the enums that equal zero must be zero. Don't change them + * All the enums that equal some starting value must be that value. Don't change them */ typedef void (*cm_exec_t)(float[], bool[]); // callback to canonical_machine execution function @@ -166,24 +165,23 @@ typedef enum { // planner operating state PLANNER_IDLE = 0, // planner and movement are idle PLANNER_STARTUP, // ingesting blocks before movement is started PLANNER_PRIMING, // preparing new moves for planning ("stitching") - PLANNER_BACK_PLANNING // plan by planning all blocks, from the newest added to the running block + PLANNER_BACK_PLANNING // actively backplanning all blocks, from the newest added to the running block } plannerState; typedef enum { // bf->buffer_state values in incresing order so > and < can be used MP_BUFFER_EMPTY = 0, // buffer is available for use (MUST BE 0) MP_BUFFER_INITIALIZING, // buffer has been checked out and is being initialzed by aline() or a command - MP_BUFFER_IN_PROCESS, // planning is in progress - at least vmaxes have been set - MP_BUFFER_PREPPED, // buffer ready for final planning; velocities have been set - MP_BUFFER_PLANNED, // buffer fully planned. May still be replanned + MP_BUFFER_NOT_PLANNED, // planning is in progress - at least vmaxes have been set + MP_BUFFER_BACK_PLANNED, // buffer ready for final planning; velocities have been set + MP_BUFFER_FULLY_PLANNED, // buffer fully planned. May still be replanned MP_BUFFER_RUNNING, // current running buffer - MP_BUFFER_POLAND, // Hitler used Poland as a buffer state - MP_BUFFER_UKRAINE // Later Stalin did the same to Ukraine } bufferState; typedef enum { // bf->block_type values BLOCK_TYPE_NULL = 0, // MUST=0 null move - does a no-op BLOCK_TYPE_ALINE = 1, // MUST=1 acceleration planned line BLOCK_TYPE_COMMAND = 2, // MUST=2 general command + // All other non-move commands are > BLOCK_TYPE_COMMAND BLOCK_TYPE_DWELL, // Gcode dwell BLOCK_TYPE_JSON_WAIT, // JSON wait command BLOCK_TYPE_TOOL, // T command (T, not M6 tool change) @@ -227,33 +225,22 @@ typedef enum { // code blocks for planning and trapezoid ge ASYMMETRIC_BUMP, // (Ve != Vx) < Vc } blockHint; -typedef enum { - ZOID_EXIT_NULL = 0, - ZOID_EXIT_1a, - ZOID_EXIT_1c, - ZOID_EXIT_1d, - ZOID_EXIT_2a, - ZOID_EXIT_2c, - ZOID_EXIT_2d, - ZOID_EXIT_3c, - ZOID_EXIT_3s, - ZOID_EXIT_3s2, - ZOID_EXIT_3d2, - ZOID_EXIT_3a2 -} zoidExitPoint; - /*** Most of these factors are the result of a lot of tweaking. Change with caution.***/ -#ifndef PLANNER_BUFFER_POOL_SIZE -#define PLANNER_BUFFER_POOL_SIZE (48) // Suggest 12 min. Limit is 255 +#ifdef PLANNER_BUFFER_POOL_SIZE +#error Please change PLANNER_BUFFER_POOL_SIZE define to PLANNER_QUEUE_SIZE (found elsewhere, unfortunately) #endif -#define PLANNER_BUFFER_HEADROOM (4) // Buffers to reserve in planner before processing new input line +#ifndef PLANNER_QUEUE_SIZE +#define PLANNER_QUEUE_SIZE ((uint8_t)48) // Suggest 12 min. Limit is 255 +#endif +#define SECONDARY_QUEUE_SIZE ((uint8_t)12) // Secondary planner queue for feedhold operations +#define PLANNER_BUFFER_HEADROOM ((uint8_t)4) // Buffers to reserve in planner before processing new input line #define JERK_MULTIPLIER ((float)1000000) // DO NOT CHANGE - must always be 1 million -#define JUNCTION_INTEGRATION_MIN (0.05) // minimum allowable setting -#define JUNCTION_INTEGRATION_MAX (5.00) // maximum allowable setting +#define JUNCTION_INTEGRATION_MIN (0.05) // JT minimum allowable setting +#define JUNCTION_INTEGRATION_MAX (5.00) // JT maximum allowable setting -#ifndef MIN_SEGMENT_MS +#ifndef MIN_SEGMENT_MS // boards can override this value in hardware.h #define MIN_SEGMENT_MS ((float)0.75) // minimum segment milliseconds #endif #define NOM_SEGMENT_MS ((float)MIN_SEGMENT_MS*2.0) // nominal segment ms (at LEAST MIN_SEGMENT_MS * 2) @@ -291,38 +278,67 @@ typedef enum { #define Veq2_lo 1.0 #define VELOCITY_ROUGHLY_EQ(v0,v1) ( (v0 > Vthr2) ? std::abs(v0-v1) < Veq2_hi : std::abs(v0-v1) < Veq2_lo ) -//#define ASCII_ART(s) xio_writeline(s) +/* Planner Diagnostics */ + +//#define __PLANNER_DIAGNOSTICS // comment this out to drop diagnostics + +#ifdef __PLANNER_DIAGNOSTICS +#define ASCII_ART(s) xio_writeline(s) + +#define UPDATE_BF_DIAGNOSTICS(bf) { bf->linenum = bf->gm.linenum; \ + bf->block_time_ms = bf->block_time*60000; \ + bf->plannable_time_ms = bf->plannable_time*60000; } + +#define UPDATE_MP_DIAGNOSTICS { mp->plannable_time_ms = mp->plannable_time*60000; } +#define SET_PLANNER_ITERATIONS(i) { bf->iterations = i; } +#define INC_PLANNER_ITERATIONS { bf->iterations++; } +#define SET_MEET_ITERATIONS(i) { bf->meet_iterations = i; } +#define INC_MEET_ITERATIONS { bf->meet_iterations++; } + +#else #define ASCII_ART(s) -//#define UPDATE_BF_DIAGNOSTICS(bf) { bf->block_time_ms = bf->block_time*60000; bf->plannable_time_ms = bf->plannable_time*60000; } -#define UPDATE_MP_DIAGNOSTICS { mp.plannable_time_ms = mp.plannable_time*60000; } +#define UPDATE_BF_DIAGNOSTICS +#define UPDATE_MP_DIAGNOSTICS +#define SET_PLANNER_ITERATIONS(i) +#define INC_PLANNER_ITERATIONS +#define SET_MEET_ITERATIONS(i) +#define INC_MEET_ITERATIONS +#endif /* * Planner structures * - * You should be aware of the distinction between 'buffers' and 'blocks' + * Be aware of the distinction between 'buffers' and 'blocks' * Please refer to header comments in for important details on buffers and blocks * - plan_zoid.cpp / mp_calculate_ramps() * - plan_exec.cpp / mp_exec_aline() */ -struct mpBuffer_to_clear { - // Note: _clear_buffer() zeros all data from this point down - stat_t (*bf_func)(struct mpBuffer *bf); // callback to buffer exec function - cm_exec_t cm_func; // callback to canonical machine execution function +//**** Planner Queue Structures **** - //+++++ DIAGNOSTICS for easier debugging - uint32_t linenum; // mirror of bf->gm.linenum +typedef struct mpBuffer { + + // *** CAUTION *** These two pointers are not reset by _clear_buffer() + struct mpBuffer *pv; // static pointer to previous buffer + struct mpBuffer *nx; // static pointer to next buffer + uint8_t buffer_number; // DIAGNOSTIC for easier debugging + + stat_t (*bf_func)(struct mpBuffer *bf); // callback to buffer exec function + cm_exec_t cm_func; // callback to canonical machine execution function + +#ifdef __PLANNER_DIAGNOSTICS + uint32_t linenum; // mirror of bf->gm.linenum int iterations; float block_time_ms; - float plannable_time_ms; // time in planner - float plannable_length; // length in planner - int8_t meet_iterations; // iterations needed in _get_meet_velocity - //+++++ to here + float plannable_time_ms; // time in planner + float plannable_length; // length in planner + uint8_t meet_iterations; // iterations needed in _get_meet_velocity +#endif - bufferState buffer_state; // used to manage queuing/dequeuing - blockType block_type; // used to dispatch to run routine - blockState block_state; // move state machine sequence - blockHint hint; // hint the block for zoid and other planning operations. Must be accurate or NO_HINT + bufferState buffer_state; // used to manage queuing/dequeuing + blockType block_type; // used to dispatch to run routine + blockState block_state; // move state machine sequence + blockHint hint; // hint the block for zoid and other planning operations. Must be accurate or NO_HINT // block parameters float unit[AXES]; // unit vector for axis scaling & planning @@ -337,44 +353,43 @@ struct mpBuffer_to_clear { float block_time; // computed move time for entire block (move) float override_factor; // feed rate or rapid override factor for this block ("override" is a reserved word) - // We are removing all entry_* values. - // To get the entry_* values, look at pv->exit_* or mr.exit_* - // *** SEE NOTES ON THESE VARIABLES, in aline() *** - float cruise_velocity; // cruise velocity requested & achieved - float exit_velocity; // exit velocity requested for the move - // is also the entry velocity of the *next* move + // We removed all entry_* values. + // To get the entry_* values, look at pv->exit_* or mr->exit_* + float cruise_velocity; // cruise velocity requested & achieved + float exit_velocity; // exit velocity requested for the move + // is also the entry velocity of the *next* move - float cruise_vset; // cruise velocity requested for move - prior to overrides - float cruise_vmax; // cruise max velocity adjusted for overrides - float exit_vmax; // max exit velocity possible for this move - // is also the maximum entry velocity of the next move + float cruise_vset; // cruise velocity requested for move - prior to overrides + float cruise_vmax; // cruise max velocity adjusted for overrides + float exit_vmax; // max exit velocity possible for this move + // is also the maximum entry velocity of the next move - float absolute_vmax; // fastest this block can move w/o exceeding constraints - float junction_vmax; // maximum the exit velocity can be to go through the junction - // between the NEXT BLOCK AND THIS ONE + float absolute_vmax; // fastest this block can move w/o exceeding constraints + float junction_vmax; // maximum the exit velocity can be to go through the junction + // between the NEXT BLOCK AND THIS ONE - float jerk; // maximum linear jerk term for this move - float jerk_sq; // Jm^2 is used for planning (computed and cached) - float recip_jerk; // 1/Jm used for planning (computed and cached) - float sqrt_j; // sqrt(jM) used for planning (computed and cached) - float q_recip_2_sqrt_j; // (q/(2 sqrt(jM))) where q = (sqrt(10)/(3^(1/4))), used in length computations (computed and cached) + float jerk; // maximum linear jerk term for this move + float jerk_sq; // Jm^2 is used for planning (computed and cached) + float recip_jerk; // 1/Jm used for planning (computed and cached) + float sqrt_j; // sqrt(jM) used for planning (computed and cached) + float q_recip_2_sqrt_j; // (q/(2 sqrt(jM))) where q = (sqrt(10)/(3^(1/4))), used in length computations (computed and cached) - GCodeState_t gm; // Gcode model state - passed from model, used by planner and runtime + GCodeState_t gm; // Gcode model state - passed from model, used by planner and runtime + // clears the above structure void reset() { - //memset((void *)(this), 0, sizeof(mpBuffer_to_clear)); - bf_func = nullptr; cm_func = nullptr; +#ifdef __PLANNER_DIAGNOSTICS linenum = 0; iterations = 0; block_time_ms = 0; plannable_time_ms = 0; plannable_length = 0; meet_iterations = 0; - +#endif buffer_state = MP_BUFFER_EMPTY; block_type = BLOCK_TYPE_NULL; block_state = BLOCK_INACTIVE; @@ -386,9 +401,8 @@ struct mpBuffer_to_clear { junction_length_since = 0; axis_flags[i] = 0; } - plannable = false; - length = 0.0; + length = 0.0; block_time = 0.0; override_factor = 0.0; cruise_velocity = 0.0; @@ -405,87 +419,45 @@ struct mpBuffer_to_clear { q_recip_2_sqrt_j = 0.0; gm.reset(); } -}; - -typedef struct mpBuffer : mpBuffer_to_clear { // See Planning Velocity Notes for variable usage - - // *** CAUTION *** These two pointers are not reset by _clear_buffer() - struct mpBuffer *pv; // static pointer to previous buffer - struct mpBuffer *nx; // static pointer to next buffer - uint8_t buffer_number; //+++++ DIAGNOSTIC for easier debugging } mpBuf_t; -typedef struct mpBufferPool { // ring buffer for sub-moves - magic_t magic_start; // magic number to test memory integrity - - mpBuf_t *r; // run buffer pointer - mpBuf_t *w; // write buffer pointer - uint8_t buffers_available; // running count of available buffers - mpBuf_t bf[PLANNER_BUFFER_POOL_SIZE];// buffer storage - +typedef struct mpPlannerQueue { // control structure for queue + magic_t magic_start; // magic number to test memory integrity + mpBuf_t *r; // run buffer pointer + mpBuf_t *w; // write buffer pointer + uint8_t queue_size; // total number of buffers, one-based (e.g. 48 not 47) + uint8_t buffers_available; // running count of available buffers in queue + mpBuf_t *bf; // pointer to buffer pool (storage array) magic_t magic_end; -} mpBufferPool_t; +} mpPlannerQueue_t; -typedef struct mpMotionPlannerSingleton { // common variables for planning (move master) - magic_t magic_start; // magic number to test memory integrity +//**** Planner Runtime structures **** - //+++++ DIAGNOSTICS - float run_time_remaining_ms; - float plannable_time_ms; +typedef struct mpBlockRuntimeBuf { // Data structure for just the parts of RunTime that we need to plan a BLOCK + struct mpBlockRuntimeBuf *nx; // singly-linked-list - // planner position - float position[AXES]; // final move position for planning purposes - - // timing variables - float run_time_remaining; // time left in runtime (including running block) - float plannable_time; // time in planner that can actually be planned - - // planner state variables - plannerState planner_state; // current state of planner - bool request_planning; // set true to request backplanning - bool backplanning; // true if planner is in a back-planning pass - bool mfo_active; // true if mfo override is in effect - bool ramp_active; // true when a ramp is occurring - bool entry_changed; // mark if exit_velocity changed to invalidate next block's hint - - // feed overrides and ramp variables (these extend the variables in cm.gmx) - float mfo_factor; // runtime override factor - float ramp_target; - float ramp_dvdt; - - // objects - Timeout block_timeout; // Timeout object for block planning - - // planner pointers - mpBuf_t *p; // planner buffer pointer - mpBuf_t *c; // pointer to buffer immediately following critical region - mpBuf_t *planning_return; // buffer to return to once back-planning is complete - - magic_t magic_end; -} mpMotionPlannerSingleton_t; - -typedef struct mpBlockRuntimeBuf { // Data structure for just the parts of RunTime that we need to plan a BLOCK - struct mpBlockRuntimeBuf *nx; // singly-linked-list - - float head_length; // copies of bf variables of same name + float head_length; // copies of bf variables of same name float body_length; float tail_length; - float head_time; // copies of bf variables of same name + float head_time; // copies of bf variables of same name float body_time; float tail_time; - float cruise_velocity; // velocity at the end of the head and the beginning of the tail - float exit_velocity; // velocity at the end of the move + float cruise_velocity; // velocity at the end of the head and the beginning of the tail + float exit_velocity; // velocity at the end of the move } mpBlockRuntimeBuf_t; -typedef struct mpMotionRuntimeSingleton { // persistent runtime variables -// uint8_t (*run_move)(struct mpMoveRuntimeSingleton *m); // currently running move - left in for reference +typedef struct mpPlannerRuntime { // persistent runtime variables + // uint8_t (*run_move)(struct mpMoveRuntimeSingleton *m); // currently running move - left in for reference magic_t magic_start; // magic number to test memory integrity blockState block_state; // state of the overall move moveSection section; // what section is the move in? sectionState section_state; // state within a move section + bool out_of_band_dwell_flag; // set true to conditionally execute out-of-band dwell + float out_of_band_dwell_seconds; // time for out-of-band dwell + float unit[AXES]; // unit vector for axis scaling & planning bool axis_flags[AXES]; // set true for axes participating in the move float target[AXES]; // final target for bf (used to correct rounding errors) @@ -500,7 +472,10 @@ typedef struct mpMotionRuntimeSingleton { // persistent runtime variables mpBlockRuntimeBuf_t *r; // block that is running mpBlockRuntimeBuf_t *p; // block that is being planned, p might == r - mpBlockRuntimeBuf_t bf[2]; // buffer holding the two blocks + mpBlockRuntimeBuf_t block[2]; // buffer holding the two blocks + + mpBuf_t *plan_bf; // DIAGNOSTIC - pointer to next buffer to plan + mpBuf_t *run_bf; // DIAGNOSTIC - pointer to currently running buffer float entry_velocity; // entry values for the currently running block @@ -519,48 +494,120 @@ typedef struct mpMotionRuntimeSingleton { // persistent runtime variables GCodeState_t gm; // gcode model state currently executing magic_t magic_end; -} mpMotionRuntimeSingleton_t; + + // resets mpPlannerRuntime structure without actually wiping it + void reset() { + block_state = BLOCK_INACTIVE; + section = SECTION_HEAD; + section_state = SECTION_OFF; + entry_velocity = 0; // needed to ensure next block in forward planning starts from 0 velocity + r->exit_velocity = 0; // ditto + segment_velocity = 0; + } + +} mpPlannerRuntime_t; + +//**** Master Planner Structure *** + +typedef struct mpPlanner { // common variables for a planner context + magic_t magic_start; // magic number to test memory integrity + + // DIAGNOSTICS + float run_time_remaining_ms; + float plannable_time_ms; + + // planner position + float position[AXES]; // final move position for planning purposes + + // timing variables + float run_time_remaining; // time left in runtime (including running block) + float plannable_time; // time in planner that can actually be planned + + // planner state variables + plannerState planner_state; // current state of planner + bool request_planning; // set true to request backplanning + bool backplanning; // true if planner is in a back-planning pass + bool mfo_active; // true if mfo override is in effect + bool ramp_active; // true when a ramp is occurring + bool entry_changed; // mark if exit_velocity changed to invalidate next block's hint + + // feed overrides and ramp variables (these extend the variables in cm->gmx) + float mfo_factor; // runtime override factor + float ramp_target; + float ramp_dvdt; + + // objects + Timeout block_timeout; // Timeout object for block planning + + // planner pointers + mpBuf_t *p; // planner buffer pointer + mpBuf_t *c; // pointer to buffer immediately following critical region + mpBuf_t *planning_return; // buffer to return to once back-planning is complete + mpPlannerRuntime_t *mr; // bind to mr associated with this planner + mpPlannerQueue_t q; // embed a planner buffer queue manager + + magic_t magic_end; + + // clears mpPlanner structure but leaves position alone + void reset() { + run_time_remaining = 0; + plannable_time = 0; + planner_state = PLANNER_IDLE; + request_planning = false; + backplanning = false; + mfo_active = false; + ramp_active = false; + entry_changed = false; + block_timeout.clear(); + } +} mpPlanner_t; // Reference global scope structures -extern mpBufferPool_t mb HOT_DATA; // buffer pool management -extern mpMotionPlannerSingleton_t mp HOT_DATA; // context for block planning -extern mpMotionRuntimeSingleton_t mr HOT_DATA; // context for block runtime + +extern mpPlanner_t *mp HOT_DATA; // currently active planner (global variable) +extern mpPlanner_t mp1 HOT_DATA; // primary planning context +extern mpPlanner_t mp2 HOT_DATA; // secondary planning context + +extern mpPlannerRuntime_t *mr HOT_DATA; // context for block runtime +extern mpPlannerRuntime_t mr1 HOT_DATA; // primary planner runtime context +extern mpPlannerRuntime_t mr2 HOT_DATA; // secondary planner runtime context + +extern mpBuf_t mp1_queue[PLANNER_QUEUE_SIZE]; // storage allocation for primary planner queue buffers +extern mpBuf_t mp2_queue[SECONDARY_QUEUE_SIZE] ; // storage allocation for secondary planner queue buffers /* * Global Scope Functions */ -//planner.cpp functions +//**** planner.cpp functions -void planner_init(void); -void planner_reset(void); -void planner_init_assertions(void); -stat_t planner_test_assertions(void); +void planner_init(mpPlanner_t *_mp, mpPlannerRuntime_t *_mr, mpBuf_t *queue, uint8_t queue_size); +void planner_reset(mpPlanner_t *_mp); +stat_t planner_assert(const mpPlanner_t *_mp); void mp_halt_runtime(void); -void mp_flush_planner(void); + void mp_set_planner_position(uint8_t axis, const float position); void mp_set_runtime_position(uint8_t axis, const float position); void mp_set_steps_to_runtime_position(void); stat_t mp_set_target_steps(const float target_steps[]) HOT_FUNC; stat_t mp_set_target_steps(const float target_steps[MOTORS], const float start_velocities[MOTORS], const float end_velocities[MOTORS], const float segment_time) HOT_FUNC; -void mp_queue_command(void(*cm_exec_t)(float[], bool[]), float *value, bool *flag); +void mp_queue_command(void(*cm_exec)(float *, bool *), float *value, bool *flag); stat_t mp_runtime_command(mpBuf_t *bf); stat_t mp_json_command(char *json_string); -stat_t mp_json_wait(char *json_string); stat_t mp_json_command_immediate(char *json_string); +stat_t mp_json_wait(char *json_string); stat_t mp_dwell(const float seconds); void mp_end_dwell(void); void mp_request_out_of_band_dwell(float seconds); -stat_t mp_exec_out_of_band_dwell(void); -// planner functions and helpers -uint8_t mp_get_planner_buffers(void); -bool mp_planner_is_full(void); -bool mp_has_runnable_buffer(void); +//**** planner functions and helpers +uint8_t mp_get_planner_buffers(const mpPlanner_t *_mp); +bool mp_planner_is_full(const mpPlanner_t *_mp); +bool mp_has_runnable_buffer(const mpPlanner_t *_mp); bool mp_is_phat_city_time(void); stat_t mp_planner_callback(); @@ -571,8 +618,12 @@ void mp_start_traverse_override(const float ramp_time, const float override); void mp_end_traverse_override(const float ramp_time); void mp_planner_time_accounting(void); -// planner buffer primitives -void mp_init_buffers(void); +//**** planner buffer primitives +//void mp_init_planner_buffers(void); +//mpBuf_t * mp_get_w(int8_t q); +//mpBuf_t * mp_get_r(int8_t q); +mpBuf_t * mp_get_w(void); +mpBuf_t * mp_get_r(void); //mpBuf_t * mp_get_prev_buffer(const mpBuf_t *bf); // Use the following macro instead //mpBuf_t * mp_get_next_buffer(const mpBuf_t *bf); // Use the following macro instead @@ -584,35 +635,35 @@ void mp_commit_write_buffer(const blockType block_type) HOT_FUNC; mpBuf_t * mp_get_run_buffer(void) HOT_FUNC; bool mp_free_run_buffer(void) HOT_FUNC; -// plan_line.c functions -void mp_zero_segment_velocity(void) HOT_FUNC; // getters and setters... -float mp_get_runtime_velocity(void) HOT_FUNC; -float mp_get_runtime_absolute_position(uint8_t axis); -float mp_get_runtime_work_position(uint8_t axis); -void mp_set_runtime_work_offset(float offset[]); -bool mp_get_runtime_busy(void) HOT_FUNC; -bool mp_runtime_is_idle(void) HOT_FUNC; +//**** plan_line.c functions +void mp_zero_segment_velocity(void); // getters and setters... +float mp_get_runtime_velocity(void); +float mp_get_runtime_absolute_position(mpPlannerRuntime_t *_mr, uint8_t axis); +float mp_get_runtime_display_position(uint8_t axis); +void mp_set_runtime_display_offset(float offset[]); +bool mp_get_runtime_busy(void); +bool mp_runtime_is_idle(void); -stat_t mp_aline(GCodeState_t *gm_in) HOT_FUNC; // line planning... -void mp_plan_block_list(void) HOT_FUNC; -void mp_plan_block_forward(mpBuf_t *bf) HOT_FUNC; +stat_t mp_aline(GCodeState_t *_gm); // line planning... +void mp_plan_block_list(void); +void mp_plan_block_forward(mpBuf_t *bf); -// plan_zoid.c functions -void mp_calculate_ramps(mpBlockRuntimeBuf_t *block, mpBuf_t *bf, const float entry_velocity) HOT_FUNC; -float mp_get_target_length(const float v_0, const float v_1, const mpBuf_t *bf) HOT_FUNC; -float mp_get_target_velocity(const float v_0, const float L, const mpBuf_t *bf) HOT_FUNC; // acceleration ONLY -float mp_get_decel_velocity(const float v_0, const float L, const mpBuf_t *bf) HOT_FUNC; // deceleration ONLY -float mp_find_t(const float v_0, const float v_1, const float L, const float totalL, const float initial_t, const float T) HOT_FUNC; +//**** plan_zoid.c functions +stat_t mp_calculate_ramps(mpBlockRuntimeBuf_t *block, mpBuf_t *bf, const float entry_velocity); +float mp_get_target_length(const float v_0, const float v_1, const mpBuf_t *bf); +float mp_get_target_velocity(const float v_0, const float L, const mpBuf_t *bf); // acceleration ONLY +float mp_get_decel_velocity(const float v_0, const float L, const mpBuf_t *bf); // deceleration ONLY +float mp_find_t(const float v_0, const float v_1, const float L, const float totalL, const float initial_t, const float T); float mp_calc_v(const float t, const float v_0, const float v_1); // compute the velocity along the curve accelerating from v_0 to v_1, at position t=[0,1] float mp_calc_a(const float t, const float v_0, const float v_1, const float T); // compute acceleration over curve accelerating from v_0 to v_1, at position t=[0,1], total time T float mp_calc_j(const float t, const float v_0, const float v_1, const float T); // compute jerk over curve accelerating from v_0 to v_1, at position t=[0,1], total time T //float mp_calc_l(const float t, const float v_0, const float v_1, const float T); // compute length over curve accelerating from v_0 to v_1, at position t=[0,1], total time T -// plan_exec.c functions -stat_t mp_forward_plan(void) HOT_FUNC; -stat_t mp_exec_move(void) HOT_FUNC; -stat_t mp_exec_aline(mpBuf_t *bf) HOT_FUNC; +//**** plan_exec.c functions +stat_t mp_forward_plan(void); +stat_t mp_exec_move(void); +stat_t mp_exec_aline(mpBuf_t *bf); void mp_exit_hold_state(void); void mp_dump_planner(mpBuf_t *bf_start); diff --git a/g2core/pwm.cpp b/g2core/pwm.cpp old mode 100755 new mode 100644 index fe91d3f2..50c52134 --- a/g2core/pwm.cpp +++ b/g2core/pwm.cpp @@ -2,7 +2,7 @@ * pwm.cpp - pulse width modulation drivers * This file is part of the g2core project * - * Copyright (c) 2012 - 2016 Alden S. Hart, Jr. + * Copyright (c) 2012 - 2019 Alden S. Hart, Jr. * * This file ("the software") is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License, version 2 as published by the @@ -37,13 +37,9 @@ /***** PWM defines, structures and memory allocation *****/ -pwmSingleton_t pwm; +pwmControl_t pwm; -// Setup motate PWM pins -//Motate::PWMOutputPin spindle_pwm_pin {Motate::kNormal, P1_PWM_FREQUENCY}; -//Motate::PWMOutputPin secondary_pwm_pin {Motate::kNormal, P1_PWM_FREQUENCY}; // assume the same frequency, to start with at least - gpioDigitalOutput *spindle_pwm_output = nullptr; gpioDigitalOutput *secondary_pwm_output = nullptr; diff --git a/g2core/pwm.h b/g2core/pwm.h old mode 100755 new mode 100644 index eac4a081..d367a873 --- a/g2core/pwm.h +++ b/g2core/pwm.h @@ -2,7 +2,7 @@ * pwm.h - pulse width modulation drivers * This file is part of the g2core project * - * Copyright (c) 2012 - 2016 Alden S. Hart, Jr. + * Copyright (c) 2012 - 2018 Alden S. Hart, Jr. * * This file ("the software") is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License, version 2 as published by the @@ -45,12 +45,12 @@ typedef struct pwmChannel { // no data required in ARM } pwmChannel_t; -typedef struct pwmSingleton { +typedef struct pwmControl { pwmConfigChannel_t c[PWMS]; // array of channel configs pwmChannel_t p[PWMS]; // array of PWM channels -} pwmSingleton_t; +} pwmControl_t; -extern pwmSingleton_t pwm; +extern pwmControl_t pwm; /*** function prototypes ***/ diff --git a/g2core/report.cpp b/g2core/report.cpp index d9e39d0c..554ecc2f 100644 --- a/g2core/report.cpp +++ b/g2core/report.cpp @@ -2,7 +2,7 @@ * report.cpp - Status reports and other reporting functions * This file is part of the g2core project * - * Copyright (c) 2010 - 2016 Alden S. Hart, Jr. + * Copyright (c) 2010 - 2019 Alden S. Hart, Jr. * * This file ("the software") is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License, version 2 as published by the @@ -61,7 +61,7 @@ stat_t rpt_exception(stat_t status, const char *msg) if (cs.controller_state >= CONTROLLER_READY) { char buffer[128]; sprintf(buffer, "{\"er\":{\"fb\":%0.2f,\"st\":%d,\"msg\":\"%s - %s\"}}\n", - G2CORE_FIRMWARE_BUILD, status, get_status_message(status), msg); + G2CORE_FIRMWARE_BUILD, status, get_status_message(status), msg); xio_writeline(buffer); } } @@ -112,6 +112,7 @@ void rpt_print_loading_configs_message(void) void rpt_print_system_ready_message(void) { + // If in Marlin mode and connected as Marlin do not send a startup message #if MARLIN_COMPAT_ENABLED == true if (MARLIN_COMM_MODE != js.json_mode) { _startup_helper(STAT_OK, "SYSTEM READY"); @@ -169,10 +170,10 @@ static uint8_t _populate_filtered_status_report(void); uint8_t _is_stat(nvObj_t *nv) { - char tok[TOKEN_LEN+1]; + char token[TOKEN_LEN+1]; - GET_TOKEN_STRING(nv->value, tok); - if (strcmp(tok, "stat") == 0) { return (true);} + GET_TOKEN_STRING(nv->value_int, token); // pass in index, get back token + if (strcmp(token, "stat") == 0) { return (true);} return (false); } @@ -189,13 +190,16 @@ void sr_init_status_report() sr.status_report_request = SR_OFF; char sr_defaults[NV_STATUS_REPORT_LEN][TOKEN_LEN+1] = { STATUS_REPORT_DEFAULTS }; nv->index = nv_get_index((const char *)"", (char *)"se00"); // set first SR persistence index + + // record the index of the "stat" variable so we can use it during reporting sr.stat_index = nv_get_index((const char *)"", (const char *)"stat"); + // setup the status report array for (uint8_t i=0; i < NV_STATUS_REPORT_LEN ; i++) { if (sr_defaults[i][0] == NUL) break; // quit on first blank array entry sr.status_report_value[i] = -1234567; // pre-load values with an unlikely number - nv->value = nv_get_index((const char *)"", sr_defaults[i]);// load the index for the SR element - if (fp_EQ(nv->value, NO_MATCH)) { + nv->value_int = nv_get_index((const char *)"", sr_defaults[i]);// load the index for the SR element + if (nv->value_int == NO_MATCH) { rpt_exception(STAT_BAD_STATUS_REPORT_SETTING, "sr_init_status_report() encountered bad SR setting"); // trap mis-configured profile settings return; } @@ -203,12 +207,10 @@ void sr_init_status_report() nv_persist(nv); // conditionally persist - automatic by nv_persist() nv->index++; // increment SR NVM index } - // record the index of the "stat" variable so we can use it during reporting - sr.index_of_stat_variable = nv_get_index((const char *)"", (const char *)"stat"); } /* - * sr_set_status_report() - interpret an SR setup string and return current report + * sr_set_status_report() - read a list of NV pairs to set up SRs and return a report * * Note: By the time this function is called any unrecognized tokens have been detected and * rejected by the JSON or text parser. In other words, it should never get to here if @@ -226,9 +228,10 @@ stat_t sr_set_status_report(nvObj_t *nv) if (((nv = nv->nx) == NULL) || (nv->valuetype == TYPE_EMPTY)) { break; } - if ((nv->valuetype == TYPE_BOOL) && (fp_TRUE(nv->value))) { + // Note: valuetype may have been coerced from boolean to something else, so just treat value_int as a bool + if (nv->value_int) { status_report_list[i] = nv->index; - nv->value = nv->index; // persist the index as the value + nv->value_int = nv->index; // persist the index as the value nv->index = sr_start + i; // index of the SR persistence location nv_persist(nv); elements++; @@ -374,7 +377,12 @@ static uint8_t _populate_filtered_status_report() const char sr_str[] = "sr"; bool has_data = false; char tmp[TOKEN_LEN+1]; + float current_value; nvObj_t *nv = nv_reset_nv_list(); // sets nv to the start of the body + + // Set thresholds to detect value changes based on precision for the value. + // Allow for floating point roundoffs, i.e. precision = 2 is 0.01 becomes --> 0.009 + float precision[8] = { 0.9, 0.09, 0.009, 0.0009, 0.00009, 0.000009, 0.0000009, 0.00000009 }; nv->valuetype = TYPE_PARENT; // setup the parent object (no need to length check the copy) strcpy(nv->token, sr_str); @@ -387,20 +395,30 @@ static uint8_t _populate_filtered_status_report() } nv_get_nvObj(nv); - // report values that have changed by more than 0.0001, but always stops and ends - if ((std::abs(nv->value - sr.status_report_value[i]) > EPSILON3) || - ((nv->index == sr.stat_index) && fp_EQ(nv->value, COMBINED_PROGRAM_STOP)) || - ((nv->index == sr.stat_index) && fp_EQ(nv->value, COMBINED_PROGRAM_END))) { + // extract the value and cast into a float, regardless of value type + if ((valueType)(cfgArray[nv->index].flags & F_TYPE_MASK) == TYPE_FLOAT) { + current_value = nv->value_flt; + } else { + current_value = (float)nv->value_int; + } + + // report values that have changed by more than the indicated precision, but always stops and ends + if ((fabs(current_value - sr.status_report_value[i]) > precision[cfgArray[nv->index].precision]) || +// ((nv->index == sr.stat_index) && fp_EQ(nv->value_int, COMBINED_PROGRAM_STOP)) || +// ((nv->index == sr.stat_index) && fp_EQ(nv->value_int, COMBINED_PROGRAM_END))) { + ((nv->index == sr.stat_index) && (nv->value_int == COMBINED_PROGRAM_STOP)) || + ((nv->index == sr.stat_index) && (nv->value_int == COMBINED_PROGRAM_END))) { strcpy(tmp, nv->group); // flatten out groups - WARNING - you cannot use strncpy here... strcat(tmp, nv->token); strcpy(nv->token, tmp); //...or here. - sr.status_report_value[i] = nv->value; - if ((nv = nv->nx) == NULL) return (false); // should never be NULL unless SR length exceeds available buffer array + sr.status_report_value[i] = current_value; + if ((nv = nv->nx) == NULL) { // should never be NULL unless SR length exceeds available buffer array + return (false); + } has_data = true; - } else { - nv->valuetype = TYPE_EMPTY; // filter this value out of the report + nv->valuetype = TYPE_EMPTY; // filter this value out of the report } } return (has_data); @@ -419,42 +437,33 @@ static uint8_t _populate_filtered_status_report() /* * Wrappers and Setters - for calling from nvArray table * - * sr_get() - run status report - * sr_set() - set status report elements - * sr_set_si() - set status report interval + * sr_get() - run status report + * sr_set() - set status report elements + * sr_get_sv() - get status report verbosity + * sr_set_sv() - set status report verbosity + * sr_get_si() - get status report interval + * sr_set_si() - set status report interval */ -stat_t sr_get(nvObj_t *nv) -{ - return (_populate_unfiltered_status_report()); -} +stat_t sr_get(nvObj_t *nv) { return (_populate_unfiltered_status_report()); } +stat_t sr_set(nvObj_t *nv) { return (sr_set_status_report(nv)); } -stat_t sr_set(nvObj_t *nv) -{ - return (sr_set_status_report(nv)); -} - -stat_t sr_set_si(nvObj_t *nv) -{ - if (nv->value < STATUS_REPORT_MIN_MS) { - nv->valuetype = TYPE_NULL; - return(STAT_INPUT_LESS_THAN_MIN_VALUE); - } - set_int(nv); - return(STAT_OK); -} +stat_t sr_get_sv(nvObj_t *nv) { return(get_integer(nv, (uint8_t &)sr.status_report_verbosity)); } +stat_t sr_set_sv(nvObj_t *nv) { return(set_integer(nv, (uint8_t &)sr.status_report_verbosity, SR_OFF, SR_VERBOSE)); } +stat_t sr_get_si(nvObj_t *nv) { return(get_integer(nv, sr.status_report_interval)); } +stat_t sr_set_si(nvObj_t *nv) { return(set_int32(nv, sr.status_report_interval, STATUS_REPORT_MIN_MS, STATUS_REPORT_MAX_MS)); } /********************* * TEXT MODE SUPPORT * *********************/ #ifdef __TEXT_MODE -static const char fmt_si[] = "[si] status interval%14d ms\n"; static const char fmt_sv[] = "[sv] status report verbosity%6d [0=off,1=filtered,2=verbose]\n"; +static const char fmt_si[] = "[si] status interval%14d ms\n"; void sr_print_sr(nvObj_t *nv) { _populate_unfiltered_status_report();} -void sr_print_si(nvObj_t *nv) { text_print(nv, fmt_si);} void sr_print_sv(nvObj_t *nv) { text_print(nv, fmt_sv);} +void sr_print_si(nvObj_t *nv) { text_print(nv, fmt_si);} #endif // __TEXT_MODE @@ -499,7 +508,7 @@ void qr_init_queue_report() void qr_request_queue_report(int8_t buffers) { // get buffer depth and added/removed count - qr.buffers_available = mp_get_planner_buffers(); + qr.buffers_available = mp_get_planner_buffers(mp); if (buffers > 0) { qr.buffers_added += buffers; } else { @@ -508,7 +517,7 @@ void qr_request_queue_report(int8_t buffers) // time-throttle requests while generating arcs // qr.motion_mode = cm_get_motion_mode(ACTIVE_MODEL); - qr.motion_mode = cm_get_motion_mode((GCodeState_t *)&cm.gm); + qr.motion_mode = cm_get_motion_mode((GCodeState_t *)&(cm->gm)); if ((qr.motion_mode == MOTION_MODE_CW_ARC) || (qr.motion_mode == MOTION_MODE_CCW_ARC)) { uint32_t tick = SysTickTimer.getValue(); if (tick - qr.init_tick < MIN_ARC_QR_INTERVAL) { @@ -583,27 +592,30 @@ stat_t qr_queue_report_callback() // called by controller dispatcher */ stat_t qr_get(nvObj_t *nv) { - nv->value = (float)mp_get_planner_buffers(); // ensure that manually requested QR count is always up to date - nv->valuetype = TYPE_INT; + nv->value_int = mp_get_planner_buffers(mp); // ensure that manually requested QR count is always up to date + nv->valuetype = TYPE_INTEGER; return (STAT_OK); } stat_t qi_get(nvObj_t *nv) { - nv->value = (float)qr.buffers_added; - nv->valuetype = TYPE_INT; + nv->value_int = qr.buffers_added; + nv->valuetype = TYPE_INTEGER; qr.buffers_added = 0; // reset it return (STAT_OK); } stat_t qo_get(nvObj_t *nv) { - nv->value = (float)qr.buffers_removed; - nv->valuetype = TYPE_INT; + nv->value_int = qr.buffers_removed; + nv->valuetype = TYPE_INTEGER; qr.buffers_removed = 0; // reset it return (STAT_OK); } +stat_t qr_get_qv(nvObj_t *nv) { return(get_integer(nv, (uint8_t &)qr.queue_report_verbosity)); } +stat_t qr_set_qv(nvObj_t *nv) { return(set_integer(nv, (uint8_t &)qr.queue_report_verbosity, QR_OFF, QR_TRIPLE)); } + /***************************************************************************** * JOB ID REPORTS * @@ -648,8 +660,8 @@ stat_t job_set_job_report(nvObj_t *nv) for (uint8_t i=0; i<4; i++) { if (((nv = nv->nx) == NULL) || (nv->valuetype == TYPE_EMPTY)) { break;} - if (nv->valuetype == TYPE_INT) { - cfg.job_id[i] = nv->value; + if (nv->valuetype == TYPE_INTEGER) { + cfg.job_id[i] = nv->value_int; nv->index = job_start + i; // index of the SR persistence location nv_persist(nv); } else { diff --git a/g2core/report.h b/g2core/report.h index 01e7ad1e..d81daf5f 100644 --- a/g2core/report.h +++ b/g2core/report.h @@ -2,7 +2,7 @@ * report.h - Status reports and other reporting functions * This file is part of the g2core project * - * Copyright (c) 2010 - 2016 Alden S. Hart, Jr. + * Copyright (c) 2010 - 2018 Alden S. Hart, Jr. * * This file ("the software") is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License, version 2 as published by the @@ -36,6 +36,7 @@ #define SR_THROTTLE_COUNT 4 // scale back filtered SR's during time-constrained intervals #define MIN_ARC_QR_INTERVAL 200 // minimum interval between QRs during arc generation (in system ticks) +#define STATUS_REPORT_MAX_MS (MAX_LONG/1000) typedef enum { // status report enable, verbosity and request type SR_OFF = 0, // no reports @@ -60,12 +61,11 @@ typedef struct srSingleton { /*** config values (PUBLIC) ***/ srVerbosity status_report_verbosity; - uint32_t status_report_interval; // in milliseconds + int32_t status_report_interval; // in milliseconds /*** runtime values (PRIVATE) ***/ srVerbosity status_report_request; // flag that SR has been requested, and what type uint32_t status_report_systick; // SysTick value for next status report - index_t index_of_stat_variable; // like it says, the index of the "stat" variable index_t stat_index; // table index value for stat - determined during initialization uint8_t throttle_counter; // slow down SRs when in a constrained time (not phat_city) index_t status_report_list[NV_STATUS_REPORT_LEN]; // status report elements to report @@ -112,6 +112,10 @@ stat_t sr_run_text_status_report(void); stat_t sr_get(nvObj_t *nv); stat_t sr_set(nvObj_t *nv); + +stat_t sr_get_sv(nvObj_t *nv); +stat_t sr_set_sv(nvObj_t *nv); +stat_t sr_get_si(nvObj_t *nv); stat_t sr_set_si(nvObj_t *nv); void qr_init_queue_report(void); @@ -125,6 +129,9 @@ stat_t qr_get(nvObj_t *nv); stat_t qi_get(nvObj_t *nv); stat_t qo_get(nvObj_t *nv); +stat_t qr_get_qv(nvObj_t *nv); +stat_t qr_set_qv(nvObj_t *nv); + #ifdef __TEXT_MODE void sr_print_sr(nvObj_t *nv); diff --git a/g2core/settings.h b/g2core/settings.h old mode 100755 new mode 100644 index eaa2fb7d..fa5077a5 --- a/g2core/settings.h +++ b/g2core/settings.h @@ -2,7 +2,7 @@ * settings.h - default runtime settings * This file is part of the g2core project * - * Copyright (c) 2010 - 2016 Alden S. Hart Jr. + * Copyright (c) 2010 - 2018 Alden S. Hart Jr. * * This file ("the software") is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License, version 2 as published by the @@ -35,6 +35,10 @@ #ifndef SETTINGS_H_ONCE #define SETTINGS_H_ONCE +// Defines that need to be here instead of a more logical place like canonical_machine.h + +#define RADIUS_MIN (0.0001) // minimum value for ABC radius settings + /**** MACHINE PROFILES ****************************************************** * * Provide an optional SETTINGS_FILE in the makefile or compiler command line: @@ -54,4 +58,17 @@ // The defaults below generally disable that function #include "settings/settings_default.h" +// compile-time assertions - mostly checking the settings are not impossible + +#define stringify2(a) #a +#define stringify(a) stringify2(a) + +//static_assert ( bool_constexpr , message ) // bool_constexpr must be true or assertion will fail +static_assert ( (A_RADIUS > RADIUS_MIN), "A axis radius must be more than " stringify(RADIUS_MIN) ", but is " stringify(A_RADIUS) ); +static_assert ( (B_RADIUS > RADIUS_MIN), "B axis radius must be more than " stringify(RADIUS_MIN) ", but is " stringify(B_RADIUS) ); +static_assert ( (C_RADIUS > RADIUS_MIN), "C axis radius must be more than " stringify(RADIUS_MIN) ", but is " stringify(C_RADIUS) ); + +#undef stringify +#undef stringify2 + #endif // End of include guard: SETTINGS_H_ONCE diff --git a/g2core/settings/settings_Printrbot_Play.h b/g2core/settings/settings_Printrbot_Play.h index 1420995f..7c7acb29 100644 --- a/g2core/settings/settings_Printrbot_Play.h +++ b/g2core/settings/settings_Printrbot_Play.h @@ -2,8 +2,8 @@ * settings_Printrbot_play.h * This file is part of the the g2core project * - * Copyright (c) 2010 - 2017 Alden S. Hart, Jr. - * Copyright (c) 2010 - 2017 Robert Giseburt + * Copyright (c) 2010 - 2019 Alden S. Hart, Jr. + * Copyright (c) 2010 - 2019 Robert Giseburt * * This file ("the software") is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License, version 2 as published by the @@ -113,115 +113,50 @@ // *** motor settings ************************************************************************************ -#define MOTOR_POWER_MODE MOTOR_POWERED_IN_CYCLE // default motor power mode (see cmMotorPowerMode in stepper.h) +#define MOTOR_POWER_MODE MOTOR_POWERED_IN_CYCLE // default motor power mode (see cmMotorPowerMode in stepper.h) // 80 steps/mm at 1/16 microstepping = 40 mm/rev -#define M1_MOTOR_MAP AXIS_X // 1ma -#define M1_STEP_ANGLE 1.8 // 1sa -// Marlin says 80 steps/unit, and 16 microsteps, with a 200-step/rev motor -#define M1_TRAVEL_PER_REV 40.64 // 1tr -#define M1_MICROSTEPS 128 // 1mi 1,2,4,8,16,32 -#define M1_POLARITY 1 // 1po 0=normal, 1=reversed -#define M1_POWER_MODE MOTOR_POWERED_IN_CYCLE // 1pm standard -#define M1_POWER_LEVEL 0.8 // 1pl -#define M1_TMC2130_TPWMTHRS 1200 // 1pth -#define M1_TMC2130_TCOOLTHRS 1000 // 1cth -#define M1_TMC2130_THIGH 10 // 1hth -#define M1_TMC2130_SGT 4 // 1sgt -#define M1_TMC2130_TBL 2 // 1tbl -#define M1_TMC2130_PWM_GRAD 1 // 1pgrd -#define M1_TMC2130_PWM_AMPL 200 // 1pamp -#define M1_TMC2130_HEND 0 // 1hend -#define M1_TMC2130_HSTRT 0 // 1hsrt -#define M1_TMC2130_SMIN 5 // 1smin -#define M1_TMC2130_SMAX 5 // 1smax -#define M1_TMC2130_SUP 2 // 1sup -#define M1_TMC2130_SDN 1 // 1sdn +#define M1_MOTOR_MAP AXIS_X_EXTERNAL // 1ma +#define M1_STEP_ANGLE 1.8 // 1sa +#define M1_TRAVEL_PER_REV 40.64 // 1tr +#define M1_MICROSTEPS 32 // 1mi 1,2,4,8,16,32 +#define M1_POLARITY 1 // 1po 0=normal, 1=reversed +#define M1_POWER_MODE MOTOR_POWER_MODE // 1pm standard +#define M1_POWER_LEVEL 0.4 // 1pl: 0.0=no power, 1.0=max power // 80 steps/mm at 1/16 microstepping = 40 mm/rev -#define M2_MOTOR_MAP AXIS_Y -#define M2_STEP_ANGLE 1.8 -// Marlin says 80 steps/unit, and 16 microsteps, with a 200-step/rev motor -#define M2_TRAVEL_PER_REV 40.64 -#define M2_MICROSTEPS 128 -#define M2_POLARITY 0 -#define M2_POWER_MODE MOTOR_POWERED_IN_CYCLE -#define M2_POWER_LEVEL 0.8 -#define M2_TMC2130_TPWMTHRS 1200 -#define M2_TMC2130_TCOOLTHRS 1000 -#define M2_TMC2130_THIGH 10 -#define M2_TMC2130_SGT 4 -#define M2_TMC2130_TBL 2 -#define M2_TMC2130_PWM_GRAD 1 -#define M2_TMC2130_PWM_AMPL 200 -#define M2_TMC2130_HEND 0 -#define M2_TMC2130_HSTRT 0 -#define M2_TMC2130_SMIN 5 -#define M2_TMC2130_SMAX 5 -#define M2_TMC2130_SUP 2 -#define M2_TMC2130_SDN 1 +#define M3_MOTOR_MAP AXIS_Y_EXTERNAL +#define M3_STEP_ANGLE 1.8 +#define M3_TRAVEL_PER_REV 40.64 +#define M3_MICROSTEPS 32 +#define M3_POLARITY 1 +#define M3_POWER_MODE MOTOR_POWER_MODE +#define M3_POWER_LEVEL 0.4 -#define M3_MOTOR_MAP AXIS_Z -#define M3_STEP_ANGLE 1.8 -#define M3_TRAVEL_PER_REV 1.5875 -#define M3_MICROSTEPS 128 -#define M3_POLARITY 0 -#define M3_POWER_MODE MOTOR_POWERED_IN_CYCLE -#define M3_POWER_LEVEL 0.6 -#define M3_TMC2130_TPWMTHRS 300 -#define M3_TMC2130_TCOOLTHRS 200 -#define M3_TMC2130_THIGH 10 -#define M3_TMC2130_SGT 4 -#define M3_TMC2130_TBL 2 -#define M3_TMC2130_PWM_GRAD 1 -#define M3_TMC2130_PWM_AMPL 200 -#define M3_TMC2130_HEND 0 -#define M3_TMC2130_HSTRT 0 -#define M3_TMC2130_SMIN 5 -#define M3_TMC2130_SMAX 12 -#define M3_TMC2130_SUP 2 -#define M3_TMC2130_SDN 2 +#define M2_MOTOR_MAP AXIS_Z_EXTERNAL +#define M2_STEP_ANGLE 1.8 +#define M2_TRAVEL_PER_REV 1.5875 +#define M2_MICROSTEPS 32 +#define M2_POLARITY 1 +#define M2_POWER_MODE MOTOR_POWER_MODE +#define M2_POWER_LEVEL 0.4 -#define M4_MOTOR_MAP AXIS_A -#define M4_STEP_ANGLE 1.8 -#define M4_TRAVEL_PER_REV 360 // degrees moved per motor rev -#define M4_MICROSTEPS 128 -#define M4_POLARITY 0 -#define M4_POWER_MODE MOTOR_POWER_MODE -#define M4_POWER_LEVEL 0.8 -#define M4_TMC2130_TPWMTHRS 180000 -#define M4_TMC2130_TCOOLTHRS 100000 -#define M4_TMC2130_THIGH 10 -#define M4_TMC2130_SGT 3 -#define M4_TMC2130_TBL 2 -#define M4_TMC2130_PWM_GRAD 15 -#define M4_TMC2130_PWM_AMPL 255 -#define M4_TMC2130_HEND 0 -#define M4_TMC2130_HSTRT 0 -#define M4_TMC2130_SMIN 5 -#define M4_TMC2130_SMAX 10 -#define M4_TMC2130_SUP 3 -#define M4_TMC2130_SDN 0 +// 96 steps/mm at 1/16 microstepping = 33.3333 mm/rev +#define M4_MOTOR_MAP AXIS_A_EXTERNAL +#define M4_STEP_ANGLE 1.8 +#define M4_TRAVEL_PER_REV 360 // degrees moved per motor rev +#define M4_MICROSTEPS 32 +#define M4_POLARITY 1 +#define M4_POWER_MODE MOTOR_POWER_MODE +#define M4_POWER_LEVEL 0.4 -#define M5_MOTOR_MAP AXIS_B -#define M5_STEP_ANGLE 1.8 -#define M5_TRAVEL_PER_REV 40 -#define M5_MICROSTEPS 128 -#define M5_POLARITY 1 -#define M5_POWER_MODE MOTOR_DISABLED -#define M5_POWER_LEVEL 0.8 -#define M5_TMC2130_TPWMTHRS 1200 -#define M5_TMC2130_TCOOLTHRS 1000 -#define M5_TMC2130_THIGH 10 -#define M5_TMC2130_SGT 4 -#define M5_TMC2130_TBL 2 -#define M5_TMC2130_PWM_GRAD 1 -#define M5_TMC2130_PWM_AMPL 200 -#define M5_TMC2130_HEND 0 -#define M5_TMC2130_HSTRT 0 -#define M5_TMC2130_SMIN 5 -#define M5_TMC2130_SMAX 12 -#define M5_TMC2130_SUP 2 -#define M5_TMC2130_SDN 1 +// 96 steps/mm at 1/16 microstepping = 33.3333 mm/rev +#define M5_MOTOR_MAP AXIS_B_EXTERNAL +#define M5_STEP_ANGLE 1.8 +#define M5_TRAVEL_PER_REV 360 // degrees moved per motor rev +#define M5_MICROSTEPS 32 +#define M5_POLARITY 0 +#define M5_POWER_MODE MOTOR_POWER_MODE +#define M5_POWER_LEVEL 0.35 // *** axis settings ********************************************************************************** diff --git a/g2core/settings/settings_Printrbot_Plus.h b/g2core/settings/settings_Printrbot_Plus.h index 2ec59827..a2971968 100644 --- a/g2core/settings/settings_Printrbot_Plus.h +++ b/g2core/settings/settings_Printrbot_Plus.h @@ -2,8 +2,8 @@ * settings_Printrbot_plus.h * This file is part of the the g2core project * - * Copyright (c) 2010 - 2017 Alden S. Hart, Jr. - * Copyright (c) 2010 - 2017 Robert Giseburt + * Copyright (c) 2010 - 2018 Alden S. Hart, Jr. + * Copyright (c) 2010 - 2018 Robert Giseburt * * This file ("the software") is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License, version 2 as published by the @@ -118,7 +118,7 @@ #define MOTOR_POWER_MODE MOTOR_POWERED_IN_CYCLE // default motor power mode (see cmMotorPowerMode in stepper.h) // 80 steps/mm at 1/16 microstepping = 40 mm/rev -#define M1_MOTOR_MAP AXIS_X // 1ma +#define M1_MOTOR_MAP AXIS_X_EXTERNAL // 1ma #define M1_STEP_ANGLE 1.8 // 1sa #define M1_TRAVEL_PER_REV 40.64 // 1tr #define M1_MICROSTEPS 32 // 1mi 1,2,4,8,16,32 @@ -127,7 +127,7 @@ #define M1_POWER_LEVEL 0.4 // 1pl: 0.0=no power, 1.0=max power // 80 steps/mm at 1/16 microstepping = 40 mm/rev -#define M3_MOTOR_MAP AXIS_Y +#define M3_MOTOR_MAP AXIS_Y_EXTERNAL #define M3_STEP_ANGLE 1.8 #define M3_TRAVEL_PER_REV 40.64 #define M3_MICROSTEPS 32 @@ -135,7 +135,7 @@ #define M3_POWER_MODE MOTOR_POWER_MODE #define M3_POWER_LEVEL 0.4 -#define M2_MOTOR_MAP AXIS_Z +#define M2_MOTOR_MAP AXIS_Z_EXTERNAL #define M2_STEP_ANGLE 1.8 #define M2_TRAVEL_PER_REV 1.5875 #define M2_MICROSTEPS 32 @@ -144,7 +144,7 @@ #define M2_POWER_LEVEL 0.4 // 96 steps/mm at 1/16 microstepping = 33.3333 mm/rev -#define M4_MOTOR_MAP AXIS_A +#define M4_MOTOR_MAP AXIS_A_EXTERNAL #define M4_STEP_ANGLE 1.8 #define M4_TRAVEL_PER_REV 360 // degrees moved per motor rev #define M4_MICROSTEPS 32 @@ -153,7 +153,7 @@ #define M4_POWER_LEVEL 0.4 // 96 steps/mm at 1/16 microstepping = 33.3333 mm/rev -#define M5_MOTOR_MAP AXIS_B +#define M5_MOTOR_MAP AXIS_B_EXTERNAL #define M5_STEP_ANGLE 1.8 #define M5_TRAVEL_PER_REV 360 // degrees moved per motor rev #define M5_MICROSTEPS 32 diff --git a/g2core/settings/settings_Printrbot_Simple_1403.h b/g2core/settings/settings_Printrbot_Simple_1403.h index 25318c00..de04c7c5 100644 --- a/g2core/settings/settings_Printrbot_Simple_1403.h +++ b/g2core/settings/settings_Printrbot_Simple_1403.h @@ -2,8 +2,8 @@ * settings_printrbot_simple_1403.h - 2013 Simple model * This file is part of the the g2core project * - * Copyright (c) 2010 - 2017 Alden S. Hart, Jr. - * Copyright (c) 2010 - 2017 Robert Giseburt + * Copyright (c) 2010 - 2018 Alden S. Hart, Jr. + * Copyright (c) 2010 - 2018 Robert Giseburt * * This file ("the software") is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License, version 2 as published by the @@ -101,7 +101,7 @@ #define MOTOR_POWER_MODE MOTOR_POWERED_IN_CYCLE // default motor power mode (see cmMotorPowerMode in stepper.h) // 80 steps/mm at 1/16 microstepping = 40 mm/rev -#define M1_MOTOR_MAP AXIS_X // 1ma +#define M1_MOTOR_MAP AXIS_X_EXTERNAL // 1ma #define M1_STEP_ANGLE 1.8 // 1sa #define M1_TRAVEL_PER_REV 40.64 // 1tr #define M1_MICROSTEPS 32 // 1mi 1,2,4,8,16,32 @@ -110,7 +110,7 @@ #define M1_POWER_LEVEL 0.4 // 1pl: 0.0=no power, 1.0=max power // 80 steps/mm at 1/16 microstepping = 40 mm/rev -#define M3_MOTOR_MAP AXIS_Y +#define M3_MOTOR_MAP AXIS_Y_EXTERNAL #define M3_STEP_ANGLE 1.8 #define M3_TRAVEL_PER_REV 40.64 #define M3_MICROSTEPS 32 @@ -118,7 +118,7 @@ #define M3_POWER_MODE MOTOR_POWER_MODE #define M3_POWER_LEVEL 0.4 -#define M2_MOTOR_MAP AXIS_Z +#define M2_MOTOR_MAP AXIS_Z_EXTERNAL #define M2_STEP_ANGLE 1.8 #define M2_TRAVEL_PER_REV 1.5875 #define M2_MICROSTEPS 32 @@ -127,7 +127,7 @@ #define M2_POWER_LEVEL 0.4 // 96 steps/mm at 1/16 microstepping = 33.3333 mm/rev -#define M4_MOTOR_MAP AXIS_A +#define M4_MOTOR_MAP AXIS_A_EXTERNAL #define M4_STEP_ANGLE 1.8 #define M4_TRAVEL_PER_REV 360 // degrees moved per motor rev #define M4_MICROSTEPS 32 @@ -136,7 +136,7 @@ #define M4_POWER_LEVEL 0.4 // 96 steps/mm at 1/16 microstepping = 33.3333 mm/rev -#define M5_MOTOR_MAP AXIS_B +#define M5_MOTOR_MAP AXIS_B_EXTERNAL #define M5_STEP_ANGLE 1.8 #define M5_TRAVEL_PER_REV 360 // degrees moved per motor rev #define M5_MICROSTEPS 32 diff --git a/g2core/settings/settings_Printrbot_Simple_1608.h b/g2core/settings/settings_Printrbot_Simple_1608.h index ad64072a..bfb3ee40 100644 --- a/g2core/settings/settings_Printrbot_Simple_1608.h +++ b/g2core/settings/settings_Printrbot_Simple_1608.h @@ -2,8 +2,8 @@ * settings_printrbot_simple_1608.h - New Simple, 2016 version * This file is part of the the g2core project * - * Copyright (c) 2010 - 2017 Alden S. Hart, Jr. - * Copyright (c) 2010 - 2017 Robert Giseburt + * Copyright (c) 2010 - 2018 Alden S. Hart, Jr. + * Copyright (c) 2010 - 2018 Robert Giseburt * * This file ("the software") is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License, version 2 as published by the @@ -114,7 +114,7 @@ #define MOTOR_POWER_MODE MOTOR_POWERED_IN_CYCLE // default motor power mode (see cmMotorPowerMode in stepper.h) // 80 steps/mm at 1/16 microstepping = 40 mm/rev -#define M1_MOTOR_MAP AXIS_X // 1ma +#define M1_MOTOR_MAP AXIS_X_EXTERNAL // 1ma #define M1_STEP_ANGLE 1.8 // 1sa #define M1_TRAVEL_PER_REV 40.011604 // 1tr #define M1_MICROSTEPS 32 // 1mi 1,2,4,8,16,32 @@ -123,7 +123,7 @@ #define M1_POWER_LEVEL 0.35 // 1mp // 80 steps/mm at 1/16 microstepping = 40 mm/rev -#define M3_MOTOR_MAP AXIS_Y +#define M3_MOTOR_MAP AXIS_Y_EXTERNAL #define M3_STEP_ANGLE 1.8 #define M3_TRAVEL_PER_REV 40.011604 #define M3_MICROSTEPS 32 @@ -132,7 +132,7 @@ #define M3_POWER_LEVEL 0.35 // 2020 steps/mm at 1/16 microstepping = 1.58416 mm/rev -#define M2_MOTOR_MAP AXIS_Z +#define M2_MOTOR_MAP AXIS_Z_EXTERNAL #define M2_STEP_ANGLE 1.8 #define M2_TRAVEL_PER_REV 8 #define M2_MICROSTEPS 32 @@ -141,7 +141,7 @@ #define M2_POWER_LEVEL 0.3 // 96 steps/mm at 1/16 microstepping = 33.3333 mm/rev -#define M4_MOTOR_MAP AXIS_A +#define M4_MOTOR_MAP AXIS_A_EXTERNAL #define M4_STEP_ANGLE 1.8 #define M4_TRAVEL_PER_REV 360 // degrees moved per motor rev #define M4_MICROSTEPS 32 @@ -150,7 +150,7 @@ #define M4_POWER_LEVEL 0.3 // 96 steps/mm at 1/16 microstepping = 33.3333 mm/rev -#define M5_MOTOR_MAP AXIS_B +#define M5_MOTOR_MAP AXIS_B_EXTERNAL #define M5_STEP_ANGLE 1.8 #define M5_TRAVEL_PER_REV 360 // degrees moved per motor rev #define M5_MICROSTEPS 32 diff --git a/g2core/settings/settings_Ultimaker.h b/g2core/settings/settings_Ultimaker.h index 375d5edc..71604832 100644 --- a/g2core/settings/settings_Ultimaker.h +++ b/g2core/settings/settings_Ultimaker.h @@ -2,8 +2,8 @@ * settings_Ultimaker.h - Ultimaker motion demo * This file is part of the the g2core project * - * Copyright (c) 2010 - 2016 Alden S. Hart, Jr. - * Copyright (c) 2010 - 2016 Robert Giseburt + * Copyright (c) 2010 - 2018 Alden S. Hart, Jr. + * Copyright (c) 2010 - 2018 Robert Giseburt * * This file ("the software") is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License, version 2 as published by the @@ -85,7 +85,7 @@ #define MOTOR_POWER_MODE MOTOR_POWERED_IN_CYCLE // default motor power mode (see cmMotorPowerMode in stepper.h) #define MOTOR_POWER_TIMEOUT 2.00 // motor power timeout in seconds -#define M1_MOTOR_MAP AXIS_X // 1ma +#define M1_MOTOR_MAP AXIS_X_EXTERNAL // 1ma #define M1_STEP_ANGLE 1.8 // 1sa #define M1_TRAVEL_PER_REV 40.64 // 1tr #define M1_MICROSTEPS 8 // 1mi 1,2,4,8,16,32 @@ -93,7 +93,7 @@ #define M1_POWER_MODE MOTOR_POWER_MODE // 1pm standard #define M1_POWER_LEVEL 0.375 // 1pl: 0.0=no power, 1.0=max power -#define M2_MOTOR_MAP AXIS_Y +#define M2_MOTOR_MAP AXIS_Y_EXTERNAL #define M2_STEP_ANGLE 1.8 #define M2_TRAVEL_PER_REV 40.64 #define M2_MICROSTEPS 8 @@ -101,7 +101,7 @@ #define M2_POWER_MODE MOTOR_POWER_MODE #define M2_POWER_LEVEL 0.375 -#define M3_MOTOR_MAP AXIS_Z +#define M3_MOTOR_MAP AXIS_Z_EXTERNAL #define M3_STEP_ANGLE 1.8 #define M3_TRAVEL_PER_REV 3.00 #define M3_MICROSTEPS 8 @@ -109,7 +109,7 @@ #define M3_POWER_MODE MOTOR_POWER_MODE #define M3_POWER_LEVEL 0.375 -#define M4_MOTOR_MAP AXIS_A +#define M4_MOTOR_MAP AXIS_A_EXTERNAL #define M4_STEP_ANGLE 1.8 #define M4_TRAVEL_PER_REV 360 // degrees moved per motor rev #define M4_MICROSTEPS 8 @@ -117,7 +117,7 @@ #define M4_POWER_MODE MOTOR_POWER_MODE #define M4_POWER_LEVEL 0.375 -#define M5_MOTOR_MAP AXIS_B +#define M5_MOTOR_MAP AXIS_B_EXTERNAL #define M5_STEP_ANGLE 1.8 #define M5_TRAVEL_PER_REV 360 // degrees moved per motor rev #define M5_MICROSTEPS 8 @@ -125,7 +125,7 @@ #define M5_POWER_MODE MOTOR_POWER_MODE #define M5_POWER_LEVEL 0.375 -#define M6_MOTOR_MAP AXIS_C +#define M6_MOTOR_MAP AXIS_C_EXTERNAL #define M6_STEP_ANGLE 1.8 #define M6_TRAVEL_PER_REV 360 // degrees moved per motor rev #define M6_MICROSTEPS 8 diff --git a/g2core/settings/settings_default.h b/g2core/settings/settings_default.h index b28f3c5f..fc723799 100644 --- a/g2core/settings/settings_default.h +++ b/g2core/settings/settings_default.h @@ -2,7 +2,7 @@ * settings_default.h - default machine profile - Set for Shapeoko2 * This file is part of the g2core project * - * Copyright (c) 2012 - 2017 Alden S. Hart, Jr. + * Copyright (c) 2012 - 2018 Alden S. Hart, Jr. * * This file ("the software") is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License, version 2 as published by the @@ -58,11 +58,6 @@ // *** Machine configuration settings *** // -#ifndef USB_SERIAL_PORTS_EXPOSED -#define USB_SERIAL_PORTS_EXPOSED 1 // Valid options are 1 or 2, only! -#endif - - #ifndef JUNCTION_INTEGRATION_TIME #define JUNCTION_INTEGRATION_TIME 0.75 // {jt: cornering - between 0.05 and 2.00 (max) #endif @@ -86,6 +81,10 @@ #define SAFETY_INTERLOCK_ENABLE 1 // {saf: 0=off, 1=on #endif +#ifndef SPINDLE_MODE +#define SPINDLE_MODE 1 // {spmo; 0=diabled, 1=plan to stop, 2=continuous +#endif + #ifndef SPINDLE_ENABLE_POLARITY #define SPINDLE_ENABLE_POLARITY SPINDLE_ACTIVE_HIGH // {spep: 0=active low, 1=active high #endif @@ -95,11 +94,23 @@ #endif #ifndef SPINDLE_PAUSE_ON_HOLD -#define SPINDLE_PAUSE_ON_HOLD true // {spph: +#define SPINDLE_PAUSE_ON_HOLD false // {spph: #endif -#ifndef SPINDLE_DWELL_TIME -#define SPINDLE_DWELL_TIME 1.0 // {spdw: +#ifndef SPINDLE_SPINUP_DELAY +#define SPINDLE_SPINUP_DELAY 0 // {spde: +#endif + +#ifndef SPINDLE_DWELL_MAX +#define SPINDLE_DWELL_MAX 10000000.0 // maximum allowable dwell time. May be overridden in settings files +#endif + +#ifndef SPINDLE_SPEED_MIN +#define SPINDLE_SPEED_MIN 0.0 // {spsn: +#endif + +#ifndef SPINDLE_SPEED_MAX +#define SPINDLE_SPEED_MAX 1000000.0 // {spsm: #endif #ifndef COOLANT_MIST_POLARITY @@ -114,15 +125,14 @@ #define COOLANT_PAUSE_ON_HOLD true // {coph: #endif -#ifndef PROBE_REPORT_ENABLE -#define PROBE_REPORT_ENABLE true // {prbr: +#ifndef FEEDHOLD_Z_LIFT +#define FEEDHOLD_Z_LIFT 0 // {zl: mm to lift Z on feedhold +#endif + +#ifndef PROBE_REPORT_ENABLE +#define PROBE_REPORT_ENABLE true // {prbr: #endif -/* - * The following is to fix an issue where feedrate override was being defined in some users - * settings files but not others. This would otherwise cause an undefined compile error. - * -*/ #ifndef MANUAL_FEEDRATE_OVERRIDE_ENABLE #define MANUAL_FEEDRATE_OVERRIDE_ENABLE false #endif @@ -133,16 +143,20 @@ // *** Communications and Reporting Settings *** // -#ifndef TEXT_VERBOSITY -#define TEXT_VERBOSITY TV_VERBOSE // {tv: TV_SILENT, TV_VERBOSE +#ifndef USB_SERIAL_PORTS_EXPOSED +#define USB_SERIAL_PORTS_EXPOSED 1 // Valid options are 1 or 2, only! +#endif + +#ifndef XIO_ENABLE_FLOW_CONTROL +#define XIO_ENABLE_FLOW_CONTROL FLOW_CONTROL_RTS // {ex: FLOW_CONTROL_OFF, FLOW_CONTROL_XON, FLOW_CONTROL_RTS #endif #ifndef COMM_MODE #define COMM_MODE JSON_MODE // {ej: TEXT_MODE, JSON_MODE #endif -#ifndef XIO_ENABLE_FLOW_CONTROL -#define XIO_ENABLE_FLOW_CONTROL FLOW_CONTROL_RTS // FLOW_CONTROL_OFF, FLOW_CONTROL_XON, FLOW_CONTROL_RTS +#ifndef TEXT_VERBOSITY +#define TEXT_VERBOSITY TV_VERBOSE // {tv: TV_SILENT, TV_VERBOSE #endif #ifndef XIO_UART_MUTES_WHEN_USB_CONNECTED @@ -162,7 +176,7 @@ #endif #ifndef STATUS_REPORT_MIN_MS -#define STATUS_REPORT_MIN_MS 200 // (no JSON) milliseconds - enforces a viable minimum +#define STATUS_REPORT_MIN_MS 100 // (no JSON) milliseconds - enforces a viable minimum #endif #ifndef STATUS_REPORT_INTERVAL_MS @@ -175,7 +189,6 @@ //#define STATUS_REPORT_DEFAULTS "line","vel","mpox","mpoy","mpoz","mpoa","coor","ofsa","ofsx","ofsy","ofsz","dist","unit","stat","homz","homy","homx","momo" #endif - #ifndef MARLIN_COMPAT_ENABLED #define MARLIN_COMPAT_ENABLED false // boolean, either true or false #endif @@ -219,7 +232,7 @@ // MOTOR 1 #ifndef M1_MOTOR_MAP -#define M1_MOTOR_MAP AXIS_X // {1ma: AXIS_X, AXIS_Y... +#define M1_MOTOR_MAP AXIS_X_EXTERNAL // {1ma: AXIS_X, AXIS_Y... #endif #ifndef M1_STEP_ANGLE #define M1_STEP_ANGLE 1.8 // {1sa: degrees per step @@ -236,6 +249,12 @@ #ifndef M1_POLARITY #define M1_POLARITY 0 // {1po: 0=normal direction, 1=inverted direction #endif +#ifndef M1_ENABLE_POLARITY +#define M1_ENABLE_POLARITY IO_ACTIVE_LOW // {1ep: IO_ACTIVE_LOW or IO_ACTIVE_HIGH +#endif +#ifndef M1_STEP_POLARITY +#define M1_STEP_POLARITY IO_ACTIVE_HIGH // {1ps: IO_ACTIVE_LOW or IO_ACTIVE_HIGH +#endif #ifndef M1_POWER_MODE #define M1_POWER_MODE MOTOR_DISABLED // {1pm: MOTOR_DISABLED, MOTOR_ALWAYS_POWERED, MOTOR_POWERED_IN_CYCLE, MOTOR_POWERED_ONLY_WHEN_MOVING #endif @@ -245,7 +264,7 @@ // MOTOR 2 #ifndef M2_MOTOR_MAP -#define M2_MOTOR_MAP AXIS_Y +#define M2_MOTOR_MAP AXIS_Y_EXTERNAL #endif #ifndef M2_STEP_ANGLE #define M2_STEP_ANGLE 1.8 @@ -262,6 +281,12 @@ #ifndef M2_POLARITY #define M2_POLARITY 0 #endif +#ifndef M2_ENABLE_POLARITY +#define M2_ENABLE_POLARITY IO_ACTIVE_LOW +#endif +#ifndef M2_STEP_POLARITY +#define M2_STEP_POLARITY IO_ACTIVE_HIGH +#endif #ifndef M2_POWER_MODE #define M2_POWER_MODE MOTOR_DISABLED #endif @@ -271,7 +296,7 @@ // MOTOR 3 #ifndef M3_MOTOR_MAP -#define M3_MOTOR_MAP AXIS_Z +#define M3_MOTOR_MAP AXIS_Z_EXTERNAL #endif #ifndef M3_STEP_ANGLE #define M3_STEP_ANGLE 1.8 @@ -286,7 +311,13 @@ #define M3_STEPS_PER_UNIT 0 #endif #ifndef M3_POLARITY -#define M3_POLARITY 1 +#define M3_POLARITY 0 +#endif +#ifndef M3_ENABLE_POLARITY +#define M3_ENABLE_POLARITY IO_ACTIVE_LOW +#endif +#ifndef M3_STEP_POLARITY +#define M3_STEP_POLARITY IO_ACTIVE_HIGH #endif #ifndef M3_POWER_MODE #define M3_POWER_MODE MOTOR_DISABLED @@ -297,7 +328,7 @@ // MOTOR 4 #ifndef M4_MOTOR_MAP -#define M4_MOTOR_MAP AXIS_A +#define M4_MOTOR_MAP AXIS_A_EXTERNAL #endif #ifndef M4_STEP_ANGLE #define M4_STEP_ANGLE 1.8 @@ -314,6 +345,12 @@ #ifndef M4_POLARITY #define M4_POLARITY 0 #endif +#ifndef M4_ENABLE_POLARITY +#define M4_ENABLE_POLARITY IO_ACTIVE_LOW +#endif +#ifndef M4_STEP_POLARITY +#define M4_STEP_POLARITY IO_ACTIVE_HIGH +#endif #ifndef M4_POWER_MODE #define M4_POWER_MODE MOTOR_DISABLED #endif @@ -323,7 +360,7 @@ // MOTOR 5 #ifndef M5_MOTOR_MAP -#define M5_MOTOR_MAP AXIS_B +#define M5_MOTOR_MAP AXIS_B_EXTERNAL #endif #ifndef M5_STEP_ANGLE #define M5_STEP_ANGLE 1.8 @@ -340,6 +377,12 @@ #ifndef M5_POLARITY #define M5_POLARITY 0 #endif +#ifndef M5_ENABLE_POLARITY +#define M5_ENABLE_POLARITY IO_ACTIVE_LOW +#endif +#ifndef M5_STEP_POLARITY +#define M5_STEP_POLARITY IO_ACTIVE_HIGH +#endif #ifndef M5_POWER_MODE #define M5_POWER_MODE MOTOR_DISABLED #endif @@ -349,7 +392,7 @@ // MOTOR 6 #ifndef M6_MOTOR_MAP -#define M6_MOTOR_MAP AXIS_C +#define M6_MOTOR_MAP AXIS_C_EXTERNAL #endif #ifndef M6_STEP_ANGLE #define M6_STEP_ANGLE 1.8 @@ -366,6 +409,12 @@ #ifndef M6_POLARITY #define M6_POLARITY 0 #endif +#ifndef M6_ENABLE_POLARITY +#define M6_ENABLE_POLARITY IO_ACTIVE_LOW +#endif +#ifndef M6_STEP_POLARITY +#define M6_STEP_POLARITY IO_ACTIVE_HIGH +#endif #ifndef M6_POWER_MODE #define M6_POWER_MODE MOTOR_DISABLED #endif @@ -743,6 +792,129 @@ #define Z_ZERO_BACKOFF 2.0 #endif +// U AXIS +#ifndef U_AXIS_MODE +#define U_AXIS_MODE AXIS_DISABLED // {xam: see canonical_machine.h cmAxisMode for valid values +#endif +#ifndef U_VELOCITY_MAX +#define U_VELOCITY_MAX 1000.0 // {xvm: G0 max velocity in mm/min +#endif +#ifndef U_FEEDRATE_MAX +#define U_FEEDRATE_MAX 1000.0 // {xfr: G1 max feed rate in mm/min +#endif +#ifndef U_TRAVEL_MIN +#define U_TRAVEL_MIN 0.0 // {xtn: minimum travel for soft limits +#endif +#ifndef U_TRAVEL_MAX +#define U_TRAVEL_MAX 0.0 // {xtm: travel between switches or crashes +#endif +#ifndef U_JERK_MAX +#define U_JERK_MAX 1000.0 // {xjm: +#endif +#ifndef U_JERK_HIGH_SPEED +#define U_JERK_HIGH_SPEED 1000.0 // {xjh: +#endif +#ifndef U_HOMING_INPUT +#define U_HOMING_INPUT 0 // {xhi: input used for homing or 0 to disable +#endif +#ifndef U_HOMING_DIRECTION +#define U_HOMING_DIRECTION 0 // {xhd: 0=search moves negative, 1= search moves positive +#endif +#ifndef U_SEARCH_VELOCITY +#define U_SEARCH_VELOCITY 500.0 // {xsv: minus means move to minimum switch +#endif +#ifndef U_LATCH_VELOCITY +#define U_LATCH_VELOCITY 100.0 // {xlv: mm/min +#endif +#ifndef U_LATCH_BACKOFF +#define U_LATCH_BACKOFF 4.0 // {xlb: mm +#endif +#ifndef U_ZERO_BACKOFF +#define U_ZERO_BACKOFF 2.0 // {xzb: mm +#endif + +// V AXIS +#ifndef V_AXIS_MODE +#define V_AXIS_MODE AXIS_DISABLED +#endif +#ifndef V_VELOCITY_MAX +#define V_VELOCITY_MAX 1000.0 +#endif +#ifndef V_FEEDRATE_MAX +#define V_FEEDRATE_MAX 1000.0 +#endif +#ifndef V_TRAVEL_MIN +#define V_TRAVEL_MIN 0.0 +#endif +#ifndef V_TRAVEL_MAX +#define V_TRAVEL_MAX 0.0 +#endif +#ifndef V_JERK_MAX +#define V_JERK_MAX 1000.0 +#endif +#ifndef V_JERK_HIGH_SPEED +#define V_JERK_HIGH_SPEED 1000.0 +#endif +#ifndef V_HOMING_INPUT +#define V_HOMING_INPUT 0 +#endif +#ifndef V_HOMING_DIRECTION +#define V_HOMING_DIRECTION 0 +#endif +#ifndef V_SEARCH_VELOCITY +#define V_SEARCH_VELOCITY 500.0 +#endif +#ifndef V_LATCH_VELOCITY +#define V_LATCH_VELOCITY 100.0 +#endif +#ifndef V_LATCH_BACKOFF +#define V_LATCH_BACKOFF 4.0 +#endif +#ifndef V_ZERO_BACKOFF +#define V_ZERO_BACKOFF 2.0 +#endif + +// W AXIS +#ifndef W_AXIS_MODE +#define W_AXIS_MODE AXIS_DISABLED +#endif +#ifndef W_VELOCITY_MAX +#define W_VELOCITY_MAX 1000.0 +#endif +#ifndef W_FEEDRATE_MAX +#define W_FEEDRATE_MAX 1000.0 +#endif +#ifndef W_TRAVEL_MAX +#define W_TRAVEL_MAX 0.0 +#endif +#ifndef W_TRAVEL_MIN +#define W_TRAVEL_MIN 0.0 +#endif +#ifndef W_JERK_MAX +#define W_JERK_MAX 500.0 +#endif +#ifndef W_JERK_HIGH_SPEED +#define W_JERK_HIGH_SPEED 500.0 +#endif +#ifndef W_HOMING_INPUT +#define W_HOMING_INPUT 0 +#endif +#ifndef W_HOMING_DIRECTION +#define W_HOMING_DIRECTION 0 +#endif +#ifndef W_SEARCH_VELOCITY +#define W_SEARCH_VELOCITY 250.0 +#endif +#ifndef W_LATCH_VELOCITY +#define W_LATCH_VELOCITY 25.0 +#endif +#ifndef W_LATCH_BACKOFF +#define W_LATCH_BACKOFF 4.0 +#endif +#ifndef W_ZERO_BACKOFF +#define W_ZERO_BACKOFF 2.0 +#endif + /*************************************************************************************** * Rotary values can be chosen to make the motor react the same as X for testing * To calculate the speeds here, in Wolfram Alpha-speak: @@ -759,7 +931,7 @@ * Note that you need floating point values to always have a .0 at the end! #define A_AXIS_MODE AXIS_RADIUS -#define A_RADIUS (M1_TRAVEL_PER_REV/(2*3.14159628)) +#define A_RADIUS (M4_TRAVEL_PER_REV/(2*3.14159628)) #define A_VELOCITY_MAX ((X_VELOCITY_MAX/M1_TRAVEL_PER_REV)*360) // set to the same speed as X axis #define A_FEEDRATE_MAX A_VELOCITY_MAX #define A_TRAVEL_MIN -1.0 // min/max the same means infinite, no limit @@ -780,7 +952,7 @@ #define A_AXIS_MODE AXIS_DISABLED #endif #ifndef A_RADIUS -#define A_RADIUS (M1_TRAVEL_PER_REV/(2*3.14159628)) +#define A_RADIUS (M4_TRAVEL_PER_REV/(2*3.14159628)) #endif #ifndef A_VELOCITY_MAX #define A_VELOCITY_MAX ((X_VELOCITY_MAX/M1_TRAVEL_PER_REV)*360) // set to the same speed as X axis @@ -824,7 +996,7 @@ #define B_AXIS_MODE AXIS_DISABLED #endif #ifndef B_RADIUS -#define B_RADIUS (M1_TRAVEL_PER_REV/(2*3.14159628)) +#define B_RADIUS (M5_TRAVEL_PER_REV/(2*3.14159628)) #endif #ifndef B_VELOCITY_MAX #define B_VELOCITY_MAX ((X_VELOCITY_MAX/M1_TRAVEL_PER_REV)*360) @@ -851,10 +1023,10 @@ #define B_HOMING_DIRECTION 0 #endif #ifndef B_SEARCH_VELOCITY -#define B_SEARCH_VELOCITY (A_VELOCITY_MAX * 0.500) +#define B_SEARCH_VELOCITY (B_VELOCITY_MAX * 0.500) #endif #ifndef B_LATCH_VELOCITY -#define B_LATCH_VELOCITY (A_VELOCITY_MAX * 0.100) +#define B_LATCH_VELOCITY (B_VELOCITY_MAX * 0.100) #endif #ifndef B_LATCH_BACKOFF #define B_LATCH_BACKOFF 5.0 @@ -868,7 +1040,7 @@ #define C_AXIS_MODE AXIS_DISABLED #endif #ifndef C_RADIUS -#define C_RADIUS (M1_TRAVEL_PER_REV/(2*3.14159628)) +#define C_RADIUS (M6_TRAVEL_PER_REV/(2*3.14159628)) #endif #ifndef C_VELOCITY_MAX #define C_VELOCITY_MAX ((X_VELOCITY_MAX/M1_TRAVEL_PER_REV)*360) @@ -895,10 +1067,10 @@ #define C_HOMING_DIRECTION 0 #endif #ifndef C_SEARCH_VELOCITY -#define C_SEARCH_VELOCITY (A_VELOCITY_MAX * 0.500) +#define C_SEARCH_VELOCITY (C_VELOCITY_MAX * 0.500) #endif #ifndef C_LATCH_VELOCITY -#define C_LATCH_VELOCITY (A_VELOCITY_MAX * 0.100) +#define C_LATCH_VELOCITY (C_VELOCITY_MAX * 0.100) #endif #ifndef C_LATCH_BACKOFF #define C_LATCH_BACKOFF 5.0 @@ -1351,16 +1523,16 @@ #define P1_CW_PHASE_HI 0.17 #endif #ifndef P1_CCW_SPEED_LO -#define P1_CCW_SPEED_LO 0.0 +#define P1_CCW_SPEED_LO 7900 // 0.0 #endif #ifndef P1_CCW_SPEED_HI -#define P1_CCW_SPEED_HI 0.0 +#define P1_CCW_SPEED_HI 12800 // 0.0 #endif #ifndef P1_CCW_PHASE_LO -#define P1_CCW_PHASE_LO 0.1 +#define P1_CCW_PHASE_LO 0.13 // 0.1 #endif #ifndef P1_CCW_PHASE_HI -#define P1_CCW_PHASE_HI 0.1 +#define P1_CCW_PHASE_HI 0.17 // 0.1 #endif #ifndef P1_PWM_PHASE_OFF #define P1_PWM_PHASE_OFF 0.1 @@ -1440,6 +1612,15 @@ #ifndef G54_Z_OFFSET #define G54_Z_OFFSET 0 #endif +#ifndef G54_U_OFFSET +#define G54_U_OFFSET 0 +#endif +#ifndef G54_V_OFFSET +#define G54_V_OFFSET 0 +#endif +#ifndef G54_W_OFFSET +#define G54_W_OFFSET 0 +#endif #ifndef G54_A_OFFSET #define G54_A_OFFSET 0 #endif @@ -1459,6 +1640,15 @@ #ifndef G55_Z_OFFSET #define G55_Z_OFFSET 0 #endif +#ifndef G55_U_OFFSET +#define G55_U_OFFSET 0 +#endif +#ifndef G55_V_OFFSET +#define G55_V_OFFSET 0 +#endif +#ifndef G55_W_OFFSET +#define G55_W_OFFSET 0 +#endif #ifndef G55_A_OFFSET #define G55_A_OFFSET 0 #endif @@ -1478,6 +1668,15 @@ #ifndef G56_Z_OFFSET #define G56_Z_OFFSET 0 #endif +#ifndef G56_U_OFFSET +#define G56_U_OFFSET 0 +#endif +#ifndef G56_V_OFFSET +#define G56_V_OFFSET 0 +#endif +#ifndef G56_W_OFFSET +#define G56_W_OFFSET 0 +#endif #ifndef G56_A_OFFSET #define G56_A_OFFSET 0 #endif @@ -1497,6 +1696,15 @@ #ifndef G57_Z_OFFSET #define G57_Z_OFFSET 0 #endif +#ifndef G57_U_OFFSET +#define G57_U_OFFSET 0 +#endif +#ifndef G57_V_OFFSET +#define G57_V_OFFSET 0 +#endif +#ifndef G57_W_OFFSET +#define G57_W_OFFSET 0 +#endif #ifndef G57_A_OFFSET #define G57_A_OFFSET 0 #endif @@ -1516,6 +1724,15 @@ #ifndef G58_Z_OFFSET #define G58_Z_OFFSET 0 #endif +#ifndef G58_U_OFFSET +#define G58_U_OFFSET 0 +#endif +#ifndef G58_V_OFFSET +#define G58_V_OFFSET 0 +#endif +#ifndef G58_W_OFFSET +#define G58_W_OFFSET 0 +#endif #ifndef G58_A_OFFSET #define G58_A_OFFSET 0 #endif @@ -1535,6 +1752,15 @@ #ifndef G59_Z_OFFSET #define G59_Z_OFFSET 0 #endif +#ifndef G59_U_OFFSET +#define G59_U_OFFSET 0 +#endif +#ifndef G59_V_OFFSET +#define G59_V_OFFSET 0 +#endif +#ifndef G59_W_OFFSET +#define G59_W_OFFSET 0 +#endif #ifndef G59_A_OFFSET #define G59_A_OFFSET 0 #endif @@ -1556,6 +1782,15 @@ #ifndef TT1_Z_OFFSET #define TT1_Z_OFFSET 0 #endif +#ifndef TT1_U_OFFSET +#define TT1_U_OFFSET 0 +#endif +#ifndef TT1_V_OFFSET +#define TT1_V_OFFSET 0 +#endif +#ifndef TT1_W_OFFSET +#define TT1_W_OFFSET 0 +#endif #ifndef TT1_A_OFFSET #define TT1_A_OFFSET 0 #endif @@ -1575,6 +1810,15 @@ #ifndef TT2_Z_OFFSET #define TT2_Z_OFFSET 0 #endif +#ifndef TT2_U_OFFSET +#define TT2_U_OFFSET 0 +#endif +#ifndef TT2_V_OFFSET +#define TT2_V_OFFSET 0 +#endif +#ifndef TT2_W_OFFSET +#define TT2_W_OFFSET 0 +#endif #ifndef TT2_A_OFFSET #define TT2_A_OFFSET 0 #endif @@ -1594,6 +1838,15 @@ #ifndef TT3_Z_OFFSET #define TT3_Z_OFFSET 0 #endif +#ifndef TT3_U_OFFSET +#define TT3_U_OFFSET 0 +#endif +#ifndef TT3_V_OFFSET +#define TT3_V_OFFSET 0 +#endif +#ifndef TT3_W_OFFSET +#define TT3_W_OFFSET 0 +#endif #ifndef TT3_A_OFFSET #define TT3_A_OFFSET 0 #endif @@ -1613,6 +1866,15 @@ #ifndef TT4_Z_OFFSET #define TT4_Z_OFFSET 0 #endif +#ifndef TT4_U_OFFSET +#define TT4_U_OFFSET 0 +#endif +#ifndef TT4_V_OFFSET +#define TT4_V_OFFSET 0 +#endif +#ifndef TT4_W_OFFSET +#define TT4_W_OFFSET 0 +#endif #ifndef TT4_A_OFFSET #define TT4_A_OFFSET 0 #endif @@ -1632,6 +1894,15 @@ #ifndef TT5_Z_OFFSET #define TT5_Z_OFFSET 0 #endif +#ifndef TT5_U_OFFSET +#define TT5_U_OFFSET 0 +#endif +#ifndef TT5_V_OFFSET +#define TT5_V_OFFSET 0 +#endif +#ifndef TT5_W_OFFSET +#define TT5_W_OFFSET 0 +#endif #ifndef TT5_A_OFFSET #define TT5_A_OFFSET 0 #endif @@ -1651,6 +1922,15 @@ #ifndef TT6_Z_OFFSET #define TT6_Z_OFFSET 0 #endif +#ifndef TT6_U_OFFSET +#define TT6_U_OFFSET 0 +#endif +#ifndef TT6_V_OFFSET +#define TT6_V_OFFSET 0 +#endif +#ifndef TT6_W_OFFSET +#define TT6_W_OFFSET 0 +#endif #ifndef TT6_A_OFFSET #define TT6_A_OFFSET 0 #endif @@ -1670,6 +1950,15 @@ #ifndef TT7_Z_OFFSET #define TT7_Z_OFFSET 0 #endif +#ifndef TT7_U_OFFSET +#define TT7_U_OFFSET 0 +#endif +#ifndef TT7_V_OFFSET +#define TT7_V_OFFSET 0 +#endif +#ifndef TT7_W_OFFSET +#define TT7_W_OFFSET 0 +#endif #ifndef TT7_A_OFFSET #define TT7_A_OFFSET 0 #endif @@ -1686,6 +1975,15 @@ #ifndef TT8_Y_OFFSET #define TT8_Y_OFFSET 0 #endif +#ifndef TT8_U_OFFSET +#define TT8_U_OFFSET 0 +#endif +#ifndef TT8_V_OFFSET +#define TT8_V_OFFSET 0 +#endif +#ifndef TT8_W_OFFSET +#define TT8_W_OFFSET 0 +#endif #ifndef TT8_Z_OFFSET #define TT8_Z_OFFSET 0 #endif @@ -1708,6 +2006,15 @@ #ifndef TT9_Z_OFFSET #define TT9_Z_OFFSET 0 #endif +#ifndef TT9_U_OFFSET +#define TT9_U_OFFSET 0 +#endif +#ifndef TT9_V_OFFSET +#define TT9_V_OFFSET 0 +#endif +#ifndef TT9_W_OFFSET +#define TT9_W_OFFSET 0 +#endif #ifndef TT9_A_OFFSET #define TT9_A_OFFSET 0 #endif @@ -1727,6 +2034,15 @@ #ifndef TT10_Z_OFFSET #define TT10_Z_OFFSET 0 #endif +#ifndef TT10_U_OFFSET +#define TT10_U_OFFSET 0 +#endif +#ifndef TT10_V_OFFSET +#define TT10_V_OFFSET 0 +#endif +#ifndef TT10_W_OFFSET +#define TT10_W_OFFSET 0 +#endif #ifndef TT10_A_OFFSET #define TT10_A_OFFSET 0 #endif @@ -1746,6 +2062,15 @@ #ifndef TT11_Z_OFFSET #define TT11_Z_OFFSET 0 #endif +#ifndef TT11_U_OFFSET +#define TT11_U_OFFSET 0 +#endif +#ifndef TT11_V_OFFSET +#define TT11_V_OFFSET 0 +#endif +#ifndef TT11_W_OFFSET +#define TT11_W_OFFSET 0 +#endif #ifndef TT11_A_OFFSET #define TT11_A_OFFSET 0 #endif @@ -1765,6 +2090,15 @@ #ifndef TT12_Z_OFFSET #define TT12_Z_OFFSET 0 #endif +#ifndef TT12_U_OFFSET +#define TT12_U_OFFSET 0 +#endif +#ifndef TT12_V_OFFSET +#define TT12_V_OFFSET 0 +#endif +#ifndef TT12_W_OFFSET +#define TT12_W_OFFSET 0 +#endif #ifndef TT12_A_OFFSET #define TT12_A_OFFSET 0 #endif @@ -1784,6 +2118,15 @@ #ifndef TT13_Z_OFFSET #define TT13_Z_OFFSET 0 #endif +#ifndef TT13_U_OFFSET +#define TT13_U_OFFSET 0 +#endif +#ifndef TT13_V_OFFSET +#define TT13_V_OFFSET 0 +#endif +#ifndef TT13_W_OFFSET +#define TT13_W_OFFSET 0 +#endif #ifndef TT13_A_OFFSET #define TT13_A_OFFSET 0 #endif @@ -1803,6 +2146,15 @@ #ifndef TT14_Z_OFFSET #define TT14_Z_OFFSET 0 #endif +#ifndef TT14_U_OFFSET +#define TT14_U_OFFSET 0 +#endif +#ifndef TT14_V_OFFSET +#define TT14_V_OFFSET 0 +#endif +#ifndef TT14_W_OFFSET +#define TT14_W_OFFSET 0 +#endif #ifndef TT14_A_OFFSET #define TT14_A_OFFSET 0 #endif @@ -1822,6 +2174,15 @@ #ifndef TT15_Z_OFFSET #define TT15_Z_OFFSET 0 #endif +#ifndef TT15_U_OFFSET +#define TT15_U_OFFSET 0 +#endif +#ifndef TT15_V_OFFSET +#define TT15_V_OFFSET 0 +#endif +#ifndef TT15_W_OFFSET +#define TT15_W_OFFSET 0 +#endif #ifndef TT15_A_OFFSET #define TT15_A_OFFSET 0 #endif @@ -1841,6 +2202,15 @@ #ifndef TT16_Z_OFFSET #define TT16_Z_OFFSET 0 #endif +#ifndef TT16_U_OFFSET +#define TT16_U_OFFSET 0 +#endif +#ifndef TT16_V_OFFSET +#define TT16_V_OFFSET 0 +#endif +#ifndef TT16_W_OFFSET +#define TT16_W_OFFSET 0 +#endif #ifndef TT16_A_OFFSET #define TT16_A_OFFSET 0 #endif @@ -1860,6 +2230,15 @@ #ifndef TT17_Z_OFFSET #define TT17_Z_OFFSET 0 #endif +#ifndef TT17_U_OFFSET +#define TT17_U_OFFSET 0 +#endif +#ifndef TT17_V_OFFSET +#define TT17_V_OFFSET 0 +#endif +#ifndef TT17_W_OFFSET +#define TT17_W_OFFSET 0 +#endif #ifndef TT17_A_OFFSET #define TT17_A_OFFSET 0 #endif @@ -1879,6 +2258,15 @@ #ifndef TT18_Z_OFFSET #define TT18_Z_OFFSET 0 #endif +#ifndef TT18_U_OFFSET +#define TT18_U_OFFSET 0 +#endif +#ifndef TT18_V_OFFSET +#define TT18_V_OFFSET 0 +#endif +#ifndef TT18_W_OFFSET +#define TT18_W_OFFSET 0 +#endif #ifndef TT18_A_OFFSET #define TT18_A_OFFSET 0 #endif @@ -1898,6 +2286,15 @@ #ifndef TT19_Z_OFFSET #define TT19_Z_OFFSET 0 #endif +#ifndef TT19_U_OFFSET +#define TT19_U_OFFSET 0 +#endif +#ifndef TT19_V_OFFSET +#define TT19_V_OFFSET 0 +#endif +#ifndef TT19_W_OFFSET +#define TT19_W_OFFSET 0 +#endif #ifndef TT19_A_OFFSET #define TT19_A_OFFSET 0 #endif @@ -1917,6 +2314,15 @@ #ifndef TT20_Z_OFFSET #define TT20_Z_OFFSET 0 #endif +#ifndef TT20_U_OFFSET +#define TT20_U_OFFSET 0 +#endif +#ifndef TT20_V_OFFSET +#define TT20_V_OFFSET 0 +#endif +#ifndef TT20_W_OFFSET +#define TT20_W_OFFSET 0 +#endif #ifndef TT20_A_OFFSET #define TT20_A_OFFSET 0 #endif @@ -1936,6 +2342,15 @@ #ifndef TT21_Z_OFFSET #define TT21_Z_OFFSET 0 #endif +#ifndef TT21_U_OFFSET +#define TT21_U_OFFSET 0 +#endif +#ifndef TT21_V_OFFSET +#define TT21_V_OFFSET 0 +#endif +#ifndef TT21_W_OFFSET +#define TT21_W_OFFSET 0 +#endif #ifndef TT21_A_OFFSET #define TT21_A_OFFSET 0 #endif @@ -1955,6 +2370,15 @@ #ifndef TT22_Z_OFFSET #define TT22_Z_OFFSET 0 #endif +#ifndef TT22_U_OFFSET +#define TT22_U_OFFSET 0 +#endif +#ifndef TT22_V_OFFSET +#define TT22_V_OFFSET 0 +#endif +#ifndef TT22_W_OFFSET +#define TT22_W_OFFSET 0 +#endif #ifndef TT22_A_OFFSET #define TT22_A_OFFSET 0 #endif @@ -1974,6 +2398,15 @@ #ifndef TT23_Z_OFFSET #define TT23_Z_OFFSET 0 #endif +#ifndef TT23_U_OFFSET +#define TT23_U_OFFSET 0 +#endif +#ifndef TT23_V_OFFSET +#define TT23_V_OFFSET 0 +#endif +#ifndef TT23_W_OFFSET +#define TT23_W_OFFSET 0 +#endif #ifndef TT23_A_OFFSET #define TT23_A_OFFSET 0 #endif @@ -1993,6 +2426,15 @@ #ifndef TT24_Z_OFFSET #define TT24_Z_OFFSET 0 #endif +#ifndef TT24_U_OFFSET +#define TT24_U_OFFSET 0 +#endif +#ifndef TT24_V_OFFSET +#define TT24_V_OFFSET 0 +#endif +#ifndef TT24_W_OFFSET +#define TT24_W_OFFSET 0 +#endif #ifndef TT24_A_OFFSET #define TT24_A_OFFSET 0 #endif @@ -2012,6 +2454,15 @@ #ifndef TT25_Z_OFFSET #define TT25_Z_OFFSET 0 #endif +#ifndef TT25_U_OFFSET +#define TT25_U_OFFSET 0 +#endif +#ifndef TT25_V_OFFSET +#define TT25_V_OFFSET 0 +#endif +#ifndef TT25_W_OFFSET +#define TT25_W_OFFSET 0 +#endif #ifndef TT25_A_OFFSET #define TT25_A_OFFSET 0 #endif @@ -2031,6 +2482,15 @@ #ifndef TT26_Z_OFFSET #define TT26_Z_OFFSET 0 #endif +#ifndef TT26_U_OFFSET +#define TT26_U_OFFSET 0 +#endif +#ifndef TT26_V_OFFSET +#define TT26_V_OFFSET 0 +#endif +#ifndef TT26_W_OFFSET +#define TT26_W_OFFSET 0 +#endif #ifndef TT26_A_OFFSET #define TT26_A_OFFSET 0 #endif @@ -2050,6 +2510,15 @@ #ifndef TT27_Z_OFFSET #define TT27_Z_OFFSET 0 #endif +#ifndef TT27_U_OFFSET +#define TT27_U_OFFSET 0 +#endif +#ifndef TT27_V_OFFSET +#define TT27_V_OFFSET 0 +#endif +#ifndef TT27_W_OFFSET +#define TT27_W_OFFSET 0 +#endif #ifndef TT27_A_OFFSET #define TT27_A_OFFSET 0 #endif @@ -2069,6 +2538,15 @@ #ifndef TT28_Z_OFFSET #define TT28_Z_OFFSET 0 #endif +#ifndef TT28_U_OFFSET +#define TT28_U_OFFSET 0 +#endif +#ifndef TT28_V_OFFSET +#define TT28_V_OFFSET 0 +#endif +#ifndef TT28_W_OFFSET +#define TT28_W_OFFSET 0 +#endif #ifndef TT28_A_OFFSET #define TT28_A_OFFSET 0 #endif @@ -2088,6 +2566,15 @@ #ifndef TT29_Z_OFFSET #define TT29_Z_OFFSET 0 #endif +#ifndef TT29_U_OFFSET +#define TT29_U_OFFSET 0 +#endif +#ifndef TT29_V_OFFSET +#define TT29_V_OFFSET 0 +#endif +#ifndef TT29_W_OFFSET +#define TT29_W_OFFSET 0 +#endif #ifndef TT29_A_OFFSET #define TT29_A_OFFSET 0 #endif @@ -2107,6 +2594,15 @@ #ifndef TT30_Z_OFFSET #define TT30_Z_OFFSET 0 #endif +#ifndef TT30_U_OFFSET +#define TT30_U_OFFSET 0 +#endif +#ifndef TT30_V_OFFSET +#define TT30_V_OFFSET 0 +#endif +#ifndef TT30_W_OFFSET +#define TT30_W_OFFSET 0 +#endif #ifndef TT30_A_OFFSET #define TT30_A_OFFSET 0 #endif @@ -2126,6 +2622,15 @@ #ifndef TT31_Z_OFFSET #define TT31_Z_OFFSET 0 #endif +#ifndef TT31_U_OFFSET +#define TT31_U_OFFSET 0 +#endif +#ifndef TT31_V_OFFSET +#define TT31_V_OFFSET 0 +#endif +#ifndef TT31_W_OFFSET +#define TT31_W_OFFSET 0 +#endif #ifndef TT31_A_OFFSET #define TT31_A_OFFSET 0 #endif @@ -2145,6 +2650,15 @@ #ifndef TT32_Z_OFFSET #define TT32_Z_OFFSET 0 #endif +#ifndef TT32_U_OFFSET +#define TT32_U_OFFSET 0 +#endif +#ifndef TT32_V_OFFSET +#define TT32_V_OFFSET 0 +#endif +#ifndef TT32_W_OFFSET +#define TT32_W_OFFSET 0 +#endif #ifndef TT32_A_OFFSET #define TT32_A_OFFSET 0 #endif diff --git a/g2core/settings/settings_makeblock.h b/g2core/settings/settings_makeblock.h index 42c42807..1c0e4f04 100644 --- a/g2core/settings/settings_makeblock.h +++ b/g2core/settings/settings_makeblock.h @@ -2,7 +2,7 @@ * settings_makeblock.h - makeblock engraving table * This file is part of the g2core project * - * Copyright (c) 2016 - 2017 Alden S. Hart, Jr. + * Copyright (c) 2016 - 2018 Alden S. Hart, Jr. * * This file ("the software") is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License, version 2 as published by the @@ -34,44 +34,48 @@ // Machine configuration settings -#define JUNCTION_INTEGRATION_TIME 1.25 // cornering - between 0.10 and 2.00 (higher is faster) -#define CHORDAL_TOLERANCE 0.01 // chordal tolerance for arcs (in mm) +#define JUNCTION_INTEGRATION_TIME 1.25 // cornering - between 0.10 and 2.00 (higher is faster) +#define CHORDAL_TOLERANCE 0.1 // chordal tolerance for arcs (in mm) -#define SOFT_LIMIT_ENABLE 0 // 0=off, 1=on -#define HARD_LIMIT_ENABLE 1 // 0=off, 1=on -#define SAFETY_INTERLOCK_ENABLE 0 // 0=off, 1=on +#define SOFT_LIMIT_ENABLE 0 // 0=off, 1=on +#define HARD_LIMIT_ENABLE 0 // 0=off, 1=on +#define SAFETY_INTERLOCK_ENABLE 1 // 0=off, 1=on -#define SPINDLE_ENABLE_POLARITY 1 // 0=active low, 1=active high -#define SPINDLE_DIR_POLARITY 0 // 0=clockwise is low, 1=clockwise is high +#define SPINDLE_ENABLE_POLARITY 1 // 0=active low, 1=active high +#define SPINDLE_DIR_POLARITY 0 // 0=clockwise is low, 1=clockwise is high #define SPINDLE_PAUSE_ON_HOLD true -#define SPINDLE_DWELL_TIME 1.0 +#define SPINDLE_SPINUP_DELAY 2.0 +#define SPINDLE_SPEED_MIN 0.0 +#define SPINDLE_SPEED_MAX 20000.0 -#define COOLANT_MIST_POLARITY 1 // 0=active low, 1=active high -#define COOLANT_FLOOD_POLARITY 1 // 0=active low, 1=active high +#define COOLANT_MIST_POLARITY 1 // 0=active low, 1=active high +#define COOLANT_FLOOD_POLARITY 1 // 0=active low, 1=active high #define COOLANT_PAUSE_ON_HOLD true +#define FEEDHOLD_Z_LIFT 10 // mm to lift Z on feedhold #define PROBE_REPORT_ENABLE true // Communications and reporting settings -#define MARLIN_COMPAT_ENABLED false // enable marlin compatibility mode -#define COMM_MODE JSON_MODE // one of: TEXT_MODE, JSON_MODE -#define XIO_ENABLE_FLOW_CONTROL FLOW_CONTROL_RTS // FLOW_CONTROL_OFF, FLOW_CONTROL_RTS -#define USB_SERIAL_PORTS_EXPOSED 1 +#define USB_SERIAL_PORTS_EXPOSED 1 // Valid options are 1 or 2, only! +#define MARLIN_COMPAT_ENABLED false // enable marlin compatibility mode +#define COMM_MODE JSON_MODE // one of: TEXT_MODE, JSON_MODE -#define TEXT_VERBOSITY TV_VERBOSE // one of: TV_SILENT, TV_VERBOSE -#define JSON_VERBOSITY JV_MESSAGES // one of: JV_SILENT, JV_FOOTER, JV_CONFIGS, JV_MESSAGES, JV_LINENUM, JV_VERBOSE -#define QUEUE_REPORT_VERBOSITY QR_OFF // one of: QR_OFF, QR_SINGLE, QR_TRIPLE +#define TEXT_VERBOSITY TV_VERBOSE // one of: TV_SILENT, TV_VERBOSE +#define JSON_VERBOSITY JV_MESSAGES // one of: JV_SILENT, JV_FOOTER, JV_CONFIGS, JV_MESSAGES, JV_LINENUM, JV_VERBOSE +#define QUEUE_REPORT_VERBOSITY QR_OFF // one of: QR_OFF, QR_SINGLE, QR_TRIPLE -#define STATUS_REPORT_VERBOSITY SR_FILTERED // one of: SR_OFF, SR_FILTERED, SR_VERBOSE +#define STATUS_REPORT_VERBOSITY SR_FILTERED // one of: SR_OFF, SR_FILTERED, SR_VERBOSE +#define STATUS_REPORT_MIN_MS 200 // milliseconds - enforces a viable minimum +#define STATUS_REPORT_INTERVAL_MS 250 // milliseconds - set $SV=0 to disable -#define STATUS_REPORT_MIN_MS 200 // milliseconds - enforces a viable minimum -#define STATUS_REPORT_INTERVAL_MS 250 // milliseconds - set $SV=0 to disable - -#define STATUS_REPORT_DEFAULTS "line","stat","posx","posy","posz",\ +#define STATUS_REPORT_DEFAULTS "line","stat",\ + "posx","posy","posz",\ + "posu","posv","posw",\ "vel", "unit","feed","coor","momo",\ "plan","path","dist","prbe","prbz",\ "mpox","mpoy","mpoz",\ + "mpou","mpov","mpow",\ "admo","frmo","cycs","hold" // "_ts1","_cs1","_es1","_xs1","_fe1" @@ -84,49 +88,49 @@ // *** motor settings ************************************************************************************ +#define MOTOR_POWER_LEVEL 0.2 // power level: 0.0=no power, 1.0=max power #define MOTOR_POWER_MODE MOTOR_POWERED_IN_CYCLE // default motor power mode (see stPowerMode in stepper.h) // 0=MOTOR_DISABLED, // 1=MOTOR_ALWAYS_POWERED, // 2=MOTOR_POWERED_IN_CYCLE, // 3=MOTOR_POWERED_ONLY_WHEN_MOVING -#define M1_POWER_LEVEL 0.4 // 1pl: 0.0=no power, 1.0=max power -#define MOTOR_POWER_TIMEOUT 10.00 // motor power timeout in seconds +#define MOTOR_POWER_TIMEOUT 5.00 // motor power timeout in seconds -#define M1_MOTOR_MAP AXIS_X // 1ma +#define M1_MOTOR_MAP AXIS_X_EXTERNAL // 1ma #define M1_STEP_ANGLE 1.8 // 1sa #define M1_TRAVEL_PER_REV 36.576 // 1tr 2.032mm pitch * 18 teeth per revolution #define M1_MICROSTEPS 8 // 1mi 1,2,4,8,16,32 #define M1_POLARITY 0 // 1po 0=normal, 1=reversed #define M1_POWER_MODE MOTOR_POWER_MODE // 1pm 0=MOTOR_DISABLED, 1=MOTOR_ALWAYS_POWERED, 2=MOTOR_POWERED_IN_CYCLE, 3=MOTOR_POWERED_ONLY_WHEN_MOVING -#define M1_POWER_LEVEL 0.4 +#define M1_POWER_LEVEL MOTOR_POWER_LEVEL // 1pl: 0.0=no power, 1.0=max power -#define M2_MOTOR_MAP AXIS_Y +#define M2_MOTOR_MAP AXIS_Y_EXTERNAL #define M2_STEP_ANGLE 1.8 #define M2_TRAVEL_PER_REV 36.576 #define M2_MICROSTEPS 8 #define M2_POLARITY 0 #define M2_POWER_MODE MOTOR_POWER_MODE -#define M2_POWER_LEVEL 0.4 +#define M2_POWER_LEVEL MOTOR_POWER_LEVEL -#define M3_MOTOR_MAP AXIS_Z // Imaginary Z axis. For testing +#define M3_MOTOR_MAP AXIS_Z_EXTERNAL // Imaginary Z axis. For testing #define M3_STEP_ANGLE 1.8 #define M3_TRAVEL_PER_REV 1.25 #define M3_MICROSTEPS 8 #define M3_POLARITY 0 #define M3_POWER_MODE MOTOR_POWER_MODE -#define M3_POWER_LEVEL 0.4 +#define M3_POWER_LEVEL MOTOR_POWER_LEVEL -#define M4_MOTOR_MAP AXIS_A +#define M4_MOTOR_MAP AXIS_W_EXTERNAL // Imaginary W axis. For testing #define M4_STEP_ANGLE 1.8 #define M4_TRAVEL_PER_REV 1.25 #define M4_MICROSTEPS 8 -#define M4_POLARITY 0 +#define M4_POLARITY 1 #define M4_POWER_MODE MOTOR_POWER_MODE -#define M4_POWER_LEVEL 0.4 +#define M4_POWER_LEVEL MOTOR_POWER_LEVEL // *** axis settings ********************************************************************************** -#define JERK_MAX 5000 +#define JERK_MAX 20000 #define X_AXIS_MODE AXIS_STANDARD // xam see canonical_machine.h cmAxisMode for valid values #define X_VELOCITY_MAX 40000 // xvm G0 max velocity in mm/min @@ -170,6 +174,48 @@ #define Z_LATCH_BACKOFF 4 #define Z_ZERO_BACKOFF 2 +#define U_AXIS_MODE AXIS_STANDARD // xam see canonical_machine.h cmAxisMode for valid values +#define U_VELOCITY_MAX 40000 // xvm G0 max velocity in mm/min +#define U_FEEDRATE_MAX U_VELOCITY_MAX // xfr G1 max feed rate in mm/min +#define U_TRAVEL_MIN 0 // xtn minimum travel for soft limits +#define U_TRAVEL_MAX 420 // xtm travel between switches or crashes +#define U_JERK_MAX JERK_MAX // xjm jerk * 1,000,000 +#define U_JERK_HIGH_SPEED 20000 // xjh +#define U_HOMING_INPUT 1 // xhi input used for homing or 0 to disable +#define U_HOMING_DIRECTION 0 // xhd 0=search moves negative, 1= search moves positive +#define U_SEARCH_VELOCITY 3000 // xsv minus means move to minimum switch +#define U_LATCH_VELOCITY 100 // xlv mm/min +#define U_LATCH_BACKOFF 4 // xlb mm +#define U_ZERO_BACKOFF 2 // xzb mm + +#define V_AXIS_MODE AXIS_STANDARD +#define V_VELOCITY_MAX 40000 +#define V_FEEDRATE_MAX V_VELOCITY_MAX +#define V_TRAVEL_MIN 0 +#define V_TRAVEL_MAX 420 +#define V_JERK_MAX JERK_MAX +#define V_JERK_HIGH_SPEED 20000 +#define V_HOMING_INPUT 3 +#define V_HOMING_DIRECTION 0 +#define V_SEARCH_VELOCITY 3000 +#define V_LATCH_VELOCITY 100 +#define V_LATCH_BACKOFF 4 +#define V_ZERO_BACKOFF 2 + +#define W_AXIS_MODE AXIS_STANDARD +#define W_VELOCITY_MAX 1200 +#define W_FEEDRATE_MAX W_VELOCITY_MAX +#define W_TRAVEL_MAX 0 +#define W_TRAVEL_MIN -95 +#define W_JERK_MAX 500 +#define W_JERK_HIGH_SPEED 1000 +#define W_HOMING_INPUT 6 +#define W_HOMING_DIRECTION 1 +#define W_SEARCH_VELOCITY (W_VELOCITY_MAX * 0.66666) +#define W_LATCH_VELOCITY 25 +#define W_LATCH_BACKOFF 4 +#define W_ZERO_BACKOFF 2 + #define A_AXIS_MODE AXIS_STANDARD #define A_RADIUS 1 #define A_VELOCITY_MAX 360000 @@ -185,6 +231,36 @@ #define A_LATCH_BACKOFF 10 #define A_ZERO_BACKOFF 2 +#define B_AXIS_MODE AXIS_STANDARD +#define B_RADIUS 1 +#define B_VELOCITY_MAX 360000 +#define B_FEEDRATE_MAX B_VELOCITY_MAX +#define B_TRAVEL_MIN -1 +#define B_TRAVEL_MAX -1 +#define B_JERK_MAX 100000 +#define B_JERK_HIGH_SPEED B_JERK_MAX +#define B_HOMING_INPUT 0 +#define B_HOMING_DIRECTION 0 +#define B_SEARCH_VELOCITY 600 +#define B_LATCH_VELOCITY 100 +#define B_LATCH_BACKOFF 10 +#define B_ZERO_BACKOFF 2 + +#define C_AXIS_MODE AXIS_STANDARD +#define C_RADIUS 1 +#define C_VELOCITY_MAX 360000 +#define C_FEEDRATE_MAX C_VELOCITY_MAX +#define C_TRAVEL_MIN -1 +#define C_TRAVEL_MAX -1 +#define C_JERK_MAX 100000 +#define C_JERK_HIGH_SPEED C_JERK_MAX +#define C_HOMING_INPUT 0 +#define C_HOMING_DIRECTION 0 +#define C_SEARCH_VELOCITY 600 +#define C_LATCH_VELOCITY 100 +#define C_LATCH_BACKOFF 10 +#define C_ZERO_BACKOFF 2 + //*** Input / output settings *** /* See gpio.h GPIO defines for options @@ -252,4 +328,4 @@ // Safety line w/HW timer // Unused #define DI9_MODE IO_MODE_DISABLED #define DI9_ACTION INPUT_ACTION_NONE -#define DI9_FUNCTION INPUT_FUNCTION_NONE \ No newline at end of file +#define DI9_FUNCTION INPUT_FUNCTION_NONE diff --git a/g2core/settings/settings_othermill.h b/g2core/settings/settings_othermill.h index 98799bcc..88e9719a 100644 --- a/g2core/settings/settings_othermill.h +++ b/g2core/settings/settings_othermill.h @@ -32,28 +32,31 @@ //**** GLOBAL / GENERAL SETTINGS ****************************************************** -#define JUNCTION_INTEGRATION_TIME 0.75 // cornering - between 0.10 and 2.00 (higher is faster) -#define CHORDAL_TOLERANCE 0.01 // chordal accuracy for arc drawing (in mm) +#define JUNCTION_INTEGRATION_TIME 0.75 // cornering - between 0.10 and 2.00 (higher is faster) +#define CHORDAL_TOLERANCE 0.01 // chordal accuracy for arc drawing (in mm) -#define SOFT_LIMIT_ENABLE 0 // 0=off, 1=on -#define HARD_LIMIT_ENABLE 1 // 0=off, 1=on -#define SAFETY_INTERLOCK_ENABLE 1 // 0=off, 1=on +#define SOFT_LIMIT_ENABLE 0 // 0=off, 1=on +#define HARD_LIMIT_ENABLE 1 // 0=off, 1=on +#define SAFETY_INTERLOCK_ENABLE 1 // 0=off, 1=on -#define SPINDLE_ENABLE_POLARITY 1 // 0=active low, 1=active high -#define SPINDLE_DIR_POLARITY 0 // 0=clockwise is low, 1=clockwise is high -#define SPINDLE_PAUSE_ON_HOLD true -#define SPINDLE_DWELL_TIME 1.5 // after unpausing and turning the spindle on, dwell for 1.5s +#define SPINDLE_ENABLE_POLARITY 1 // 0=active low, 1=active high +#define SPINDLE_DIR_POLARITY 0 // 0=clockwise is low, 1=clockwise is high +#define SPINDLE_PAUSE_ON_HOLD true +#define SPINDLE_DWELL_TIME 1.5 // after unpausing and turning the spindle on, dwell for 1.5s -#define ESC_BOOT_TIME 5000 // how long the ESC takes to boot, in milliseconds -#define ESC_LOCKOUT_TIME 900 // how long the interlock needs to be engaged before killing power... actually 1s, but be conservative +#define ESC_BOOT_TIME 5000 // how long the ESC takes to boot, in milliseconds +#define ESC_LOCKOUT_TIME 900 // how long the interlock needs to be engaged before killing power... actually 1s, but be conservative -#define COOLANT_MIST_POLARITY 1 // 0=active low, 1=active high -#define COOLANT_FLOOD_POLARITY 1 // 0=active low, 1=active high -#define COOLANT_PAUSE_ON_HOLD true +#define COOLANT_MIST_POLARITY 1 // 0=active low, 1=active high +#define COOLANT_FLOOD_POLARITY 1 // 0=active low, 1=active high +#define COOLANT_PAUSE_ON_HOLD true -// WARNING: Older Othermill machines use a 15deg can stack for their Z axis. -// new machines use a stepper which has the same config as the other axis. -#define HAS_CANSTACK_Z_AXIS 0 +#define FEEDHOLD_Z_LIFT 3 // mm to lift Z on feedhold +#define PROBE_REPORT_ENABLE true + +// WARNING: Very old, pre-release Othermills may have a 15deg can stack for their Z axis. +// All other machines use a stepper which has the same config as the other axis. +#define HAS_CANSTACK_Z_AXIS false /* // Switch definitions for interlock & E-stop #define ENABLE_INTERLOCK_AND_ESTOP @@ -66,20 +69,25 @@ // Communications and reporting settings -#define COMM_MODE JSON_MODE // one of: TEXT_MODE, JSON_MODE -#define XIO_ENABLE_FLOW_CONTROL FLOW_CONTROL_RTS // FLOW_CONTROL_OFF, FLOW_CONTROL_RTS +#define USB_SERIAL_PORTS_EXPOSED 1 // Valid options are 1 or 2, only! -#define TEXT_VERBOSITY TV_VERBOSE // one of: TV_SILENT, TV_VERBOSE -#define JSON_VERBOSITY JV_MESSAGES // one of: JV_SILENT, JV_FOOTER, JV_CONFIGS, JV_MESSAGES, JV_LINENUM, JV_VERBOSE -#define QUEUE_REPORT_VERBOSITY QR_SINGLE // one of: QR_OFF, QR_SINGLE, QR_TRIPLE +#define COMM_MODE JSON_MODE // one of: TEXT_MODE, JSON_MODE +#define XIO_ENABLE_FLOW_CONTROL FLOW_CONTROL_RTS // FLOW_CONTROL_OFF, FLOW_CONTROL_RTS -#define STATUS_REPORT_VERBOSITY SR_FILTERED // one of: SR_OFF, SR_FILTERED, SR_VERBOSE -#define STATUS_REPORT_MIN_MS 100 // milliseconds - enforces a viable minimum -#define STATUS_REPORT_INTERVAL_MS 250 // milliseconds - set $SV=0 to disable -#define STATUS_REPORT_DEFAULTS \ - "mpox", "mpoy", "mpoz", "ofsx", "ofsy", "ofsz", "g55x", "g55y", "g55z", "unit", "stat", "coor", "momo", "dist", \ - "home", "mots", "plan", "line", "path", "frmo", "hold", "macs", "cycs" -// "path","frmo","prbe","safe","spe","spd","hold","macs","cycs","sps" +#define TEXT_VERBOSITY TV_VERBOSE // one of: TV_SILENT, TV_VERBOSE +#define JSON_VERBOSITY JV_MESSAGES // one of: JV_SILENT, JV_FOOTER, JV_CONFIGS, JV_MESSAGES, JV_LINENUM, JV_VERBOSE +#define QUEUE_REPORT_VERBOSITY QR_SINGLE // one of: QR_OFF, QR_SINGLE, QR_TRIPLE + +#define STATUS_REPORT_VERBOSITY SR_FILTERED // one of: SR_OFF, SR_FILTERED, SR_VERBOSE +#define STATUS_REPORT_MIN_MS 100 // milliseconds - enforces a viable minimum +#define STATUS_REPORT_INTERVAL_MS 250 // milliseconds - set $SV=0 to disable +#define STATUS_REPORT_DEFAULTS "mpox", "mpoy", "mpoz", \ + "ofsx", "ofsy", "ofsz", \ + "g55x", "g55y", "g55z", \ + "unit", "stat", "coor", "momo", "dist", \ + "home", "mots", "plan", "line", "path", \ + "frmo", "hold", "macs", "cycs" +// "prbe", "safe", "spe", "spd", "sps" // Gcode startup defaults #define GCODE_DEFAULT_UNITS MILLIMETERS // MILLIMETERS or INCHES @@ -92,100 +100,123 @@ // NOTE: Motor numbers are reversed from TinyGv8 in order to maintain compatibility with wiring harnesses -#define MOTOR_POWER_LEVEL_XY 0.375 // default motor power level 0.00 - 1.00 -#define MOTOR_POWER_LEVEL_XY_IDLE 0.15 -#define MOTOR_POWER_LEVEL_Z 0.375 -#define MOTOR_POWER_LEVEL_Z_IDLE 0.15 -#define MOTOR_POWER_LEVEL_DISABLED 0.05 +#define MOTOR_POWER_LEVEL_XY 0.375 // default motor power level 0.00 - 1.00 +#define MOTOR_POWER_LEVEL_XY_IDLE 0.15 +#define MOTOR_POWER_LEVEL_Z 0.375 +#define MOTOR_POWER_LEVEL_Z_IDLE 0.15 +#define MOTOR_POWER_LEVEL_DISABLED 0.05 -#define MOTOR_POWER_MODE MOTOR_POWERED_IN_CYCLE -#define MOTOR_POWER_TIMEOUT 2.00 // motor power timeout in seconds +#define MOTOR_POWER_MODE MOTOR_POWERED_IN_CYCLE +#define MOTOR_POWER_TIMEOUT 2.00 // motor power timeout in seconds -#define M1_MOTOR_MAP AXIS_X // 1ma -#define M1_STEP_ANGLE 1.8 // 1sa -#define M1_TRAVEL_PER_REV 4.8768 // 1tr -#define M1_MICROSTEPS 8 // 1mi 1,2,4,8,16,32 -#define M1_POLARITY 1 // 1po 0=normal, 1=reversed -#define M1_POWER_MODE MOTOR_POWER_MODE // 1pm See enum cmMotorPowerMode in stepper.h -#define M1_POWER_LEVEL MOTOR_POWER_LEVEL_XY // 0.00=off, 1.00=max -#define M1_POWER_LEVEL_IDLE MOTOR_POWER_LEVEL_XY_IDLE +#define M1_MOTOR_MAP AXIS_X_EXTERNAL // 1ma +#define M1_STEP_ANGLE 1.8 // 1sa +#define M1_TRAVEL_PER_REV 4.8768 // 1tr +#define M1_MICROSTEPS 8 // 1mi 1,2,4,8,16,32 +#define M1_POLARITY 1 // 1po 0=normal, 1=reversed +#define M1_POWER_MODE MOTOR_POWER_MODE // 1pm See enum cmMotorPowerMode in stepper.h +#define M1_POWER_LEVEL MOTOR_POWER_LEVEL_XY // 0.00=off, 1.00=max +#define M1_POWER_LEVEL_IDLE MOTOR_POWER_LEVEL_XY_IDLE -#define M2_MOTOR_MAP AXIS_Y -#define M2_STEP_ANGLE 1.8 -#define M2_TRAVEL_PER_REV 4.8768 -#define M2_MICROSTEPS 8 -#define M2_POLARITY 1 -#define M2_POWER_MODE MOTOR_POWER_MODE -#define M2_POWER_LEVEL MOTOR_POWER_LEVEL_XY -#define M2_POWER_LEVEL_IDLE MOTOR_POWER_LEVEL_XY_IDLE +#define M2_MOTOR_MAP AXIS_Y_EXTERNAL +#define M2_STEP_ANGLE 1.8 +#define M2_TRAVEL_PER_REV 4.8768 +#define M2_MICROSTEPS 8 +#define M2_POLARITY 1 +#define M2_POWER_MODE MOTOR_POWER_MODE +#define M2_POWER_LEVEL MOTOR_POWER_LEVEL_XY +#define M2_POWER_LEVEL_IDLE MOTOR_POWER_LEVEL_XY_IDLE -#define M3_MOTOR_MAP AXIS_Z +#define M3_MOTOR_MAP AXIS_Z_EXTERNAL #if HAS_CANSTACK_Z_AXIS -#define M3_STEP_ANGLE 15 -#define M3_TRAVEL_PER_REV 1.27254 +#define M3_STEP_ANGLE 15 +#define M3_TRAVEL_PER_REV 1.27254 #else -#define M3_STEP_ANGLE 1.8 -#define M3_TRAVEL_PER_REV 4.8768 +#define M3_STEP_ANGLE 1.8 +#define M3_TRAVEL_PER_REV 4.8768 #endif -#define M3_MICROSTEPS 8 -#define M3_POLARITY 0 -#define M3_POWER_MODE MOTOR_POWER_MODE -#define M3_POWER_LEVEL MOTOR_POWER_LEVEL_Z -#define M3_POWER_LEVEL_IDLE MOTOR_POWER_LEVEL_Z_IDLE +#define M3_MICROSTEPS 8 +#define M3_POLARITY 0 +#define M3_POWER_MODE MOTOR_POWER_MODE +#define M3_POWER_LEVEL MOTOR_POWER_LEVEL_Z +#define M3_POWER_LEVEL_IDLE MOTOR_POWER_LEVEL_Z_IDLE + +#define M4_MOTOR_MAP AXIS_A_EXTERNAL +#define M4_STEP_ANGLE 1.8 +#define M4_TRAVEL_PER_REV 360 // degrees moved per motor rev +#define M4_MICROSTEPS 8 +#define M4_POLARITY 1 +#define M4_POWER_MODE MOTOR_DISABLED +#define M4_POWER_LEVEL MOTOR_POWER_LEVEL_DISABLED +#define M4_POWER_LEVEL_IDLE MOTOR_POWER_LEVEL_DISABLED + +#define M5_MOTOR_MAP AXIS_B_EXTERNAL +#define M5_STEP_ANGLE 1.8 +#define M5_TRAVEL_PER_REV 360 // degrees moved per motor rev +#define M5_MICROSTEPS 8 +#define M5_POLARITY 0 +#define M5_POWER_MODE MOTOR_DISABLED +#define M5_POWER_LEVEL MOTOR_POWER_LEVEL_DISABLED +#define M5_POWER_LEVEL_IDLE MOTOR_POWER_LEVEL_DISABLED + +#define M6_MOTOR_MAP AXIS_C_EXTERNAL +#define M6_STEP_ANGLE 1.8 +#define M6_TRAVEL_PER_REV 360 // degrees moved per motor rev +#define M6_MICROSTEPS 8 +#define M6_POLARITY 0 +#define M6_POWER_MODE MOTOR_DISABLED +#define M6_POWER_LEVEL MOTOR_POWER_LEVEL_DISABLED +#define M6_POWER_LEVEL_IDLE MOTOR_POWER_LEVEL_DISABLED // *** axis settings ********************************************************************************** -#define JERK_MAX 500 // 500 million mm/(min^3) -#define JERK_HIGH_SPEED 1000 // 1000 million mm/(min^3) // Jerk during homing needs to stop *fast* -#define VELOCITY_MAX 1500 -#define SEARCH_VELOCITY (VELOCITY_MAX / 3) -#define LATCH_VELOCITY 25 // reeeeally slow for accuracy +#define JERK_MAX 500 // 500 million mm/(min^3) +#define JERK_HIGH_SPEED 1000 // 1000 million mm/(min^3) // Jerk during homing needs to stop *fast* +#define VELOCITY_MAX 1500 +#define SEARCH_VELOCITY (VELOCITY_MAX / 3) +#define LATCH_VELOCITY 25 // reeeeally slow for accuracy -#define X_AXIS_MODE AXIS_STANDARD // xam see canonical_machine.h cmAxisMode for valid values -#define X_VELOCITY_MAX VELOCITY_MAX // xvm G0 max velocity in mm/min -#define X_FEEDRATE_MAX X_VELOCITY_MAX // xfr G1 max feed rate in mm/min -#define X_TRAVEL_MIN 0 // xtn minimum travel for soft limits -#define X_TRAVEL_MAX 145.6 // xtr travel between switches or crashes -#define X_JERK_MAX JERK_MAX // xjm -#define X_JERK_HIGH_SPEED JERK_HIGH_SPEED // xjh -#define X_HOMING_INPUT 1 // xhi input used for homing or 0 to disable -#define X_HOMING_DIRECTION 0 // xhd 0=search moves negative, 1= search moves positive -#define X_SEARCH_VELOCITY SEARCH_VELOCITY // xsv -#define X_LATCH_VELOCITY LATCH_VELOCITY // xlv mm/min -#define X_LATCH_BACKOFF 1 // xlb mm -#define X_ZERO_BACKOFF 0.4 // xzb mm +#define X_AXIS_MODE AXIS_STANDARD // xam see canonical_machine.h cmAxisMode for valid values +#define X_VELOCITY_MAX VELOCITY_MAX // xvm G0 max velocity in mm/min +#define X_FEEDRATE_MAX X_VELOCITY_MAX // xfr G1 max feed rate in mm/min +#define X_TRAVEL_MIN 0 // xtn minimum travel for soft limits +#define X_TRAVEL_MAX 145.6 // xtr travel between switches or crashes +#define X_JERK_MAX JERK_MAX // xjm +#define X_JERK_HIGH_SPEED JERK_HIGH_SPEED // xjh +#define X_HOMING_INPUT 1 // xhi input used for homing or 0 to disable +#define X_HOMING_DIRECTION 0 // xhd 0=search moves negative, 1= search moves positive +#define X_SEARCH_VELOCITY SEARCH_VELOCITY // xsv +#define X_LATCH_VELOCITY LATCH_VELOCITY // xlv mm/min +#define X_LATCH_BACKOFF 2 // xlb mm +#define X_ZERO_BACKOFF 0.4 // xzb mm -#define Y_AXIS_MODE AXIS_STANDARD -#define Y_VELOCITY_MAX VELOCITY_MAX -#define Y_FEEDRATE_MAX Y_VELOCITY_MAX -#define Y_TRAVEL_MIN 0 -#define Y_TRAVEL_MAX 119.1 -#define Y_JERK_MAX JERK_MAX -#define Y_JERK_HIGH_SPEED JERK_HIGH_SPEED -#define Y_HOMING_INPUT 3 -#define Y_HOMING_DIRECTION 0 -#define Y_SEARCH_VELOCITY SEARCH_VELOCITY -#define Y_LATCH_VELOCITY LATCH_VELOCITY -#define Y_LATCH_BACKOFF 1 -#define Y_ZERO_BACKOFF 0.4 +#define Y_AXIS_MODE AXIS_STANDARD +#define Y_VELOCITY_MAX X_VELOCITY_MAX +#define Y_FEEDRATE_MAX Y_VELOCITY_MAX +#define Y_TRAVEL_MIN 0 +#define Y_TRAVEL_MAX 119.1 +#define Y_JERK_MAX JERK_MAX +#define Y_JERK_HIGH_SPEED JERK_HIGH_SPEED +#define Y_HOMING_INPUT 3 +#define Y_HOMING_DIRECTION 0 +#define Y_SEARCH_VELOCITY (Y_FEEDRATE_MAX / 3) +#define Y_LATCH_VELOCITY LATCH_VELOCITY +#define Y_LATCH_BACKOFF 2 +#define Y_ZERO_BACKOFF 0.4 -#define Z_AXIS_MODE AXIS_STANDARD -#if HAS_CANSTACK_Z_AXIS -#define Z_VELOCITY_MAX 1000 -#else -#define Z_VELOCITY_MAX VELOCITY_MAX -#endif -#define Z_FEEDRATE_MAX Z_VELOCITY_MAX -#define Z_TRAVEL_MIN -60.1 -#define Z_TRAVEL_MAX 0 -#define Z_JERK_MAX JERK_MAX -#define Z_JERK_HIGH_SPEED JERK_HIGH_SPEED -#define Z_HOMING_INPUT 6 -#define Z_HOMING_DIRECTION 1 -#define Z_SEARCH_VELOCITY SEARCH_VELOCITY -#define Z_LATCH_VELOCITY LATCH_VELOCITY -#define Z_LATCH_BACKOFF 1 -#define Z_ZERO_BACKOFF 0.4 +#define Z_AXIS_MODE AXIS_STANDARD +#define Z_VELOCITY_MAX X_VELOCITY_MAX +#define Z_FEEDRATE_MAX Z_VELOCITY_MAX +#define Z_TRAVEL_MIN -60.1 +#define Z_TRAVEL_MAX 0 +#define Z_JERK_MAX JERK_MAX +#define Z_JERK_HIGH_SPEED JERK_HIGH_SPEED +#define Z_HOMING_INPUT 6 +#define Z_HOMING_DIRECTION 1 +#define Z_SEARCH_VELOCITY (Z_FEEDRATE_MAX / 3) +#define Z_LATCH_VELOCITY LATCH_VELOCITY +#define Z_LATCH_BACKOFF 2 +#define Z_ZERO_BACKOFF 0.4 //*** Input / output settings *** /* @@ -212,65 +243,61 @@ */ // Xmin on v9 board // X homing - see X axis setup -#define DI1_MODE NORMALLY_CLOSED -#define DI1_ACTION INPUT_ACTION_NONE -#define DI1_FUNCTION INPUT_FUNCTION_NONE +#define DI1_MODE IO_ACTIVE_HIGH // Normally Closed +#define DI1_ACTION INPUT_ACTION_NONE +#define DI1_FUNCTION INPUT_FUNCTION_NONE // Xmax // External ESTOP -#define DI2_MODE IO_ACTIVE_HIGH -#define DI2_ACTION INPUT_ACTION_HALT -#define DI2_FUNCTION INPUT_FUNCTION_SHUTDOWN +#define DI2_MODE IO_ACTIVE_HIGH +#define DI2_ACTION INPUT_ACTION_HALT +#define DI2_FUNCTION INPUT_FUNCTION_SHUTDOWN // Ymin // Y homing - see Y axis setup -#define DI3_MODE NORMALLY_CLOSED -#define DI3_ACTION INPUT_ACTION_NONE -#define DI3_FUNCTION INPUT_FUNCTION_NONE +#define DI3_MODE IO_ACTIVE_HIGH +#define DI3_ACTION INPUT_ACTION_NONE +#define DI3_FUNCTION INPUT_FUNCTION_NONE // Ymax // Safety interlock -#define DI4_MODE IO_ACTIVE_HIGH -#define DI4_ACTION INPUT_ACTION_NONE // (hold is performed by Interlock function) -#define DI4_FUNCTION INPUT_FUNCTION_INTERLOCK +#define DI4_MODE IO_ACTIVE_HIGH +#define DI4_ACTION INPUT_ACTION_NONE // (hold is performed by Interlock function) +#define DI4_FUNCTION INPUT_FUNCTION_INTERLOCK // Zmin // Z probe -#define DI5_MODE IO_ACTIVE_LOW -#define DI5_ACTION INPUT_ACTION_NONE -#define DI5_FUNCTION INPUT_FUNCTION_NONE +#define DI5_MODE IO_ACTIVE_LOW // Normally Open +#define DI5_ACTION INPUT_ACTION_NONE +#define DI5_FUNCTION INPUT_FUNCTION_PROBE // Zmax // Z homing - see Z axis for setup -#define DI6_MODE NORMALLY_CLOSED -#define DI6_ACTION INPUT_ACTION_NONE -#define DI6_FUNCTION INPUT_FUNCTION_NONE +#define DI6_MODE IO_ACTIVE_HIGH +#define DI6_ACTION INPUT_ACTION_NONE +#define DI6_FUNCTION INPUT_FUNCTION_NONE // Amin // Unused -#define DI7_MODE IO_MODE_DISABLED -#define DI7_ACTION INPUT_ACTION_NONE -#define DI7_FUNCTION INPUT_FUNCTION_NONE +#define DI7_MODE IO_MODE_DISABLED +#define DI7_ACTION INPUT_ACTION_NONE +#define DI7_FUNCTION INPUT_FUNCTION_NONE // Amax // Unused -#define DI8_MODE IO_MODE_DISABLED -#define DI8_ACTION INPUT_ACTION_NONE -#define DI8_FUNCTION INPUT_FUNCTION_NONE +#define DI8_MODE IO_MODE_DISABLED +#define DI8_ACTION INPUT_ACTION_NONE +#define DI8_FUNCTION INPUT_FUNCTION_NONE // Safety line w/HW timer // Unused -#define DI9_MODE IO_MODE_DISABLED -#define DI9_ACTION INPUT_ACTION_NONE -#define DI9_FUNCTION INPUT_FUNCTION_NONE +#define DI9_MODE IO_MODE_DISABLED +#define DI9_ACTION INPUT_ACTION_NONE +#define DI9_FUNCTION INPUT_FUNCTION_NONE + // *** PWM SPINDLE CONTROL *** -#define P1_PWM_FREQUENCY 100 // in Hz -#define P1_CW_SPEED_LO 10500 // in RPM (arbitrary units) -#define P1_CW_SPEED_HI 16400 -#define P1_CW_PHASE_LO 0.13 // phase [0..1] -#define P1_CW_PHASE_HI 0.17 -#define P1_CCW_SPEED_LO 0 -#define P1_CCW_SPEED_HI 0 -#define P1_CCW_PHASE_LO 0.1 -#define P1_CCW_PHASE_HI 0.1 -#define P1_PWM_PHASE_OFF 0.1 +#define P1_PWM_FREQUENCY 100 // in Hz +#define P1_CW_SPEED_LO 10500 // in RPM (arbitrary units) +#define P1_CW_SPEED_HI 16400 +#define P1_CW_PHASE_LO 0.13 // phase [0..1] +#define P1_CW_PHASE_HI 0.17 +#define P1_CCW_SPEED_LO 0 +#define P1_CCW_SPEED_HI 0 +#define P1_CCW_PHASE_LO 0.1 +#define P1_CCW_PHASE_HI 0.1 +#define P1_PWM_PHASE_OFF 0.1 -#define P1_USE_MAPPING_CUBIC -#define P1_MAPPING_CUBIC_X3 2.1225328766717546e-013 -#define P1_MAPPING_CUBIC_X2 -7.2900167282605129e-009 -#define P1_MAPPING_CUBIC_X1 8.5854646785876479e-005 -#define P1_MAPPING_CUBIC_X0 -2.1301489219406905e-001 diff --git a/g2core/settings/settings_othermill_test.h b/g2core/settings/settings_othermill_test.h index 662f73f7..8c66b58a 100644 --- a/g2core/settings/settings_othermill_test.h +++ b/g2core/settings/settings_othermill_test.h @@ -1,5 +1,5 @@ /* - * settings_othermill.h - Other Machine Company Mini Milling Machine + * settings_othermill_test.h - Other Machine Company Mini Milling Machine * This file is part of the g2core project * * This file ("the software") is free software: you can redistribute it and/or modify @@ -28,213 +28,221 @@ /***********************************************************************/ // ***> NOTE: The init message must be a single line with no CRs or LFs -#define INIT_MESSAGE "Initializing configs to OMC OtherMill settings" +#define INIT_MESSAGE "Initializing configs to OMC OtherMill Test settings" //**** GLOBAL / GENERAL SETTINGS ****************************************************** -#define JUNCTION_INTEGRATION_TIME 0.75 // cornering - between 0.10 and 2.00 (higher is faster) -#define CHORDAL_TOLERANCE 0.01 // chordal accuracy for arc drawing (in mm) +#define JUNCTION_INTEGRATION_TIME 0.75 // cornering - between 0.10 and 2.00 (higher is faster) +#define CHORDAL_TOLERANCE 0.01 // chordal accuracy for arc drawing (in mm) -#define SOFT_LIMIT_ENABLE 0 // 0=off, 1=on -#define HARD_LIMIT_ENABLE 1 // 0=off, 1=on -#define SAFETY_INTERLOCK_ENABLE 1 // 0=off, 1=on +#define SOFT_LIMIT_ENABLE 0 // 0=off, 1=on +#define HARD_LIMIT_ENABLE 1 // 0=off, 1=on +#define SAFETY_INTERLOCK_ENABLE 1 // 0=off, 1=on -#define SPINDLE_ENABLE_POLARITY 1 // 0=active low, 1=active high -#define SPINDLE_DIR_POLARITY 0 // 0=clockwise is low, 1=clockwise is high -#define SPINDLE_PAUSE_ON_HOLD true -#define SPINDLE_DWELL_TIME 1.5 // after unpausing and turning the spindle on, dwell for 1.5s +#define SPINDLE_ENABLE_POLARITY 1 // 0=active low, 1=active high +#define SPINDLE_DIR_POLARITY 0 // 0=clockwise is low, 1=clockwise is high +#define SPINDLE_PAUSE_ON_HOLD true +#define SPINDLE_DWELL_TIME 1.5 // after unpausing and turning the spindle on, dwell for 1.5s -#define ESC_BOOT_TIME 5000 // how long the ESC takes to boot, in milliseconds -#define ESC_LOCKOUT_TIME \ - 900 // how long the interlock needs to be engaged before killing power... actually 1s, but be conservative +#define ESC_BOOT_TIME 5000 // how long the ESC takes to boot, in milliseconds +#define ESC_LOCKOUT_TIME 900 // how long the interlock needs to be engaged before killing power... actually 1s, but be conservative -#define COOLANT_MIST_POLARITY 1 // 0=active low, 1=active high -#define COOLANT_FLOOD_POLARITY 1 // 0=active low, 1=active high -#define COOLANT_PAUSE_ON_HOLD true +#define COOLANT_MIST_POLARITY 1 // 0=active low, 1=active high +#define COOLANT_FLOOD_POLARITY 1 // 0=active low, 1=active high +#define COOLANT_PAUSE_ON_HOLD true + +#define FEEDHOLD_Z_LIFT 3 // mm to lift Z on feedhold +#define PROBE_REPORT_ENABLE true // Communications and reporting settings -#define COMM_MODE JSON_MODE // one of: TEXT_MODE, JSON_MODE -#define XIO_ENABLE_FLOW_CONTROL FLOW_CONTROL_RTS // FLOW_CONTROL_OFF, FLOW_CONTROL_RTS +#define USB_SERIAL_PORTS_EXPOSED 1 // Valid options are 1 or 2, only! -#define TEXT_VERBOSITY TV_VERBOSE // one of: TV_SILENT, TV_VERBOSE -#define JSON_VERBOSITY JV_CONFIGS // one of: JV_SILENT, JV_FOOTER, JV_CONFIGS, JV_MESSAGES, JV_LINENUM, JV_VERBOSE -#define QUEUE_REPORT_VERBOSITY QR_SINGLE +#define COMM_MODE JSON_MODE // one of: TEXT_MODE, JSON_MODE +#define XIO_ENABLE_FLOW_CONTROL FLOW_CONTROL_RTS // FLOW_CONTROL_OFF, FLOW_CONTROL_RTS -#define STATUS_REPORT_VERBOSITY SR_FILTERED // one of: SR_OFF, SR_FILTERED, SR_VERBOSE -#define STATUS_REPORT_MIN_MS 100 // milliseconds - enforces a viable minimum -#define STATUS_REPORT_INTERVAL_MS 250 // milliseconds - set $SV=0 to disable -#define STATUS_REPORT_DEFAULTS \ - "mpox", "mpoy", "mpoz", "ofsx", "ofsy", "ofsz", "g55x", "g55y", "g55z", "unit", "stat", "coor", "momo", "dist", \ - "home", "mots", "plan", "line", "path", "frmo", "prbe", "safe", "spe", "spd", "hold", "macs", "cycs", "sps" +#define TEXT_VERBOSITY TV_VERBOSE // one of: TV_SILENT, TV_VERBOSE +#define JSON_VERBOSITY JV_CONFIGS // one of: JV_SILENT, JV_FOOTER, JV_CONFIGS, JV_MESSAGES, JV_LINENUM, JV_VERBOSE +#define QUEUE_REPORT_VERBOSITY QR_OFF // one of: QR_OFF, QR_SINGLE, QR_TRIPLE + +#define STATUS_REPORT_VERBOSITY SR_FILTERED // one of: SR_OFF, SR_FILTERED, SR_VERBOSE +#define STATUS_REPORT_MIN_MS 200 // milliseconds - enforces a viable minimum +#define STATUS_REPORT_INTERVAL_MS 250 // milliseconds - set $SV=0 to disable +#define STATUS_REPORT_DEFAULTS "mpox", "mpoy", "mpoz", \ + "ofsx", "ofsy", "ofsz", \ + "g55x", "g55y", "g55z", \ + "unit", "stat", "coor", "momo", "dist", \ + "home", "mots", "plan", "line", "path", \ + "frmo", "prbe", "safe", "spe", "spd", \ + "hold", "macs", "cycs", "sps" // Gcode startup defaults -#define GCODE_DEFAULT_UNITS MILLIMETERS // MILLIMETERS or INCHES -#define GCODE_DEFAULT_PLANE CANON_PLANE_XY // CANON_PLANE_XY, CANON_PLANE_XZ, or CANON_PLANE_YZ -#define GCODE_DEFAULT_COORD_SYSTEM G55 // G54, G55, G56, G57, G58 or G59 -#define GCODE_DEFAULT_PATH_CONTROL PATH_CONTINUOUS +#define GCODE_DEFAULT_UNITS MILLIMETERS // MILLIMETERS or INCHES +#define GCODE_DEFAULT_PLANE CANON_PLANE_XY // CANON_PLANE_XY, CANON_PLANE_XZ, or CANON_PLANE_YZ +#define GCODE_DEFAULT_COORD_SYSTEM G55 // G54, G55, G56, G57, G58 or G59 +#define GCODE_DEFAULT_PATH_CONTROL PATH_CONTINUOUS #define GCODE_DEFAULT_DISTANCE_MODE ABSOLUTE_DISTANCE_MODE // *** motor settings ************************************************************************************ // NOTE: Motor numbers are reversed from TinyGv8 in order to maintain compatibility with wiring harnesses -#define MOTOR_POWER_LEVEL_XY 0.375 // default motor power level 0.00 - 1.00 -#define MOTOR_POWER_LEVEL_XY_IDLE 0.15 -#define MOTOR_POWER_LEVEL_Z 0.375 -#define MOTOR_POWER_LEVEL_Z_IDLE 0.15 -#define MOTOR_POWER_LEVEL_DISABLED 0.05 +#define MOTOR_POWER_LEVEL_XY 0.375 // default motor power level 0.00 - 1.00 +#define MOTOR_POWER_LEVEL_XY_IDLE 0.15 +#define MOTOR_POWER_LEVEL_Z 0.375 +#define MOTOR_POWER_LEVEL_Z_IDLE 0.15 +#define MOTOR_POWER_LEVEL_DISABLED 0.05 -#define MOTOR_POWER_MODE MOTOR_POWERED_IN_CYCLE -#define MOTOR_POWER_TIMEOUT 2.00 // motor power timeout in seconds +#define MOTOR_POWER_MODE MOTOR_POWERED_IN_CYCLE +#define MOTOR_POWER_TIMEOUT 2.00 // motor power timeout in seconds -#define M1_MOTOR_MAP AXIS_X // 1ma -#define M1_STEP_ANGLE 1.8 // 1sa -#define M1_TRAVEL_PER_REV 4.8768 // 1tr -#define M1_MICROSTEPS 8 // 1mi 1,2,4,8,16,32 -#define M1_POLARITY 1 // 1po 0=normal, 1=reversed -#define M1_POWER_MODE MOTOR_POWER_MODE // 1pm See enum cmMotorPowerMode in stepper.h -#define M1_POWER_LEVEL MOTOR_POWER_LEVEL_XY // 0.00=off, 1.00=max -#define M1_POWER_LEVEL_IDLE MOTOR_POWER_LEVEL_XY_IDLE +#define M1_MOTOR_MAP AXIS_X_EXTERNAL // 1ma +#define M1_STEP_ANGLE 1.8 // 1sa +#define M1_TRAVEL_PER_REV 4.8768 // 1tr +#define M1_MICROSTEPS 8 // 1mi 1,2,4,8,16,32 +#define M1_POLARITY 1 // 1po 0=normal, 1=reversed +#define M1_POWER_MODE MOTOR_POWER_MODE // 1pm See enum cmMotorPowerMode in stepper.h +#define M1_POWER_LEVEL MOTOR_POWER_LEVEL_XY // 0.00=off, 1.00=max +#define M1_POWER_LEVEL_IDLE MOTOR_POWER_LEVEL_XY_IDLE -#define M2_MOTOR_MAP AXIS_Y -#define M2_STEP_ANGLE 1.8 -#define M2_TRAVEL_PER_REV 4.8768 -#define M2_MICROSTEPS 8 -#define M2_POLARITY 1 -#define M2_POWER_MODE MOTOR_POWER_MODE -#define M2_POWER_LEVEL MOTOR_POWER_LEVEL_XY -#define M2_POWER_LEVEL_IDLE MOTOR_POWER_LEVEL_XY_IDLE +#define M2_MOTOR_MAP AXIS_Y_EXTERNAL +#define M2_STEP_ANGLE 1.8 +#define M2_TRAVEL_PER_REV 4.8768 +#define M2_MICROSTEPS 8 +#define M2_POLARITY 1 +#define M2_POWER_MODE MOTOR_POWER_MODE +#define M2_POWER_LEVEL MOTOR_POWER_LEVEL_XY +#define M2_POWER_LEVEL_IDLE MOTOR_POWER_LEVEL_XY_IDLE -#define M3_MOTOR_MAP AXIS_Z -#define M3_STEP_ANGLE 1.8 -#define M3_TRAVEL_PER_REV 4.8768 -#define M3_MICROSTEPS 8 -#define M3_POLARITY 0 -#define M3_POWER_MODE MOTOR_POWER_MODE -#define M3_POWER_LEVEL MOTOR_POWER_LEVEL_Z -#define M3_POWER_LEVEL_IDLE MOTOR_POWER_LEVEL_Z_IDLE +#define M3_MOTOR_MAP AXIS_Z_EXTERNAL +#define M3_STEP_ANGLE 1.8 +#define M3_TRAVEL_PER_REV 4.8768 +#define M3_MICROSTEPS 8 +#define M3_POLARITY 0 +#define M3_POWER_MODE MOTOR_POWER_MODE +#define M3_POWER_LEVEL MOTOR_POWER_LEVEL_Z +#define M3_POWER_LEVEL_IDLE MOTOR_POWER_LEVEL_Z_IDLE -#define M4_MOTOR_MAP AXIS_A -#define M4_STEP_ANGLE 1.8 -#define M4_TRAVEL_PER_REV 360 // degrees moved per motor rev -#define M4_MICROSTEPS 8 -#define M4_POLARITY 1 -#define M4_POWER_MODE MOTOR_DISABLED -#define M4_POWER_LEVEL MOTOR_POWER_LEVEL_DISABLED -#define M4_POWER_LEVEL_IDLE MOTOR_POWER_LEVEL_DISABLED +#define M4_MOTOR_MAP AXIS_A_EXTERNAL +#define M4_STEP_ANGLE 1.8 +#define M4_TRAVEL_PER_REV 360 // degrees moved per motor rev +#define M4_MICROSTEPS 8 +#define M4_POLARITY 1 +#define M4_POWER_MODE MOTOR_DISABLED +#define M4_POWER_LEVEL MOTOR_POWER_LEVEL_DISABLED +#define M4_POWER_LEVEL_IDLE MOTOR_POWER_LEVEL_DISABLED -#define M5_MOTOR_MAP AXIS_B -#define M5_STEP_ANGLE 1.8 -#define M5_TRAVEL_PER_REV 360 // degrees moved per motor rev -#define M5_MICROSTEPS 8 -#define M5_POLARITY 0 -#define M5_POWER_MODE MOTOR_DISABLED -#define M5_POWER_LEVEL MOTOR_POWER_LEVEL_DISABLED -#define M5_POWER_LEVEL_IDLE MOTOR_POWER_LEVEL_DISABLED +#define M5_MOTOR_MAP AXIS_B_EXTERNAL +#define M5_STEP_ANGLE 1.8 +#define M5_TRAVEL_PER_REV 360 // degrees moved per motor rev +#define M5_MICROSTEPS 8 +#define M5_POLARITY 0 +#define M5_POWER_MODE MOTOR_DISABLED +#define M5_POWER_LEVEL MOTOR_POWER_LEVEL_DISABLED +#define M5_POWER_LEVEL_IDLE MOTOR_POWER_LEVEL_DISABLED -#define M6_MOTOR_MAP AXIS_C -#define M6_STEP_ANGLE 1.8 -#define M6_TRAVEL_PER_REV 360 // degrees moved per motor rev -#define M6_MICROSTEPS 8 -#define M6_POLARITY 0 -#define M6_POWER_MODE MOTOR_DISABLED -#define M6_POWER_LEVEL MOTOR_POWER_LEVEL_DISABLED -#define M6_POWER_LEVEL_IDLE MOTOR_POWER_LEVEL_DISABLED +#define M6_MOTOR_MAP AXIS_C_EXTERNAL +#define M6_STEP_ANGLE 1.8 +#define M6_TRAVEL_PER_REV 360 // degrees moved per motor rev +#define M6_MICROSTEPS 8 +#define M6_POLARITY 0 +#define M6_POWER_MODE MOTOR_DISABLED +#define M6_POWER_LEVEL MOTOR_POWER_LEVEL_DISABLED +#define M6_POWER_LEVEL_IDLE MOTOR_POWER_LEVEL_DISABLED // *** axis settings ********************************************************************************** -#define JERK_MAX 500 // 500 million mm/(min^3) -#define JERK_HIGH_SPEED 1000 // 1000 million mm/(min^3) // Jerk during homing needs to stop *fast* -#define LATCH_VELOCITY 25 // reeeeally slow for accuracy +#define JERK_MAX 500 // 500 million mm/(min^3) +#define JERK_HIGH_SPEED 1000 // 1000 million mm/(min^3) // Jerk during homing needs to stop *fast* +#define LATCH_VELOCITY 25 // reeeeally slow for accuracy -#define X_AXIS_MODE AXIS_STANDARD // xam see canonical_machine.h cmAxisMode for valid values -#define X_VELOCITY_MAX 1500 // xvm G0 max velocity in mm/min -#define X_FEEDRATE_MAX X_VELOCITY_MAX // xfr G1 max feed rate in mm/min -#define X_TRAVEL_MIN 0 // xtn minimum travel for soft limits -#define X_TRAVEL_MAX 145.6 // xtr travel between switches or crashes -#define X_JERK_MAX JERK_MAX // xjm -#define X_JERK_HIGH_SPEED JERK_HIGH_SPEED // xjh -#define X_HOMING_INPUT 1 // xhi input used for homing or 0 to disable -#define X_HOMING_DIRECTION 0 // xhd 0=search moves negative, 1= search moves positive -#define X_SEARCH_VELOCITY (X_FEEDRATE_MAX / 3) // xsv -#define X_LATCH_VELOCITY LATCH_VELOCITY // xlv mm/min -#define X_LATCH_BACKOFF 1.5 // xlb mm -#define X_ZERO_BACKOFF 0.4 // xzb mm +#define X_AXIS_MODE AXIS_STANDARD // xam see canonical_machine.h cmAxisMode for valid values +#define X_VELOCITY_MAX 1500 // xvm G0 max velocity in mm/min +#define X_FEEDRATE_MAX X_VELOCITY_MAX // xfr G1 max feed rate in mm/min +#define X_TRAVEL_MIN 0 // xtn minimum travel for soft limits +#define X_TRAVEL_MAX 145.6 // xtr travel between switches or crashes +#define X_JERK_MAX JERK_MAX // xjm +#define X_JERK_HIGH_SPEED JERK_HIGH_SPEED // xjh +#define X_HOMING_INPUT 1 // xhi input used for homing or 0 to disable +#define X_HOMING_DIRECTION 0 // xhd 0=search moves negative, 1= search moves positive +#define X_SEARCH_VELOCITY (X_FEEDRATE_MAX / 3)// xsv +#define X_LATCH_VELOCITY LATCH_VELOCITY // xlv mm/min +#define X_LATCH_BACKOFF 2 // xlb mm +#define X_ZERO_BACKOFF 0.4 // xzb mm -#define Y_AXIS_MODE AXIS_STANDARD -#define Y_VELOCITY_MAX X_VELOCITY_MAX -#define Y_FEEDRATE_MAX Y_VELOCITY_MAX -#define Y_TRAVEL_MIN 0 -#define Y_TRAVEL_MAX 119.1 -#define Y_JERK_MAX JERK_MAX -#define Y_JERK_HIGH_SPEED JERK_HIGH_SPEED -#define Y_HOMING_INPUT 3 -#define Y_HOMING_DIRECTION 0 -#define Y_SEARCH_VELOCITY (Y_FEEDRATE_MAX / 3) -#define Y_LATCH_VELOCITY LATCH_VELOCITY -#define Y_LATCH_BACKOFF 1.5 -#define Y_ZERO_BACKOFF 0.4 +#define Y_AXIS_MODE AXIS_STANDARD +#define Y_VELOCITY_MAX X_VELOCITY_MAX +#define Y_FEEDRATE_MAX Y_VELOCITY_MAX +#define Y_TRAVEL_MIN 0 +#define Y_TRAVEL_MAX 119.1 +#define Y_JERK_MAX JERK_MAX +#define Y_JERK_HIGH_SPEED JERK_HIGH_SPEED +#define Y_HOMING_INPUT 3 +#define Y_HOMING_DIRECTION 0 +#define Y_SEARCH_VELOCITY (Y_FEEDRATE_MAX / 3) +#define Y_LATCH_VELOCITY LATCH_VELOCITY +#define Y_LATCH_BACKOFF 2 +#define Y_ZERO_BACKOFF 0.4 -#define Z_AXIS_MODE AXIS_STANDARD -#define Z_VELOCITY_MAX X_VELOCITY_MAX -#define Z_FEEDRATE_MAX Z_VELOCITY_MAX -#define Z_TRAVEL_MIN -60.1 -#define Z_TRAVEL_MAX 0 -#define Z_JERK_MAX JERK_MAX -#define Z_JERK_HIGH_SPEED JERK_HIGH_SPEED -#define Z_HOMING_INPUT 6 -#define Z_HOMING_DIRECTION 1 -#define Z_SEARCH_VELOCITY (Z_FEEDRATE_MAX / 3) -#define Z_LATCH_VELOCITY LATCH_VELOCITY -#define Z_LATCH_BACKOFF 1.5 -#define Z_ZERO_BACKOFF 0.4 +#define Z_AXIS_MODE AXIS_STANDARD +#define Z_VELOCITY_MAX X_VELOCITY_MAX +#define Z_FEEDRATE_MAX Z_VELOCITY_MAX +#define Z_TRAVEL_MIN -60.1 +#define Z_TRAVEL_MAX 0 +#define Z_JERK_MAX JERK_MAX +#define Z_JERK_HIGH_SPEED JERK_HIGH_SPEED +#define Z_HOMING_INPUT 6 +#define Z_HOMING_DIRECTION 1 +#define Z_SEARCH_VELOCITY (Z_FEEDRATE_MAX / 3) +#define Z_LATCH_VELOCITY LATCH_VELOCITY +#define Z_LATCH_BACKOFF 2 +#define Z_ZERO_BACKOFF 0.4 // Rotary values are chosen to make the motor react the same as X for testing -#define A_AXIS_MODE AXIS_DISABLED // DISABLED -#define A_VELOCITY_MAX ((X_VELOCITY_MAX / M1_TRAVEL_PER_REV) * 360) // set to the same speed as X axis -#define A_FEEDRATE_MAX A_VELOCITY_MAX -#define A_TRAVEL_MIN -1 // min/max the same means infinite, no limit -#define A_TRAVEL_MAX -1 -#define A_JERK_MAX (X_JERK_MAX * (360 / M1_TRAVEL_PER_REV)) -#define A_JERK_HIGH_SPEED A_JERK_MAX -#define A_RADIUS (M1_TRAVEL_PER_REV / (2 * 3.14159628)) -#define A_HOMING_INPUT 0 -#define A_HOMING_DIRECTION 0 -#define A_SEARCH_VELOCITY 600 -#define A_LATCH_VELOCITY 100 -#define A_LATCH_BACKOFF 5 -#define A_ZERO_BACKOFF 2 +#define A_AXIS_MODE AXIS_DISABLED // DISABLED +#define A_VELOCITY_MAX ((X_VELOCITY_MAX / M1_TRAVEL_PER_REV) * 360) // set to the same speed as X axis +#define A_FEEDRATE_MAX A_VELOCITY_MAX +#define A_TRAVEL_MIN -1 // min/max the same means infinite, no limit +#define A_TRAVEL_MAX -1 +#define A_JERK_MAX (X_JERK_MAX * (360 / M1_TRAVEL_PER_REV)) +#define A_JERK_HIGH_SPEED A_JERK_MAX +#define A_RADIUS (M1_TRAVEL_PER_REV / (2 * 3.14159628)) +#define A_HOMING_INPUT 0 +#define A_HOMING_DIRECTION 0 +#define A_SEARCH_VELOCITY 600 +#define A_LATCH_VELOCITY 100 +#define A_LATCH_BACKOFF 5 +#define A_ZERO_BACKOFF 2 -#define B_AXIS_MODE AXIS_DISABLED // DISABLED -#define B_VELOCITY_MAX ((X_VELOCITY_MAX / M1_TRAVEL_PER_REV) * 360) -#define B_FEEDRATE_MAX B_VELOCITY_MAX -#define B_TRAVEL_MIN -1 -#define B_TRAVEL_MAX -1 -#define B_JERK_MAX (X_JERK_MAX * (360 / M1_TRAVEL_PER_REV)) -#define B_JERK_HIGH_SPEED B_JERK_MAX -#define B_RADIUS (M1_TRAVEL_PER_REV / (2 * 3.14159628)) -#define B_HOMING_INPUT 0 -#define B_HOMING_DIRECTION 0 -#define B_SEARCH_VELOCITY 600 -#define B_LATCH_VELOCITY 100 -#define B_LATCH_BACKOFF 5 -#define B_ZERO_BACKOFF 2 +#define B_AXIS_MODE AXIS_DISABLED // DISABLED +#define B_VELOCITY_MAX ((X_VELOCITY_MAX / M1_TRAVEL_PER_REV) * 360) +#define B_FEEDRATE_MAX B_VELOCITY_MAX +#define B_TRAVEL_MIN -1 +#define B_TRAVEL_MAX -1 +#define B_JERK_MAX (X_JERK_MAX * (360 / M1_TRAVEL_PER_REV)) +#define B_JERK_HIGH_SPEED B_JERK_MAX +#define B_RADIUS (M1_TRAVEL_PER_REV / (2 * 3.14159628)) +#define B_HOMING_INPUT 0 +#define B_HOMING_DIRECTION 0 +#define B_SEARCH_VELOCITY 600 +#define B_LATCH_VELOCITY 100 +#define B_LATCH_BACKOFF 5 +#define B_ZERO_BACKOFF 2 -#define C_AXIS_MODE AXIS_DISABLED // DISABLED -#define C_VELOCITY_MAX ((X_VELOCITY_MAX / M1_TRAVEL_PER_REV) * 360) -#define C_FEEDRATE_MAX C_VELOCITY_MAX -#define C_TRAVEL_MIN -1 -#define C_TRAVEL_MAX -1 -#define C_JERK_MAX (X_JERK_MAX * (360 / M1_TRAVEL_PER_REV)) -#define C_JERK_HIGH_SPEED C_JERK_MAX -#define C_RADIUS (M1_TRAVEL_PER_REV / (2 * 3.14159628)) -#define C_HOMING_INPUT 0 -#define C_HOMING_DIRECTION 0 -#define C_SEARCH_VELOCITY 600 -#define C_LATCH_VELOCITY 100 -#define C_LATCH_BACKOFF 5 -#define C_ZERO_BACKOFF 2 +#define C_AXIS_MODE AXIS_DISABLED // DISABLED +#define C_VELOCITY_MAX ((X_VELOCITY_MAX / M1_TRAVEL_PER_REV) * 360) +#define C_FEEDRATE_MAX C_VELOCITY_MAX +#define C_TRAVEL_MIN -1 +#define C_TRAVEL_MAX -1 +#define C_JERK_MAX (X_JERK_MAX * (360 / M1_TRAVEL_PER_REV)) +#define C_JERK_HIGH_SPEED C_JERK_MAX +#define C_RADIUS (M1_TRAVEL_PER_REV / (2 * 3.14159628)) +#define C_HOMING_INPUT 0 +#define C_HOMING_DIRECTION 0 +#define C_SEARCH_VELOCITY 600 +#define C_LATCH_VELOCITY 100 +#define C_LATCH_BACKOFF 5 +#define C_ZERO_BACKOFF 2 //*** Input / output settings *** /* @@ -261,140 +269,67 @@ */ // Xmin on v9 board // X homing - see X axis setup -#define DI1_MODE NORMALLY_CLOSED -#define DI1_ACTION INPUT_ACTION_NONE -#define DI1_FUNCTION INPUT_FUNCTION_NONE +#define DI1_MODE IO_ACTIVE_HIGH // Normally CLosed +#define DI1_ACTION INPUT_ACTION_NONE +#define DI1_FUNCTION INPUT_FUNCTION_NONE // Xmax // External ESTOP -#define DI2_MODE IO_ACTIVE_HIGH -#define DI2_ACTION INPUT_ACTION_HALT -#define DI2_FUNCTION INPUT_FUNCTION_SHUTDOWN +#define DI2_MODE IO_ACTIVE_HIGH +#define DI2_ACTION INPUT_ACTION_HALT +#define DI2_FUNCTION INPUT_FUNCTION_SHUTDOWN // Ymin // Y homing - see Y axis setup -#define DI3_MODE NORMALLY_CLOSED -#define DI3_ACTION INPUT_ACTION_NONE -#define DI3_FUNCTION INPUT_FUNCTION_NONE +#define DI3_MODE IO_ACTIVE_HIGH +#define DI3_ACTION INPUT_ACTION_NONE +#define DI3_FUNCTION INPUT_FUNCTION_NONE // Ymax // Safety interlock -#define DI4_MODE IO_ACTIVE_HIGH -#define DI4_ACTION INPUT_ACTION_NONE // (hold is performed by Interlock function) -#define DI4_FUNCTION INPUT_FUNCTION_INTERLOCK +#define DI4_MODE IO_ACTIVE_HIGH +#define DI4_ACTION INPUT_ACTION_NONE // (hold is performed by Interlock function) +#define DI4_FUNCTION INPUT_FUNCTION_INTERLOCK // Zmin // Z probe -#define DI5_MODE IO_ACTIVE_LOW -#define DI5_ACTION INPUT_ACTION_NONE -#define DI5_FUNCTION INPUT_FUNCTION_PROBE +#define DI5_MODE IO_ACTIVE_LOW // Normally Open +#define DI5_ACTION INPUT_ACTION_NONE +#define DI5_FUNCTION INPUT_FUNCTION_PROBE // Zmax // Z homing - see Z axis for setup -#define DI6_MODE NORMALLY_CLOSED -#define DI6_ACTION INPUT_ACTION_NONE -#define DI6_FUNCTION INPUT_FUNCTION_NONE +#define DI6_MODE IO_ACTIVE_HIGH +#define DI6_ACTION INPUT_ACTION_NONE +#define DI6_FUNCTION INPUT_FUNCTION_NONE // Amin // Unused -#define DI7_MODE IO_MODE_DISABLED -#define DI7_ACTION INPUT_ACTION_NONE -#define DI7_FUNCTION INPUT_FUNCTION_NONE +#define DI7_MODE IO_MODE_DISABLED +#define DI7_ACTION INPUT_ACTION_NONE +#define DI7_FUNCTION INPUT_FUNCTION_NONE // Amax // Unused -#define DI8_MODE IO_MODE_DISABLED -#define DI8_ACTION INPUT_ACTION_NONE -#define DI8_FUNCTION INPUT_FUNCTION_NONE +#define DI8_MODE IO_MODE_DISABLED +#define DI8_ACTION INPUT_ACTION_NONE +#define DI8_FUNCTION INPUT_FUNCTION_NONE // Safety line w/HW timer // Unused -#define DI9_MODE IO_MODE_DISABLED -#define DI9_ACTION INPUT_ACTION_NONE -#define DI9_FUNCTION INPUT_FUNCTION_NONE +#define DI9_MODE IO_MODE_DISABLED +#define DI9_ACTION INPUT_ACTION_NONE +#define DI9_FUNCTION INPUT_FUNCTION_NONE // *** PWM SPINDLE CONTROL *** -#define P1_PWM_FREQUENCY 100 // in Hz -#define P1_CW_SPEED_LO 10500 // in RPM (arbitrary units) -#define P1_CW_SPEED_HI 16400 -#define P1_CW_PHASE_LO 0.13 // phase [0..1] -#define P1_CW_PHASE_HI 0.17 -#define P1_CCW_SPEED_LO 0 -#define P1_CCW_SPEED_HI 0 -#define P1_CCW_PHASE_LO 0.1 -#define P1_CCW_PHASE_HI 0.1 -#define P1_PWM_PHASE_OFF 0.1 +#define P1_PWM_FREQUENCY 100 // in Hz +#define P1_CW_SPEED_LO 10500 // in RPM (arbitrary units) +#define P1_CW_SPEED_HI 16400 +#define P1_CW_PHASE_LO 0.13 // phase [0..1] +#define P1_CW_PHASE_HI 0.17 +#define P1_CCW_SPEED_LO 0 +#define P1_CCW_SPEED_HI 0 +#define P1_CCW_PHASE_LO 0.1 +#define P1_CCW_PHASE_HI 0.1 +#define P1_PWM_PHASE_OFF 0.1 +/* #define P1_USE_MAPPING_CUBIC -#define P1_MAPPING_CUBIC_X3 2.1225328766717546e-013 -#define P1_MAPPING_CUBIC_X2 -7.2900167282605129e-009 -#define P1_MAPPING_CUBIC_X1 8.5854646785876479e-005 -#define P1_MAPPING_CUBIC_X0 -2.1301489219406905e-001 - -// *** DEFAULT COORDINATE SYSTEM OFFSETS *** - -#define G54_X_OFFSET 0 // G54 is traditionally set to all zeros -#define G54_Y_OFFSET 0 -#define G54_Z_OFFSET 0 -#define G54_A_OFFSET 0 -#define G54_B_OFFSET 0 -#define G54_C_OFFSET 0 - -#define G55_X_OFFSET 0 // but the again, so is everyting else (at least for start) -#define G55_Y_OFFSET 0 -#define G55_Z_OFFSET 0 -#define G55_A_OFFSET 0 -#define G55_B_OFFSET 0 -#define G55_C_OFFSET 0 // this is where we currently store the tool offset - -#define G56_X_OFFSET 0 -#define G56_Y_OFFSET 0 -#define G56_Z_OFFSET 0 -#define G56_A_OFFSET 0 -#define G56_B_OFFSET 0 -#define G56_C_OFFSET 0 - -#define G57_X_OFFSET 0 -#define G57_Y_OFFSET 0 -#define G57_Z_OFFSET 0 -#define G57_A_OFFSET 0 -#define G57_B_OFFSET 0 -#define G57_C_OFFSET 0 - -#define G58_X_OFFSET 0 -#define G58_Y_OFFSET 0 -#define G58_Z_OFFSET 0 -#define G58_A_OFFSET 0 -#define G58_B_OFFSET 0 -#define G58_C_OFFSET 0 - -#define G59_X_OFFSET 0 -#define G59_Y_OFFSET 0 -#define G59_Z_OFFSET 0 -#define G59_A_OFFSET 0 -#define G59_B_OFFSET 0 -#define G59_C_OFFSET 0 - -/*** User-Defined Data Defaults ***/ - -#define USER_DATA_A0 0 -#define USER_DATA_A1 0 -#define USER_DATA_A2 0 -#define USER_DATA_A3 0 -#define USER_DATA_B0 0 -#define USER_DATA_B1 0 -#define USER_DATA_B2 0 -#define USER_DATA_B3 0 -#define USER_DATA_C0 0 -#define USER_DATA_C1 0 -#define USER_DATA_C2 0 -#define USER_DATA_C3 0 -#define USER_DATA_D0 0 -#define USER_DATA_D1 0 -#define USER_DATA_D2 0 -#define USER_DATA_D3 0 - -constexpr float H1_DEFAULT_P = 7.0; -constexpr float H1_DEFAULT_I = 0.2; -constexpr float H1_DEFAULT_D = 100.0; - -constexpr float H2_DEFAULT_P = 7.0; -constexpr float H2_DEFAULT_I = 0.2; -constexpr float H2_DEFAULT_D = 100.0; - -constexpr float H3_DEFAULT_P = 7.0; -constexpr float H3_DEFAULT_I = 0.2; -constexpr float H3_DEFAULT_D = 100.0; +#define P1_MAPPING_CUBIC_X3 2.1225328766717546e-013 +#define P1_MAPPING_CUBIC_X2 -7.2900167282605129e-009 +#define P1_MAPPING_CUBIC_X1 8.5854646785876479e-005 +#define P1_MAPPING_CUBIC_X0 -2.1301489219406905e-001 +*/ \ No newline at end of file diff --git a/g2core/settings/settings_probotixV90.h b/g2core/settings/settings_probotixV90.h index 33549c86..30ff9688 100644 --- a/g2core/settings/settings_probotixV90.h +++ b/g2core/settings/settings_probotixV90.h @@ -2,7 +2,7 @@ * settings_probotix.h - Probotix Fireball V90 machine profile * This file is part the g2core project * - * Copyright (c) 2011 - 2016 Alden S. Hart, Jr. + * Copyright (c) 2011 - 2018 Alden S. Hart, Jr. * * This file ("the software") is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License, version 2 as published by the @@ -83,7 +83,7 @@ #define MOTOR_POWER_MODE MOTOR_POWERED_IN_CYCLE // default motor power mode (see cmMotorPowerMode in stepper.h) #define MOTOR_POWER_TIMEOUT 2.00 // motor power timeout in seconds -#define M1_MOTOR_MAP AXIS_X // 1ma +#define M1_MOTOR_MAP AXIS_X_EXTERNAL // 1ma #define M1_STEP_ANGLE 1.8 // 1sa #define M1_TRAVEL_PER_REV 5.08 // 1tr #define M1_MICROSTEPS 8 // 1mi 1,2,4,8 @@ -91,7 +91,7 @@ #define M1_POWER_MODE MOTOR_POWER_MODE // 1pm standard #define M1_POWER_LEVEL 0.75 -#define M2_MOTOR_MAP AXIS_Y +#define M2_MOTOR_MAP AXIS_Y_EXTERNAL #define M2_STEP_ANGLE 1.8 #define M2_TRAVEL_PER_REV 5.08 #define M2_MICROSTEPS 8 @@ -99,7 +99,7 @@ #define M2_POWER_MODE MOTOR_POWER_MODE #define M2_POWER_LEVEL 0.75 -#define M3_MOTOR_MAP AXIS_Z +#define M3_MOTOR_MAP AXIS_Z_EXTERNAL #define M3_STEP_ANGLE 1.8 #define M3_TRAVEL_PER_REV 2.1166666 #define M3_MICROSTEPS 8 diff --git a/g2core/settings/settings_shapeoko2.h b/g2core/settings/settings_shapeoko2.h index d5265d7d..1590195c 100644 --- a/g2core/settings/settings_shapeoko2.h +++ b/g2core/settings/settings_shapeoko2.h @@ -2,7 +2,7 @@ * settings_shapeoko2.h - Shapeoko2 500mm table * This file is part of the g2core project * - * Copyright (c) 2010 - 2016 Alden S. Hart, Jr. + * Copyright (c) 2010 - 2018 Alden S. Hart, Jr. * * This file ("the software") is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License, version 2 as published by the @@ -86,7 +86,7 @@ #define MOTOR_POWER_MODE MOTOR_POWERED_IN_CYCLE // default motor power mode (see cmMotorPowerMode in stepper.h) #define MOTOR_POWER_TIMEOUT 2.00 // motor power timeout in seconds -#define M1_MOTOR_MAP AXIS_X // 1ma +#define M1_MOTOR_MAP AXIS_X_EXTERNAL // 1ma #define M1_STEP_ANGLE 1.8 // 1sa #define M1_TRAVEL_PER_REV 40.00 // 1tr #define M1_MICROSTEPS 8 // 1mi 1,2,4,8,16,32 @@ -94,7 +94,7 @@ #define M1_POWER_MODE MOTOR_POWER_MODE // 1pm TRUE=low power idle enabled #define M1_POWER_LEVEL 0.500 -#define M2_MOTOR_MAP AXIS_Y +#define M2_MOTOR_MAP AXIS_Y_EXTERNAL #define M2_STEP_ANGLE 1.8 #define M2_TRAVEL_PER_REV 40.00 #define M2_MICROSTEPS 8 @@ -102,7 +102,7 @@ #define M2_POWER_MODE MOTOR_POWER_MODE #define M2_POWER_LEVEL 0.500 -#define M3_MOTOR_MAP AXIS_Z +#define M3_MOTOR_MAP AXIS_Z_EXTERNAL #define M3_STEP_ANGLE 1.8 #define M3_TRAVEL_PER_REV 1.25 #define M3_MICROSTEPS 8 diff --git a/g2core/settings/settings_shopbot_sbv300.h b/g2core/settings/settings_shopbot_sbv300.h index 2cf538bb..ff6f5257 100644 --- a/g2core/settings/settings_shopbot_sbv300.h +++ b/g2core/settings/settings_shopbot_sbv300.h @@ -1,8 +1,8 @@ /* - * settings_shapeoko375.h - Shopbot Test + * settings_shopbot_sbv300.h - Shopbot sbv3000 * This file is part of the g2core project * - * Copyright (c) 2015 - 2016 Alden S. Hart, Jr. + * Copyright (c) 2015 - 2018 Alden S. Hart, Jr. * * This file ("the software") is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License, version 2 as published by the @@ -38,218 +38,197 @@ //**** GLOBAL / GENERAL SETTINGS ****************************************************** -#define JUNCTION_INTEGRATION_TIME (5000 * 25.4) // centripetal acceleration around corners -#define CHORDAL_TOLERANCE 0.001 // chordal accuracy for arc drawing (in mm) +#define JUNCTION_INTEGRATION_TIME 0.8 // cornering - between 0.10 and 2.00 (higher is faster) +#define CHORDAL_TOLERANCE 0.0001 // chordal accuracy for arc drawing (in mm) -#define SOFT_LIMIT_ENABLE 0 // 0=off, 1=on -#define HARD_LIMIT_ENABLE 1 // 0=off, 1=on -#define SAFETY_INTERLOCK_ENABLE 1 // 0=off, 1=on +#define SOFT_LIMIT_ENABLE 0 // 0=off, 1=on +#define HARD_LIMIT_ENABLE 1 // 0=off, 1=on +#define SAFETY_INTERLOCK_ENABLE 1 // 0=off, 1=on -#define SPINDLE_ENABLE_POLARITY 1 // 0=active low, 1=active high -#define SPINDLE_DIR_POLARITY 0 // 0=clockwise is low, 1=clockwise is high -#define SPINDLE_PAUSE_ON_HOLD true -#define SPINDLE_DWELL_TIME 1.0 +#define SPINDLE_ENABLE_POLARITY 1 // 0=active low, 1=active high +#define SPINDLE_DIR_POLARITY 0 // 0=clockwise is low, 1=clockwise is high +#define SPINDLE_PAUSE_ON_HOLD true +#define SPINDLE_DWELL_TIME 1.0 -#define COOLANT_MIST_POLARITY 1 // 0=active low, 1=active high -#define COOLANT_FLOOD_POLARITY 1 // 0=active low, 1=active high -#define COOLANT_PAUSE_ON_HOLD true +#define COOLANT_MIST_POLARITY 1 // 0=active low, 1=active high +#define COOLANT_FLOOD_POLARITY 1 // 0=active low, 1=active high +#define COOLANT_PAUSE_ON_HOLD true -// Communications and reporting settings - -#define USB_SERIAL_PORTS_EXPOSED 1 // Valid options are 1 or 2, only! - -#define COMM_MODE JSON_MODE // one of: TEXT_MODE, JSON_MODE -#define XIO_ENABLE_FLOW_CONTROL FLOW_CONTROL_RTS // FLOW_CONTROL_OFF, FLOW_CONTROL_RTS - -#define TEXT_VERBOSITY TV_VERBOSE // one of: TV_SILENT, TV_VERBOSE -#define JSON_VERBOSITY JV_MESSAGES // one of: JV_SILENT, JV_FOOTER, JV_CONFIGS, JV_MESSAGES, JV_LINENUM, JV_VERBOSE -#define QUEUE_REPORT_VERBOSITY QR_TRIPLE // one of: QR_OFF, QR_SINGLE, QR_TRIPLE - -#define STATUS_REPORT_VERBOSITY SR_FILTERED // one of: SR_OFF, SR_FILTERED, SR_VERBOSE -#define STATUS_REPORT_MIN_MS 200 // milliseconds - enforces a viable minimum -#define STATUS_REPORT_INTERVAL_MS 250 // milliseconds - set $SV=0 to disable -#define STATUS_REPORT_DEFAULTS "posx", "posy", "posz", "posa", "posb", "vel", "stat", "hold", "line", "coor" - -// Gcode startup defaults -#define GCODE_DEFAULT_UNITS INCHES // MILLIMETERS or INCHES -#define GCODE_DEFAULT_PLANE CANON_PLANE_XY // CANON_PLANE_XY, CANON_PLANE_XZ, or CANON_PLANE_YZ -#define GCODE_DEFAULT_COORD_SYSTEM G55 // G54, G55, G56, G57, G58 or G59 -#define GCODE_DEFAULT_PATH_CONTROL PATH_CONTINUOUS -#define GCODE_DEFAULT_DISTANCE_MODE ABSOLUTE_DISTANCE_MODE - -constexpr float H1_DEFAULT_P = 7.0; -constexpr float H1_DEFAULT_I = 0.2; -constexpr float H1_DEFAULT_D = 100.0; - -constexpr float H2_DEFAULT_P = 7.0; -constexpr float H2_DEFAULT_I = 0.2; -constexpr float H2_DEFAULT_D = 100.0; - -constexpr float H3_DEFAULT_P = 7.0; -constexpr float H3_DEFAULT_I = 0.2; -constexpr float H3_DEFAULT_D = 100.0; - -// *** motor settings ************************************************************************************ - -#define MOTOR_POWER_MODE MOTOR_POWERED_IN_CYCLE // default motor power mode (see cmMotorPowerMode in stepper.h) -#define MOTOR_POWER_TIMEOUT 2.00 // motor power timeout in seconds -#define MOTOR_POWER_LEVEL 0.375 // default motor power level 0.00 - 1.00 - -#define M1_MOTOR_MAP AXIS_X // 1ma -#define M1_STEP_ANGLE 1.8 // 1sa -#define M1_TRAVEL_PER_REV (0.5 * 25.4) // 1tr -#define M1_MICROSTEPS 10 // 1mi 1,2,4,8 -#define M1_POLARITY 0 // 1po 0=normal, 1=reversed -#define M1_POWER_MODE MOTOR_POWER_MODE // 1pm TRUE=low power idle enabled -#define M1_POWER_LEVEL MOTOR_POWER_LEVEL // 1pl Irrelevant to Shopbot sbv300 - -#define M2_MOTOR_MAP AXIS_Y -#define M2_STEP_ANGLE 1.8 -#define M2_TRAVEL_PER_REV (0.5 * 25.4) -#define M2_MICROSTEPS 10 -#define M2_POLARITY 0 -#define M2_POWER_MODE MOTOR_POWER_MODE -#define M2_POWER_LEVEL MOTOR_POWER_LEVEL - -#define M3_MOTOR_MAP AXIS_Z -#define M3_STEP_ANGLE 1.8 -#define M3_TRAVEL_PER_REV (0.5 * 25.4) -#define M3_MICROSTEPS 10 -#define M3_POLARITY 0 -#define M3_POWER_MODE MOTOR_POWER_MODE -#define M3_POWER_LEVEL MOTOR_POWER_LEVEL - -#define M4_MOTOR_MAP AXIS_A -#define M4_STEP_ANGLE 1.8 -#define M4_TRAVEL_PER_REV (0.5 * 25.4) -#define M4_MICROSTEPS 10 -#define M4_POLARITY 0 -#define M4_POWER_MODE MOTOR_POWER_MODE -#define M4_POWER_LEVEL MOTOR_POWER_LEVEL - -#if (MOTORS >= 5) -#define M5_MOTOR_MAP AXIS_B -#define M5_STEP_ANGLE 1.8 -#define M5_TRAVEL_PER_REV (0.5 * 25.4) -#define M5_MICROSTEPS 10 -#define M5_POLARITY 0 -#define M5_POWER_MODE MOTOR_POWER_MODE -#define M5_POWER_LEVEL MOTOR_POWER_LEVEL - -#define M6_MOTOR_MAP AXIS_C -#define M6_STEP_ANGLE 1.8 -#define M6_TRAVEL_PER_REV (0.5 * 25.4) -#define M6_MICROSTEPS 10 -#define M6_POLARITY 0 -#define M6_POWER_MODE MOTOR_POWER_MODE -#define M6_POWER_LEVEL MOTOR_POWER_LEVEL -#endif +#define FEEDHOLD_Z_LIFT (0.5 * 25.4) #define MANUAL_FEEDRATE_OVERRIDE_ENABLE false #define MANUAL_FEEDRATE_OVERRIDE_PARAMETER 1.00 + +// Communications and reporting settings + +#define USB_SERIAL_PORTS_EXPOSED 1 // Valid options are 1 or 2, only! + +#define COMM_MODE JSON_MODE // one of: TEXT_MODE, JSON_MODE +#define XIO_ENABLE_FLOW_CONTROL FLOW_CONTROL_RTS // FLOW_CONTROL_OFF, FLOW_CONTROL_RTS + +#define TEXT_VERBOSITY TV_VERBOSE // one of: TV_SILENT, TV_VERBOSE +#define JSON_VERBOSITY JV_CONFIGS // one of: JV_SILENT, JV_FOOTER, JV_CONFIGS, JV_MESSAGES, JV_LINENUM, JV_VERBOSE +#define QUEUE_REPORT_VERBOSITY QR_TRIPLE // one of: QR_OFF, QR_SINGLE, QR_TRIPLE + +#define STATUS_REPORT_VERBOSITY SR_FILTERED // one of: SR_OFF, SR_FILTERED, SR_VERBOSE +#define STATUS_REPORT_MIN_MS 200 // milliseconds - enforces a viable minimum +#define STATUS_REPORT_INTERVAL_MS 250 // milliseconds - set $SV=0 to disable + +#define STATUS_REPORT_DEFAULTS "posx", "posy", "posz", "posa", "posb", \ + "vel", "stat", "hold", "line", "coor", "unit", \ + "in1", "in2", "in3", "in4", "in5", "in6", "in7", "in8" + +// Gcode startup defaults +#define GCODE_DEFAULT_UNITS INCHES // MILLIMETERS or INCHES +#define GCODE_DEFAULT_PLANE CANON_PLANE_XY // CANON_PLANE_XY, CANON_PLANE_XZ, or CANON_PLANE_YZ +#define GCODE_DEFAULT_COORD_SYSTEM G55 // G54, G55, G56, G57, G58 or G59 +#define GCODE_DEFAULT_PATH_CONTROL PATH_CONTINUOUS +#define GCODE_DEFAULT_DISTANCE_MODE ABSOLUTE_DISTANCE_MODE + +// *** motor settings ************************************************************************************ + +#define MOTOR_POWER_MODE MOTOR_POWERED_IN_CYCLE // default motor power mode (see cmMotorPowerMode in stepper.h) +#define MOTOR_POWER_TIMEOUT 2.00 // motor power timeout in seconds +#define MOTOR_POWER_LEVEL 0.375 // default motor power level 0.00 - 1.00 + +#define M1_MOTOR_MAP AXIS_X_EXTERNAL // 1ma +#define M1_STEP_ANGLE 1.8 // 1sa +#define M1_TRAVEL_PER_REV (0.5 * 25.4) // 1tr +#define M1_MICROSTEPS 10 // 1mi 1,2,4,8 +#define M1_POLARITY 0 // 1po 0=normal, 1=reversed +#define M1_POWER_MODE MOTOR_POWER_MODE // 1pm TRUE=low power idle enabled +#define M1_POWER_LEVEL MOTOR_POWER_LEVEL // 1pl Irrelevant to Shopbot sbv300 + +#define M2_MOTOR_MAP AXIS_Y_EXTERNAL +#define M2_STEP_ANGLE 1.8 +#define M2_TRAVEL_PER_REV (0.5 * 25.4) +#define M2_MICROSTEPS 10 +#define M2_POLARITY 0 +#define M2_POWER_MODE MOTOR_POWER_MODE +#define M2_POWER_LEVEL MOTOR_POWER_LEVEL + +#define M3_MOTOR_MAP AXIS_Z_EXTERNAL +#define M3_STEP_ANGLE 1.8 +#define M3_TRAVEL_PER_REV (0.5 * 25.4) +#define M3_MICROSTEPS 10 +#define M3_POLARITY 0 +#define M3_POWER_MODE MOTOR_POWER_MODE +#define M3_POWER_LEVEL MOTOR_POWER_LEVEL + +#define M4_MOTOR_MAP AXIS_A_EXTERNAL +#define M4_STEP_ANGLE 1.8 +#define M4_TRAVEL_PER_REV (0.5 * 25.4) +#define M4_MICROSTEPS 10 +#define M4_POLARITY 0 +#define M4_POWER_MODE MOTOR_POWER_MODE +#define M4_POWER_LEVEL MOTOR_POWER_LEVEL + +#if (MOTORS >= 5) +#define M5_MOTOR_MAP AXIS_B_EXTERNAL +#define M5_STEP_ANGLE 1.8 +#define M5_TRAVEL_PER_REV (0.5 * 25.4) +#define M5_MICROSTEPS 10 +#define M5_POLARITY 0 +#define M5_POWER_MODE MOTOR_POWER_MODE +#define M5_POWER_LEVEL MOTOR_POWER_LEVEL + +#define M6_MOTOR_MAP AXIS_C_EXTERNAL +#define M6_STEP_ANGLE 1.8 +#define M6_TRAVEL_PER_REV (0.5 * 25.4) +#define M6_MICROSTEPS 10 +#define M6_POLARITY 0 +#define M6_POWER_MODE MOTOR_POWER_MODE +#define M6_POWER_LEVEL MOTOR_POWER_LEVEL +#endif + // *** axis settings ********************************************************************************* -#define X_AXIS_MODE AXIS_STANDARD // xam see canonical_machine.h cmAxisMode for valid values -#define X_VELOCITY_MAX (360 * 25.4) // xvm G0 max velocity in mm/min -#define X_FEEDRATE_MAX X_VELOCITY_MAX // xfr G1 max feed rate in mm/min -#define X_TRAVEL_MIN 0 // xtn minimum travel for soft limits -#define X_TRAVEL_MAX (25 * 25.4) // xtm travel between switches or crashes -#define X_JERK_MAX (2 * 25.4) // xjm jerk is multipled by 1,000,000 internally -#define X_JERK_HIGH_SPEED 10000 // xjh -//#define X_HOMING_INPUT 1 // xhi input used for homing or 0 to disable -#define X_HOMING_DIRECTION 0 // xhd 0=search moves negative, 1= search moves positive -#define X_SEARCH_VELOCITY (60 * 25.4) // xsv minus means move to minimum switch -#define X_LATCH_VELOCITY (30 * 25.4) // xlv mm/min -#define X_LATCH_BACKOFF (0.125 * 25.4) // xlb mm -#define X_ZERO_BACKOFF (0.375 * 25.4) // xzb mm -#define X_HOMING_INPUT 0 -#define X_HOMING_DIR 0 +#define X_AXIS_MODE AXIS_STANDARD // xam see canonical_machine.h cmAxisMode for valid values +#define X_VELOCITY_MAX (360 * 25.4) // xvm G0 max velocity in mm/min +#define X_FEEDRATE_MAX X_VELOCITY_MAX // xfr G1 max feed rate in mm/min +#define X_TRAVEL_MIN 0 // xtn minimum travel for soft limits +#define X_TRAVEL_MAX (25 * 25.4) // xtm travel between switches or crashes +#define X_JERK_MAX (2 * 25.4) // xjm jerk is multiplied by 1,000,000 internally +#define X_JERK_HIGH_SPEED 10000 // xjh +#define X_HOMING_INPUT 1 // xhi input used for homing or 0 to disable +#define X_HOMING_DIRECTION 0 // xhd 0=search moves negative, 1= search moves positive +#define X_SEARCH_VELOCITY (60 * 25.4) // xsv minus means move to minimum switch +#define X_LATCH_VELOCITY (30 * 25.4) // xlv mm/min +#define X_LATCH_BACKOFF (0.125 * 25.4) // xlb mm +#define X_ZERO_BACKOFF (0.375 * 25.4) // xzb mm -#define Y_AXIS_MODE AXIS_STANDARD -#define Y_VELOCITY_MAX (360 * 25.4) -#define Y_FEEDRATE_MAX Y_VELOCITY_MAX -#define Y_TRAVEL_MIN 0 -#define Y_TRAVEL_MAX (19 * 25.4) -#define Y_JERK_MAX (2 * 25.4) -#define Y_JERK_HIGH_SPEED 10000 -#define Y_HOMING_INPUT 3 -#define Y_HOMING_DIRECTION 0 -#define Y_SEARCH_VELOCITY (60 * 25.4) -#define Y_LATCH_VELOCITY (30 * 25.4) -#define Y_LATCH_BACKOFF (0.125 * 25.4) -#define Y_ZERO_BACKOFF (0.375 * 25.4) -//#define Y_HOMING_INPUT 0 -#define Y_HOMING_DIR 0 +#define Y_AXIS_MODE AXIS_STANDARD +#define Y_VELOCITY_MAX (360 * 25.4) +#define Y_FEEDRATE_MAX Y_VELOCITY_MAX +#define Y_TRAVEL_MIN 0 +#define Y_TRAVEL_MAX (19 * 25.4) +#define Y_JERK_MAX (2 * 25.4) +#define Y_JERK_HIGH_SPEED 10000 +#define Y_HOMING_INPUT 3 +#define Y_HOMING_DIRECTION 0 +#define Y_SEARCH_VELOCITY (60 * 25.4) +#define Y_LATCH_VELOCITY (30 * 25.4) +#define Y_LATCH_BACKOFF (0.125 * 25.4) +#define Y_ZERO_BACKOFF (0.375 * 25.4) +#define Z_AXIS_MODE AXIS_STANDARD +#define Z_VELOCITY_MAX (360 * 25.4) +#define Z_FEEDRATE_MAX Z_VELOCITY_MAX +#define Z_TRAVEL_MAX (6.5 * 25.4) +#define Z_TRAVEL_MIN 0 +#define Z_JERK_MAX (2 * 25.4) +#define Z_JERK_HIGH_SPEED 1000 +#define Z_HOMING_INPUT 6 +#define Z_HOMING_DIRECTION 1 +#define Z_SEARCH_VELOCITY (60 * 25.4) +#define Z_LATCH_VELOCITY (30 * 25.4) +#define Z_LATCH_BACKOFF (0.125 * 25.4) +#define Z_ZERO_BACKOFF (0.375 * 25.4) -#define Z_AXIS_MODE AXIS_STANDARD -#define Z_VELOCITY_MAX (360 * 25.4) -#define Z_FEEDRATE_MAX Z_VELOCITY_MAX -#define Z_TRAVEL_MAX (6.5 * 25.4) -#define Z_TRAVEL_MIN 0 -#define Z_JERK_MAX (2 * 25.4) -#define Z_JERK_HIGH_SPEED 1000 -#define Z_HOMING_INPUT 6 -#define Z_HOMING_DIRECTION 1 -#define Z_SEARCH_VELOCITY (60 * 25.4) -#define Z_LATCH_VELOCITY (30 * 25.4) -#define Z_LATCH_BACKOFF (0.125 * 25.4) -#define Z_ZERO_BACKOFF (0.375 * 25.4) -//#define Z_HOMING_INPUT 0 -#define Z_HOMING_DIR 0 +#define A_AXIS_MODE AXIS_STANDARD +#define A_VELOCITY_MAX (360 * 25.4) +#define A_FEEDRATE_MAX 48000 +#define A_TRAVEL_MIN -1 // degrees +#define A_TRAVEL_MAX -1 // same value means infinite, no limit +#define A_JERK_MAX (2 * 25.4) +#define A_JERK_HIGH_SPEED A_JERK_MAX +#define A_RADIUS 1.0 +#define A_HOMING_INPUT 0 +#define A_HOMING_DIRECTION 0 +#define A_SEARCH_VELOCITY (60 * 25.4) +#define A_LATCH_VELOCITY (30 * 25.4) +#define A_LATCH_BACKOFF (0.125 * 25.4) +#define A_ZERO_BACKOFF (0.375 * 25.4) +#define B_AXIS_MODE AXIS_DISABLED +#define B_VELOCITY_MAX (360 * 25.4) +#define B_FEEDRATE_MAX B_VELOCITY_MAX +#define B_TRAVEL_MAX -1 +#define B_TRAVEL_MIN -1 +#define B_JERK_MAX (2 * 25.4) +#define B_JERK_HIGH_SPEED B_JERK_MAX +#define B_RADIUS 1 +#define B_HOMING_INPUT 0 +#define B_HOMING_DIRECTION 0 +#define B_SEARCH_VELOCITY (60 * 25.4) +#define B_LATCH_VELOCITY (30 * 25.4) +#define B_LATCH_BACKOFF (0.125 * 25.4) +#define B_ZERO_BACKOFF (0.375 * 25.4) -#define A_AXIS_MODE AXIS_STANDARD -#define A_VELOCITY_MAX (360 * 25.4) -#define A_FEEDRATE_MAX 48000 -#define A_TRAVEL_MIN -1 // degrees -#define A_TRAVEL_MAX -1 // same value means infinite, no limit -#define A_JERK_MAX (2 * 25.4) -#define A_JERK_HIGH_SPEED A_JERK_MAX -#define A_RADIUS 1.0 -#define A_HOMING_INPUT 0 -#define A_HOMING_DIRECTION 0 -#define A_SEARCH_VELOCITY (60 * 25.4) -#define A_LATCH_VELOCITY (30 * 25.4) -#define A_LATCH_BACKOFF (0.125 * 25.4) -#define A_ZERO_BACKOFF (0.375 * 25.4) -#define A_HOMING_INPUT 0 -#define A_HOMING_DIR 0 - -#define B_AXIS_MODE AXIS_DISABLED -#define B_VELOCITY_MAX (360 * 25.4) -#define B_FEEDRATE_MAX B_VELOCITY_MAX -#define B_TRAVEL_MAX -1 -#define B_TRAVEL_MIN -1 -#define B_JERK_MAX (2 * 25.4) -#define B_JERK_HIGH_SPEED B_JERK_MAX -#define B_RADIUS 1 -#define B_HOMING_INPUT 0 -#define B_HOMING_DIRECTION 0 -#define B_SEARCH_VELOCITY (60 * 25.4) -#define B_LATCH_VELOCITY (30 * 25.4) -#define B_LATCH_BACKOFF (0.125 * 25.4) -#define B_ZERO_BACKOFF (0.375 * 25.4) -#define B_HOMING_INPUT 0 -#define B_HOMING_DIR 0 - - -#define C_AXIS_MODE AXIS_DISABLED -#define C_VELOCITY_MAX (360 * 25.4) -#define C_FEEDRATE_MAX C_VELOCITY_MAX -#define C_TRAVEL_MAX -1 -#define C_TRAVEL_MIN -1 -#define C_JERK_MAX (2 * 25.4) -#define C_JERK_HIGH_SPEED C_JERK_MAX -#define C_RADIUS 1 -#define C_HOMING_INPUT 0 -#define C_HOMING_DIRECTION 0 -#define C_SEARCH_VELOCITY (60 * 25.4) -#define C_LATCH_VELOCITY (30 * 25.4) -#define C_LATCH_BACKOFF (0.125 * 25.4) -#define C_ZERO_BACKOFF (0.375 * 25.4) -#define C_HOMING_INPUT 0 -#define C_HOMING_DIR 0 +#define C_AXIS_MODE AXIS_DISABLED +#define C_VELOCITY_MAX (360 * 25.4) +#define C_FEEDRATE_MAX C_VELOCITY_MAX +#define C_TRAVEL_MAX -1 +#define C_TRAVEL_MIN -1 +#define C_JERK_MAX (2 * 25.4) +#define C_JERK_HIGH_SPEED C_JERK_MAX +#define C_RADIUS 1 +#define C_HOMING_INPUT 0 +#define C_HOMING_DIRECTION 0 +#define C_SEARCH_VELOCITY (60 * 25.4) +#define C_LATCH_VELOCITY (30 * 25.4) +#define C_LATCH_BACKOFF (0.125 * 25.4) +#define C_ZERO_BACKOFF (0.375 * 25.4) //*** Input / output settings *** @@ -316,7 +295,6 @@ constexpr float H3_DEFAULT_D = 100.0; #define DI9_ACTION INPUT_ACTION_NONE #define DI9_FUNCTION INPUT_FUNCTION_NONE - // Extruder1_PWM #define DO1_MODE IO_ACTIVE_HIGH // Extruder2_PWM @@ -340,80 +318,14 @@ constexpr float H3_DEFAULT_D = 100.0; /*** Handle optional modules that may not be in every machine ***/ -#define P1_PWM_FREQUENCY 100 // in Hz -#define P1_CW_SPEED_LO 7900 // in RPM (arbitrary units) -#define P1_CW_SPEED_HI 12800 -#define P1_CW_PHASE_LO 0.13 // phase [0..1] -#define P1_CW_PHASE_HI 0.17 -#define P1_CCW_SPEED_LO 0 -#define P1_CCW_SPEED_HI 0 -#define P1_CCW_PHASE_LO 0.1 -#define P1_CCW_PHASE_HI 0.1 -#define P1_PWM_PHASE_OFF 0.1 +#define P1_PWM_FREQUENCY 100 // in Hz +#define P1_CW_SPEED_LO 7900 // in RPM (arbitrary units) +#define P1_CW_SPEED_HI 12800 +#define P1_CW_PHASE_LO 0.13 // phase [0..1] +#define P1_CW_PHASE_HI 0.17 +#define P1_CCW_SPEED_LO 0 +#define P1_CCW_SPEED_HI 0 +#define P1_CCW_PHASE_LO 0.1 +#define P1_CCW_PHASE_HI 0.1 +#define P1_PWM_PHASE_OFF 0.1 -// *** DEFAULT COORDINATE SYSTEM OFFSETS *** -// Our convention is: -// - leave G54 in machine coordinates to act as a persistent absolute coordinate system -// - set G55 to be a zero in the middle of the table -// - no action for the others - -#define G54_X_OFFSET 0 // G54 is traditionally set to all zeros -#define G54_Y_OFFSET 0 -#define G54_Z_OFFSET 0 -#define G54_A_OFFSET 0 -#define G54_B_OFFSET 0 -#define G54_C_OFFSET 0 - -#define G55_X_OFFSET 0 // use (X_TRAVEL_MAX/2) to set g55 to middle of table -#define G55_Y_OFFSET 0 // use (Y_TRAVEL_MAX/2) to set g55 to middle of table -#define G55_Z_OFFSET 0 -#define G55_A_OFFSET 0 -#define G55_B_OFFSET 0 -#define G55_C_OFFSET 0 - -#define G56_X_OFFSET 0 -#define G56_Y_OFFSET 0 -#define G56_Z_OFFSET 0 -#define G56_A_OFFSET 0 -#define G56_B_OFFSET 0 -#define G56_C_OFFSET 0 - -#define G57_X_OFFSET 0 -#define G57_Y_OFFSET 0 -#define G57_Z_OFFSET 0 -#define G57_A_OFFSET 0 -#define G57_B_OFFSET 0 -#define G57_C_OFFSET 0 - -#define G58_X_OFFSET 0 -#define G58_Y_OFFSET 0 -#define G58_Z_OFFSET 0 -#define G58_A_OFFSET 0 -#define G58_B_OFFSET 0 -#define G58_C_OFFSET 0 - -#define G59_X_OFFSET 0 -#define G59_Y_OFFSET 0 -#define G59_Z_OFFSET 0 -#define G59_A_OFFSET 0 -#define G59_B_OFFSET 0 -#define G59_C_OFFSET 0 - -/*** User-Defined Data Defaults ***/ - -#define USER_DATA_A0 0 -#define USER_DATA_A1 0 -#define USER_DATA_A2 0 -#define USER_DATA_A3 0 -#define USER_DATA_B0 0 -#define USER_DATA_B1 0 -#define USER_DATA_B2 0 -#define USER_DATA_B3 0 -#define USER_DATA_C0 0 -#define USER_DATA_C1 0 -#define USER_DATA_C2 0 -#define USER_DATA_C3 0 -#define USER_DATA_D0 0 -#define USER_DATA_D1 0 -#define USER_DATA_D2 0 -#define USER_DATA_D3 0 diff --git a/g2core/settings/settings_shopbot_test.h b/g2core/settings/settings_shopbot_test.h index 76caf26b..95630c53 100644 --- a/g2core/settings/settings_shopbot_test.h +++ b/g2core/settings/settings_shopbot_test.h @@ -1,8 +1,8 @@ /* - * settings_shapeoko375.h - Shopbot Test + * settings_shopbot_test.h - Shopbot Test * This file is part of the g2core project * - * Copyright (c) 2015 - 2016 Alden S. Hart, Jr. + * Copyright (c) 2015 - 2018 Alden S. Hart, Jr. * * This file ("the software") is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License, version 2 as published by the @@ -39,208 +39,203 @@ //**** GLOBAL / GENERAL SETTINGS ****************************************************** -#define JUNCTION_INTEGRATION_TIME (5000 * 25.4) // cornering - usually between 0.5 and 2.0 (higher is faster) -#define CHORDAL_TOLERANCE 0.001 // chordal accuracy for arc drawing (in mm) +#define JUNCTION_INTEGRATION_TIME 0.8 // cornering - between 0.10 and 2.00 (higher is faster) +#define CHORDAL_TOLERANCE 0.0001 // chordal accuracy for arc drawing (in mm) -#define SOFT_LIMIT_ENABLE 0 // 0=off, 1=on -#define HARD_LIMIT_ENABLE 1 // 0=off, 1=on -#define SAFETY_INTERLOCK_ENABLE 1 // 0=off, 1=on +#define SOFT_LIMIT_ENABLE 0 // 0=off, 1=on +#define HARD_LIMIT_ENABLE 0 // 0=off, 1=on +#define SAFETY_INTERLOCK_ENABLE 0 // 0=off, 1=on -#define SPINDLE_ENABLE_POLARITY 1 // 0=active low, 1=active high -#define SPINDLE_DIR_POLARITY 0 // 0=clockwise is low, 1=clockwise is high -#define SPINDLE_PAUSE_ON_HOLD true -#define SPINDLE_DWELL_TIME 1.0 +#define SPINDLE_ENABLE_POLARITY 1 // 0=active low, 1=active high +#define SPINDLE_DIR_POLARITY 0 // 0=clockwise is low, 1=clockwise is high +#define SPINDLE_PAUSE_ON_HOLD true +#define SPINDLE_DWELL_TIME 1.0 -#define COOLANT_MIST_POLARITY 1 // 0=active low, 1=active high -#define COOLANT_FLOOD_POLARITY 1 // 0=active low, 1=active high -#define COOLANT_PAUSE_ON_HOLD true +#define COOLANT_MIST_POLARITY 1 // 0=active low, 1=active high +#define COOLANT_FLOOD_POLARITY 1 // 0=active low, 1=active high +#define COOLANT_PAUSE_ON_HOLD true + +#define FEEDHOLD_Z_LIFT (0.5 * 25.4) + +#define MANUAL_FEEDRATE_OVERRIDE_ENABLE false +#define MANUAL_FEEDRATE_OVERRIDE_PARAMETER 1.00 // Communications and reporting settings -#define USB_SERIAL_PORTS_EXPOSED 2 // Valid options are 1 or 2, only! +#define USB_SERIAL_PORTS_EXPOSED 1 // Valid options are 1 or 2, only! -#define COMM_MODE JSON_MODE // one of: TEXT_MODE, JSON_MODE -#define XIO_ENABLE_FLOW_CONTROL FLOW_CONTROL_RTS // FLOW_CONTROL_OFF, FLOW_CONTROL_RTS +#define COMM_MODE JSON_MODE // one of: TEXT_MODE, JSON_MODE +#define XIO_ENABLE_FLOW_CONTROL FLOW_CONTROL_RTS // FLOW_CONTROL_OFF, FLOW_CONTROL_RTS -#define TEXT_VERBOSITY TV_VERBOSE // one of: TV_SILENT, TV_VERBOSE -#define JSON_VERBOSITY JV_MESSAGES // one of: JV_SILENT, JV_FOOTER, JV_CONFIGS, JV_MESSAGES, JV_LINENUM, JV_VERBOSE -#define QUEUE_REPORT_VERBOSITY QR_OFF // one of: QR_OFF, QR_SINGLE, QR_TRIPLE +#define TEXT_VERBOSITY TV_VERBOSE // one of: TV_SILENT, TV_VERBOSE +#define JSON_VERBOSITY JV_VERBOSE // one of: JV_SILENT, JV_FOOTER, JV_CONFIGS, JV_MESSAGES, JV_LINENUM, JV_VERBOSE +#define QUEUE_REPORT_VERBOSITY QR_OFF // one of: QR_OFF, QR_SINGLE, QR_TRIPLE -#define STATUS_REPORT_VERBOSITY SR_FILTERED // one of: SR_OFF, SR_FILTERED, SR_VERBOSE -#define STATUS_REPORT_MIN_MS 200 // milliseconds - enforces a viable minimum -#define STATUS_REPORT_INTERVAL_MS 250 // milliseconds - set $SV=0 to disable -//#define STATUS_REPORT_DEFAULTS "posx","posy","posz","posa","posb","vel","stat","hold","line","coor" -//#define STATUS_REPORT_DEFAULTS -//"posx","posy","posz","posa","posb","vel","stat","macs","cycs","mots","hold","line","coor" -#define STATUS_REPORT_DEFAULTS \ - "posx", "posy", "posz", "posa", "vel", "feed", "stat", "macs", "cycs", "mots", "hold", "in1", "in2", "in3", "in4", \ - "in5", "in6", "in7", "in8", "in9" +#define STATUS_REPORT_VERBOSITY SR_FILTERED // one of: SR_OFF, SR_FILTERED, SR_VERBOSE +#define STATUS_REPORT_MIN_MS 200 // milliseconds - enforces a viable minimum +#define STATUS_REPORT_INTERVAL_MS 250 // milliseconds + +#define STATUS_REPORT_DEFAULTS "posx", "posy", "posz", "posa", "posb", \ + "vel", "stat", "hold", "line", "coor", "unit", \ + "in1", "in2", "in3", "in4", "in5", "in6", "in7", "in8" // "home","homx","homy","homz" -// "line","vel","feed","stat","macs","cycs","mots","hold","unit", // Gcode startup defaults -#define GCODE_DEFAULT_UNITS INCHES // MILLIMETERS or INCHES -#define GCODE_DEFAULT_PLANE CANON_PLANE_XY // CANON_PLANE_XY, CANON_PLANE_XZ, or CANON_PLANE_YZ -#define GCODE_DEFAULT_COORD_SYSTEM G55 // G54, G55, G56, G57, G58 or G59 -#define GCODE_DEFAULT_PATH_CONTROL PATH_CONTINUOUS +#define GCODE_DEFAULT_UNITS INCHES // MILLIMETERS or INCHES +#define GCODE_DEFAULT_PLANE CANON_PLANE_XY // CANON_PLANE_XY, CANON_PLANE_XZ, or CANON_PLANE_YZ +#define GCODE_DEFAULT_COORD_SYSTEM G55 // G54, G55, G56, G57, G58 or G59 +#define GCODE_DEFAULT_PATH_CONTROL PATH_CONTINUOUS #define GCODE_DEFAULT_DISTANCE_MODE ABSOLUTE_DISTANCE_MODE -constexpr float H1_DEFAULT_P = 7.0; -constexpr float H1_DEFAULT_I = 0.2; -constexpr float H1_DEFAULT_D = 100.0; - -constexpr float H2_DEFAULT_P = 7.0; -constexpr float H2_DEFAULT_I = 0.2; -constexpr float H2_DEFAULT_D = 100.0; - -constexpr float H3_DEFAULT_P = 7.0; -constexpr float H3_DEFAULT_I = 0.2; -constexpr float H3_DEFAULT_D = 100.0; - // *** motor settings ************************************************************************************ -#define MOTOR_POWER_MODE MOTOR_POWERED_IN_CYCLE // default motor power mode (see cmMotorPowerMode in stepper.h) -#define MOTOR_POWER_TIMEOUT 2.00 // motor power timeout in seconds -#define MOTOR_POWER_LEVEL 0.375 // default motor power level 0.00 - 1.00 +#define MOTOR_POWER_MODE MOTOR_POWERED_IN_CYCLE // default motor power mode (see cmMotorPowerMode in stepper.h) +#define MOTOR_POWER_TIMEOUT 2.00 // motor power timeout in seconds +#define MOTOR_POWER_LEVEL 0.375 // default motor power level 0.00 - 1.00 -#define M1_MOTOR_MAP AXIS_X // 1ma -#define M1_STEP_ANGLE 1.8 // 1sa -#define M1_TRAVEL_PER_REV (0.5 * 25.4) // 1tr -#define M1_MICROSTEPS 16 // 1mi 1,2,4,8 -#define M1_POLARITY 0 // 1po 0=normal, 1=reversed -#define M1_POWER_MODE MOTOR_POWER_MODE // 1pm TRUE=low power idle enabled -#define M1_POWER_LEVEL MOTOR_POWER_LEVEL // 1pl Irrelevant to Shopbot sbv300 +#define M1_MOTOR_MAP AXIS_X_EXTERNAL // 1ma +#define M1_STEP_ANGLE 1.8 // 1sa +#define M1_TRAVEL_PER_REV (0.5 * 25.4) // 1tr +#define M1_MICROSTEPS 8 // 1mi 1,2,4,8 +#define M1_POLARITY 1 // 1po 0=normal, 1=reversed +#define M1_POWER_MODE MOTOR_POWER_MODE // 1pm TRUE=low power idle enabled +#define M1_POWER_LEVEL MOTOR_POWER_LEVEL // 1pl Irrelevant to Shopbot sbv300 -#define M2_MOTOR_MAP AXIS_Y -#define M2_STEP_ANGLE 1.8 -#define M2_TRAVEL_PER_REV (0.5 * 25.4) -#define M2_MICROSTEPS 16 -#define M2_POLARITY 0 -#define M2_POWER_MODE MOTOR_POWER_MODE -#define M2_POWER_LEVEL MOTOR_POWER_LEVEL +#define M2_MOTOR_MAP AXIS_Y_EXTERNAL +#define M2_STEP_ANGLE 1.8 +#define M2_TRAVEL_PER_REV (0.5 * 25.4) +#define M2_MICROSTEPS 8 +#define M2_POLARITY 1 +#define M2_POWER_MODE MOTOR_POWER_MODE +#define M2_POWER_LEVEL MOTOR_POWER_LEVEL -#define M3_MOTOR_MAP AXIS_Z -#define M3_STEP_ANGLE 1.8 -#define M3_TRAVEL_PER_REV (0.5 * 25.4) -#define M3_MICROSTEPS 16 -#define M3_POLARITY 0 -#define M3_POWER_MODE MOTOR_POWER_MODE -#define M3_POWER_LEVEL MOTOR_POWER_LEVEL +#define M3_MOTOR_MAP AXIS_Z_EXTERNAL +#define M3_STEP_ANGLE 1.8 +#define M3_TRAVEL_PER_REV (0.5 * 25.4) +#define M3_MICROSTEPS 8 +#define M3_POLARITY 0 +#define M3_POWER_MODE MOTOR_POWER_MODE +#define M3_POWER_LEVEL MOTOR_POWER_LEVEL -#define M4_MOTOR_MAP AXIS_A -#define M4_STEP_ANGLE 1.8 -#define M4_TRAVEL_PER_REV (0.5 * 25.4) -#define M4_MICROSTEPS 16 -#define M4_POLARITY 0 -#define M4_POWER_MODE MOTOR_POWER_MODE -#define M4_POWER_LEVEL MOTOR_POWER_LEVEL +#define M4_MOTOR_MAP AXIS_A_EXTERNAL +#define M4_STEP_ANGLE 1.8 +#define M4_TRAVEL_PER_REV 48 // was (0.5 * 25.4) Set to agree with sbv300 su = 33.333333 +#define M4_MICROSTEPS 8 +#define M4_POLARITY 0 +#define M4_POWER_MODE MOTOR_POWER_MODE +#define M4_POWER_LEVEL MOTOR_POWER_LEVEL -#if (MOTORS >= 5) -#define M5_MOTOR_MAP AXIS_B -#define M5_STEP_ANGLE 1.8 -#define M5_TRAVEL_PER_REV (0.5 * 25.4) -#define M5_MICROSTEPS 16 -#define M5_POLARITY 0 -#define M5_POWER_MODE MOTOR_POWER_MODE -#define M5_POWER_LEVEL MOTOR_POWER_LEVEL +#define M5_MOTOR_MAP AXIS_B_EXTERNAL +#define M5_STEP_ANGLE 1.8 +#define M5_TRAVEL_PER_REV 48 // (0.5 * 25.4) +#define M5_MICROSTEPS 8 +#define M5_POLARITY 0 +#define M5_POWER_MODE MOTOR_POWER_MODE +#define M5_POWER_LEVEL MOTOR_POWER_LEVEL -#define M6_MOTOR_MAP AXIS_C -#define M6_STEP_ANGLE 1.8 -#define M6_TRAVEL_PER_REV (0.5 * 25.4) -#define M6_MICROSTEPS 16 -#define M6_POLARITY 0 -#define M6_POWER_MODE MOTOR_POWER_MODE -#define M6_POWER_LEVEL MOTOR_POWER_LEVEL -#endif +#define M6_MOTOR_MAP AXIS_C_EXTERNAL +#define M6_STEP_ANGLE 1.8 +#define M6_TRAVEL_PER_REV 48 // (0.5 * 25.4) +#define M6_MICROSTEPS 8 +#define M6_POLARITY 0 +#define M6_POWER_MODE MOTOR_POWER_MODE +#define M6_POWER_LEVEL MOTOR_POWER_LEVEL // *** axis settings ********************************************************************************* -#define X_AXIS_MODE AXIS_STANDARD // xam see canonical_machine.h cmAxisMode for valid values -#define X_VELOCITY_MAX (360 * 25.4) // xvm G0 max velocity in mm/min -#define X_FEEDRATE_MAX X_VELOCITY_MAX // xfr G1 max feed rate in mm/min -#define X_TRAVEL_MIN 0 // xtn minimum travel for soft limits -#define X_TRAVEL_MAX (25 * 25.4) // xtm travel between switches or crashes -#define X_JERK_MAX (2 * 25.4) // xjm jerk is multipled by 1,000,000 internally -#define X_JERK_HIGH_SPEED 10000 // xjh -#define X_HOMING_INPUT 1 // xhi input used for homing or 0 to disable -#define X_HOMING_DIRECTION 0 // xhd 0=search moves negative, 1= search moves positive -#define X_SEARCH_VELOCITY (60 * 25.4) // xsv minus means move to minimum switch -#define X_LATCH_VELOCITY (30 * 25.4) // xlv mm/min -#define X_LATCH_BACKOFF (0.125 * 25.4) // xlb mm -#define X_ZERO_BACKOFF (0.375 * 25.4) // xzb mm +// Axis values are set to make the Othermachine behave like a smaller Handibot +// Units are not to scale: i.e. G0 X1 travels approx 0.34" instead of 1" -#define Y_AXIS_MODE AXIS_STANDARD -#define Y_VELOCITY_MAX (360 * 25.4) -#define Y_FEEDRATE_MAX Y_VELOCITY_MAX -#define Y_TRAVEL_MIN 0 -#define Y_TRAVEL_MAX (19 * 25.4) -#define Y_JERK_MAX (2 * 25.4) -#define Y_JERK_HIGH_SPEED 10000 -#define Y_HOMING_INPUT 3 -#define Y_HOMING_DIRECTION 0 -#define Y_SEARCH_VELOCITY (60 * 25.4) -#define Y_LATCH_VELOCITY (30 * 25.4) -#define Y_LATCH_BACKOFF (0.125 * 25.4) -#define Y_ZERO_BACKOFF (0.375 * 25.4) +#define LINEAR_JERK_MAX 50 // value set by sbv300 initialization sequence +//#define LINEAR_JERK_MAX 25.4 // test value +#define ROTARY_JERK_MAX 6000 // sbv300 initialization value -#define Z_AXIS_MODE AXIS_STANDARD -#define Z_VELOCITY_MAX (360 * 25.4) -#define Z_FEEDRATE_MAX Z_VELOCITY_MAX -#define Z_TRAVEL_MAX (6.5 * 25.4) -#define Z_TRAVEL_MIN 0 -#define Z_JERK_MAX (2 * 25.4) -#define Z_JERK_HIGH_SPEED 1000 -#define Z_HOMING_INPUT 6 -#define Z_HOMING_DIRECTION 1 -#define Z_SEARCH_VELOCITY (60 * 25.4) -#define Z_LATCH_VELOCITY (30 * 25.4) -#define Z_LATCH_BACKOFF (0.125 * 25.4) -#define Z_ZERO_BACKOFF (0.375 * 25.4) +#define X_AXIS_MODE AXIS_STANDARD // xam see canonical_machine.h cmAxisMode for valid values +#define X_VELOCITY_MAX (100 * 25.4) // xvm G0 max velocity in mm/min (was 360) +#define X_FEEDRATE_MAX X_VELOCITY_MAX // xfr G1 max feed rate in mm/min +#define X_TRAVEL_MIN 0 // xtn minimum travel for soft limits +#define X_TRAVEL_MAX (14 * 25.4) // xtm travel between switches or crashes (was 25) +#define X_JERK_MAX LINEAR_JERK_MAX // xjm +#define X_JERK_HIGH_SPEED 10000 // xjh +#define X_HOMING_INPUT 1 // xhi input used for homing or 0 to disable +#define X_HOMING_DIRECTION 0 // xhd 0=search moves negative, 1= search moves positive +#define X_SEARCH_VELOCITY (60 * 25.4) // xsv minus means move to minimum switch +#define X_LATCH_VELOCITY (30 * 25.4) // xlv mm/min +#define X_LATCH_BACKOFF (0.125 * 25.4) // xlb mm +#define X_ZERO_BACKOFF (0.250 * 25.4) // xzb mm was 0.375 -#define A_AXIS_MODE AXIS_STANDARD -#define A_VELOCITY_MAX (360 * 25.4) -#define A_FEEDRATE_MAX 48000 -#define A_TRAVEL_MIN -1 // degrees -#define A_TRAVEL_MAX -1 // same value means infinite, no limit -#define A_JERK_MAX (2 * 25.4) -#define A_JERK_HIGH_SPEED A_JERK_MAX -#define A_RADIUS 1.0 -#define A_HOMING_INPUT 0 -#define A_HOMING_DIRECTION 0 -#define A_SEARCH_VELOCITY (60 * 25.4) -#define A_LATCH_VELOCITY (30 * 25.4) -#define A_LATCH_BACKOFF (0.125 * 25.4) -#define A_ZERO_BACKOFF (0.375 * 25.4) +#define Y_AXIS_MODE AXIS_STANDARD +#define Y_VELOCITY_MAX (100 * 25.4) // was 360 +#define Y_FEEDRATE_MAX Y_VELOCITY_MAX +#define Y_TRAVEL_MIN 0 +#define Y_TRAVEL_MAX (11.5 * 25.4) // was 19 +#define Y_JERK_MAX LINEAR_JERK_MAX +#define Y_JERK_HIGH_SPEED 10000 +#define Y_HOMING_INPUT 3 +#define Y_HOMING_DIRECTION 0 +#define Y_SEARCH_VELOCITY (60 * 25.4) +#define Y_LATCH_VELOCITY (30 * 25.4) +#define Y_LATCH_BACKOFF (0.125 * 25.4) +#define Y_ZERO_BACKOFF (0.250 * 25.4) // was 0.375 -#define B_AXIS_MODE AXIS_DISABLED -#define B_VELOCITY_MAX (360 * 25.4) -#define B_FEEDRATE_MAX B_VELOCITY_MAX -#define B_TRAVEL_MAX -1 -#define B_TRAVEL_MIN -1 -#define B_JERK_MAX (2 * 25.4) -#define B_JERK_HIGH_SPEED B_JERK_MAX -#define B_RADIUS 1 -#define B_HOMING_INPUT 0 -#define B_HOMING_DIRECTION 0 -#define B_SEARCH_VELOCITY (60 * 25.4) -#define B_LATCH_VELOCITY (30 * 25.4) -#define B_LATCH_BACKOFF (0.125 * 25.4) -#define B_ZERO_BACKOFF (0.375 * 25.4) +#define Z_AXIS_MODE AXIS_STANDARD +#define Z_VELOCITY_MAX (100 * 25.4) // was 360 +#define Z_FEEDRATE_MAX Z_VELOCITY_MAX +#define Z_TRAVEL_MAX (6 * 25.4) // was 6.5 +#define Z_TRAVEL_MIN 0 +#define Z_JERK_MAX LINEAR_JERK_MAX +#define Z_JERK_HIGH_SPEED 1000 +#define Z_HOMING_INPUT 6 +#define Z_HOMING_DIRECTION 1 +#define Z_SEARCH_VELOCITY (60 * 25.4) +#define Z_LATCH_VELOCITY (30 * 25.4) +#define Z_LATCH_BACKOFF (0.125 * 25.4) +#define Z_ZERO_BACKOFF (0.250 * 25.4) // was 0.375 -#define C_AXIS_MODE AXIS_DISABLED -#define C_VELOCITY_MAX (360 * 25.4) -#define C_FEEDRATE_MAX C_VELOCITY_MAX -#define C_TRAVEL_MAX -1 -#define C_TRAVEL_MIN -1 -#define C_JERK_MAX (2 * 25.4) -#define C_JERK_HIGH_SPEED C_JERK_MAX -#define C_RADIUS 1 -#define C_HOMING_INPUT 0 -#define C_HOMING_DIRECTION 0 -#define C_SEARCH_VELOCITY (60 * 25.4) -#define C_LATCH_VELOCITY (30 * 25.4) -#define C_LATCH_BACKOFF (0.125 * 25.4) -#define C_ZERO_BACKOFF (0.375 * 25.4) +#define A_AXIS_MODE AXIS_STANDARD +#define A_VELOCITY_MAX (360 * 25.4) // is 15000 in sbv300 +#define A_FEEDRATE_MAX 48000 +#define A_TRAVEL_MIN -1 // degrees +#define A_TRAVEL_MAX -1 // same value means infinite, no limit +#define A_JERK_MAX ROTARY_JERK_MAX +#define A_JERK_HIGH_SPEED A_JERK_MAX +#define A_RADIUS 1.0 +#define A_HOMING_INPUT 0 +#define A_HOMING_DIRECTION 0 +#define A_SEARCH_VELOCITY (60 * 25.4) +#define A_LATCH_VELOCITY (30 * 25.4) +#define A_LATCH_BACKOFF (0.125 * 25.4) +#define A_ZERO_BACKOFF (0.375 * 25.4) + +#define B_AXIS_MODE AXIS_DISABLED +#define B_VELOCITY_MAX (360 * 25.4) +#define B_FEEDRATE_MAX B_VELOCITY_MAX +#define B_TRAVEL_MAX -1 +#define B_TRAVEL_MIN -1 +#define B_JERK_MAX ROTARY_JERK_MAX +#define B_JERK_HIGH_SPEED B_JERK_MAX +#define B_RADIUS 1 +#define B_HOMING_INPUT 0 +#define B_HOMING_DIRECTION 0 +#define B_SEARCH_VELOCITY (60 * 25.4) +#define B_LATCH_VELOCITY (30 * 25.4) +#define B_LATCH_BACKOFF (0.125 * 25.4) +#define B_ZERO_BACKOFF (0.375 * 25.4) + +#define C_AXIS_MODE AXIS_DISABLED +#define C_VELOCITY_MAX (360 * 25.4) +#define C_FEEDRATE_MAX C_VELOCITY_MAX +#define C_TRAVEL_MAX -1 +#define C_TRAVEL_MIN -1 +#define C_JERK_MAX ROTARY_JERK_MAX +#define C_JERK_HIGH_SPEED C_JERK_MAX +#define C_RADIUS 1 +#define C_HOMING_INPUT 0 +#define C_HOMING_DIRECTION 0 +#define C_SEARCH_VELOCITY (60 * 25.4) +#define C_LATCH_VELOCITY (30 * 25.4) +#define C_LATCH_BACKOFF (0.125 * 25.4) +#define C_ZERO_BACKOFF (0.375 * 25.4) //*** Input / output settings *** /* @@ -272,7 +267,7 @@ constexpr float H3_DEFAULT_D = 100.0; #define DI1_FUNCTION INPUT_FUNCTION_LIMIT // configured as limit switch. Also used for X homing // Xmax -#define DI2_MODE INPUT_MODE_DISABLED // testing disable +#define DI2_MODE IO_MODE_DISABLED // testing disable //#define DI2_MODE NORMALLY_CLOSED // configured as limit switch #define DI2_ACTION INPUT_ACTION_FAST_STOP #define DI2_FUNCTION INPUT_FUNCTION_LIMIT @@ -288,7 +283,7 @@ constexpr float H3_DEFAULT_D = 100.0; #define DI4_FUNCTION INPUT_FUNCTION_LIMIT // Zmin -#define DI5_MODE INPUT_ACTIVE_LOW // used for Z probe. Active closes circuit (active LO) +#define DI5_MODE IO_ACTIVE_LOW // used for Z probe. Active closes circuit (active LO) #define DI5_ACTION INPUT_ACTION_NONE #define DI5_FUNCTION INPUT_FUNCTION_NONE @@ -302,12 +297,12 @@ constexpr float H3_DEFAULT_D = 100.0; //#define DI6_FUNCTION INPUT_FUNCTION_LIMIT // Amin -#define DI7_MODE INPUT_ACTIVE_HIGH // interlock happens when NO switch goes HI (opens) -#define DI7_ACTION INPUT_ACTION_STOP // wired as interlock switch +#define DI7_MODE IO_ACTIVE_HIGH // interlock happens when NO switch goes HI (opens) +#define DI7_ACTION INPUT_ACTION_STOP // wired as interlock switch #define DI7_FUNCTION INPUT_FUNCTION_INTERLOCK // Amax -#define DI8_MODE INPUT_ACTIVE_LOW // configured as panic switch, NO +#define DI8_MODE IO_ACTIVE_LOW // configured as panic switch, NO #define DI8_ACTION INPUT_ACTION_PANIC #define DI8_FUNCTION INPUT_FUNCTION_NONE @@ -329,69 +324,3 @@ constexpr float H3_DEFAULT_D = 100.0; #define P1_CCW_PHASE_HI 0.1 #define P1_PWM_PHASE_OFF 0.1 -// *** DEFAULT COORDINATE SYSTEM OFFSETS *** -// Our convention is: -// - leave G54 in machine coordinates to act as a persistent absolute coordinate system -// - set G55 to be a zero in the middle of the table -// - no action for the others - -#define G54_X_OFFSET 0 // G54 is traditionally set to all zeros -#define G54_Y_OFFSET 0 -#define G54_Z_OFFSET 0 -#define G54_A_OFFSET 0 -#define G54_B_OFFSET 0 -#define G54_C_OFFSET 0 - -#define G55_X_OFFSET 0 -#define G55_Y_OFFSET 0 -#define G55_Z_OFFSET 0 -#define G55_A_OFFSET 0 -#define G55_B_OFFSET 0 -#define G55_C_OFFSET 0 - -#define G56_X_OFFSET 0 -#define G56_Y_OFFSET 0 -#define G56_Z_OFFSET 0 -#define G56_A_OFFSET 0 -#define G56_B_OFFSET 0 -#define G56_C_OFFSET 0 - -#define G57_X_OFFSET 0 -#define G57_Y_OFFSET 0 -#define G57_Z_OFFSET 0 -#define G57_A_OFFSET 0 -#define G57_B_OFFSET 0 -#define G57_C_OFFSET 0 - -#define G58_X_OFFSET 0 -#define G58_Y_OFFSET 0 -#define G58_Z_OFFSET 0 -#define G58_A_OFFSET 0 -#define G58_B_OFFSET 0 -#define G58_C_OFFSET 0 - -#define G59_X_OFFSET 0 -#define G59_Y_OFFSET 0 -#define G59_Z_OFFSET 0 -#define G59_A_OFFSET 0 -#define G59_B_OFFSET 0 -#define G59_C_OFFSET 0 - -/*** User-Defined Data Defaults ***/ - -#define USER_DATA_A0 0 -#define USER_DATA_A1 0 -#define USER_DATA_A2 0 -#define USER_DATA_A3 0 -#define USER_DATA_B0 0 -#define USER_DATA_B1 0 -#define USER_DATA_B2 0 -#define USER_DATA_B3 0 -#define USER_DATA_C0 0 -#define USER_DATA_C1 0 -#define USER_DATA_C2 0 -#define USER_DATA_C3 0 -#define USER_DATA_D0 0 -#define USER_DATA_D1 0 -#define USER_DATA_D2 0 -#define USER_DATA_D3 0 diff --git a/g2core/settings/settings_test.h b/g2core/settings/settings_test.h index 9a92a955..f7f436e6 100644 --- a/g2core/settings/settings_test.h +++ b/g2core/settings/settings_test.h @@ -2,7 +2,7 @@ * settings_test.h - settings for testing - subject to wild change * This file is part of the g2core project * - * Copyright (c) 2014 - 2016 Alden S. Hart Jr. + * Copyright (c) 2014 - 2018 Alden S. Hart Jr. * * This file ("the software") is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License, version 2 as published by the @@ -89,7 +89,7 @@ #define MOTOR_POWER_MODE MOTOR_POWERED_IN_CYCLE // default motor power mode (see cmMotorPowerMode in stepper.h) #define MOTOR_POWER_TIMEOUT 2.00 // motor power timeout in seconds -#define M1_MOTOR_MAP AXIS_X // 1ma +#define M1_MOTOR_MAP AXIS_X_EXTERNAL // 1ma #define M1_STEP_ANGLE 1.8 // 1sa #define M1_TRAVEL_PER_REV 5.08 // 1tr #define M1_MICROSTEPS 8 // 1mi 1,2,4,8 @@ -97,7 +97,7 @@ #define M1_POWER_MODE MOTOR_POWER_MODE // 1pm standard #define M1_POWER_LEVEL 0.75 -#define M2_MOTOR_MAP AXIS_Y +#define M2_MOTOR_MAP AXIS_Y_EXTERNAL #define M2_STEP_ANGLE 1.8 #define M2_TRAVEL_PER_REV 5.08 #define M2_MICROSTEPS 8 @@ -105,7 +105,7 @@ #define M2_POWER_MODE MOTOR_POWER_MODE #define M2_POWER_LEVEL 0.75 -#define M3_MOTOR_MAP AXIS_Z +#define M3_MOTOR_MAP AXIS_Z_EXTERNAL #define M3_STEP_ANGLE 1.8 #define M3_TRAVEL_PER_REV 2.1166666 #define M3_MICROSTEPS 8 @@ -113,7 +113,7 @@ #define M3_POWER_MODE MOTOR_POWER_MODE #define M3_POWER_LEVEL 0.50 -#define M4_MOTOR_MAP AXIS_A +#define M4_MOTOR_MAP AXIS_A_EXTERNAL #define M4_STEP_ANGLE 1.8 #define M4_TRAVEL_PER_REV 360 // degrees moved per motor rev #define M4_MICROSTEPS 32 diff --git a/g2core/settings/settings_zen7x12.h b/g2core/settings/settings_zen7x12.h old mode 100755 new mode 100644 index f167a484..df173256 --- a/g2core/settings/settings_zen7x12.h +++ b/g2core/settings/settings_zen7x12.h @@ -2,7 +2,7 @@ * settings_zen7x12.h - Zen Toolworks 7x12 machine profile * This file is part of the g2core project * - * Copyright (c) 2011 - 2016 Alden S. Hart, Jr. + * Copyright (c) 2011 - 2018 Alden S. Hart, Jr. * * This file ("the software") is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License, version 2 as published by the @@ -46,7 +46,7 @@ // *** motor settings *** -#define M1_MOTOR_MAP AXIS_X // 1ma +#define M1_MOTOR_MAP AXIS_X_EXTERNAL // 1ma #define M1_STEP_ANGLE 1.8 // 1sa #define M1_TRAVEL_PER_REV 1.25 // 1tr #define M1_MICROSTEPS 8 // 1mi 1,2,4,8 @@ -54,7 +54,7 @@ #define M1_POWER_MODE MOTOR_POWERED_IN_CYCLE // 1pm standard #define M1_POWER_LEVEL 0.5 // 1pl: 0.0=no power, 1.0=max power -#define M2_MOTOR_MAP AXIS_Y +#define M2_MOTOR_MAP AXIS_Y_EXTERNAL #define M2_STEP_ANGLE 1.8 #define M2_TRAVEL_PER_REV 1.25 #define M2_MICROSTEPS 8 @@ -62,7 +62,7 @@ #define M2_POWER_MODE MOTOR_POWERED_IN_CYCLE #define M2_POWER_LEVEL 0.5 -#define M3_MOTOR_MAP AXIS_Z +#define M3_MOTOR_MAP AXIS_Z_EXTERNAL #define M3_STEP_ANGLE 1.8 #define M3_TRAVEL_PER_REV 1.25 #define M3_MICROSTEPS 8 diff --git a/g2core/spindle.cpp b/g2core/spindle.cpp index a24891a9..ed47f2dc 100644 --- a/g2core/spindle.cpp +++ b/g2core/spindle.cpp @@ -2,7 +2,7 @@ * spindle.cpp - canonical machine spindle driver * This file is part of the g2core project * - * Copyright (c) 2010 - 2017 Alden S. Hart, Jr. + * Copyright (c) 2010 - 2019 Alden S. Hart, Jr. * * This file ("the software") is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License, version 2 as published by the @@ -39,7 +39,7 @@ /**** Allocate structures ****/ -cmSpindleton_t spindle; +spSpindle_t spindle; gpioDigitalOutput *spindle_enable_output = nullptr; @@ -55,16 +55,21 @@ gpioDigitalOutput *spindle_direction_output = nullptr; /**** Static functions ****/ -static void _exec_spindle_speed(float *value, bool *flag); -static void _exec_spindle_control(float *value, bool *flag); -static float _get_spindle_pwm (cmSpindleEnable enable, cmSpindleDir direction); +static float _get_spindle_pwm (spSpindle_t &_spindle, pwmControl_t &_pwm); -/* +#define SPINDLE_DIRECTION_ASSERT \ + if ((spindle.direction < SPINDLE_CW) || (spindle.direction > SPINDLE_CCW)) { \ + spindle.direction = SPINDLE_CW; \ + } + +/**************************************************************************************** * spindle_init() * spindle_reset() - stop spindle, set speed to zero, and reset values */ void spindle_init() { + SPINDLE_DIRECTION_ASSERT // spindle needs an initial direction + if (SPINDLE_ENABLE_OUTPUT_NUMBER > 0) { spindle_enable_output = d_out[SPINDLE_ENABLE_OUTPUT_NUMBER-1]; spindle_enable_output->setEnabled(IO_ENABLED); @@ -85,179 +90,267 @@ void spindle_init() void spindle_reset() { - float value[AXES] = { 0,0,0,0,0,0 }; // set spindle speed to zero - bool flags[] = { 1,0,0,0,0,0 }; - _exec_spindle_speed(value, flags); - cm_spindle_off_immediate(); // turn spindle off + spindle_speed_immediate(0); + spindle_control_immediate(SPINDLE_OFF); } -/* - * cm_set_spindle_speed() - queue the S parameter to the planner buffer - * _exec_spindle_speed() - spindle speed callback from planner queue +/**************************************************************************************** + * _exec_spindle_control() - actually execute the spindle command + * spindle_control_immediate() - execute spindle control immediately + * spindle_control_sync() - queue a spindle control to the planner buffer + * + * Basic operation: Spindle function is executed by _exec_spindle_control(). + * Spindle_control_immediate() performs the control as soon as it's received. + * Spindle_control_sync() inserts spindle move into the planner, and handles spinups. + * + * Valid inputs to Spindle_control_immediate() and Spindle_control_sync() are: + * + * - SPINDLE_OFF turns off spindle and sets spindle state to SPINDLE_OFF. + * This will also re-load enable and direction polarity to the pins if they have changed. + * The spindle.direction value is not affected (although this doesn't really matter). + * + * - SPINDLE_CW or SPINDLE_CCW turns sets direction accordingly and spindle on. + * In spindle_control_sync() a non-zero spinup delay runs a dwell immediately + * following the spindle change, but only if the planner had planned the spindle + * operation to zero. (I.e. if the spindle controls / S words do not plan to zero + * the delay is not run). Spindle_control_immediate() has no spinup delay or + * dwell behavior. + * + * - SPINDLE_PAUSE is only applicable to CW and CCW states. It forces the spindle OFF and + * sets spindle.state to PAUSE. A PAUSE received when not in CW or CCW state is ignored. + * + * - SPINDLE_RESUME, if in a PAUSE state, reverts to previous SPINDLE_CW or SPINDLE_CCW. + * The SPEED is not changed, and if it were changed in the interim the "new" speed + * is used. If RESUME is received from spindle_control_sync() the usual spinup delay + * behavior occurs. If RESUME is received when not in a PAUSED state it is ignored. + * This recognizes that the main reason an immediate command would be issued - either + * manually by the user or by an alarm or some other program function - is to stop + * a spindle. So the Resume should be ignored for safety. */ - -stat_t cm_set_spindle_speed(float speed) -{ -// if (speed > cfg.max_spindle speed) { return (STAT_MAX_SPINDLE_SPEED_EXCEEDED);} - - float value[AXES] = { speed, 0,0,0,0,0 }; - bool flags[] = { 1,0,0,0,0,0 }; - mp_queue_command(_exec_spindle_speed, value, flags); - return (STAT_OK); -} - -static void _exec_spindle_speed(float *value, bool *flag) -{ - if (flag[0]) { - spindle.speed = value[0]; - } - - if (flag[1]) { - spindle.direction = (cmSpindleDir)value[1]; - } - - // update spindle speed if we're running - pwm_set_duty(PWM_1, _get_spindle_pwm(spindle.enable, spindle.direction)); -} - -/* - * cm_spindle_off_immediate() - turn on/off spindle w/o planning - * cm_spindle_optional_pause() - pause spindle immediately if option is true - * cm_spindle_resume() - restart a paused spindle with an optional dwell +/* Notes: + * - Since it's possible to queue a sync'd control, and then set any spindle state + * with an immediate() before the queued command is reached, _exec_spindle_control() + * must gracefully handle any arbitrary state transition (not just the "legal" ones). + * + * - The spinup and spindown rows are present, but are not implemented unless we + * find we need them. It's easy enough to set these flags using the bit vector + * passed from sync(),but unsetting them once the delay is complete would take + * some more work. + * + * Q: Do we need a spin-down for direction reversal? + * Q: Should the JSON be able to pause and resume? For test purposes only? */ +/* State/Control matrix. Read "If you are in state X and get control Y do action Z" -void cm_spindle_off_immediate() -{ - spindle.enable = SPINDLE_OFF; - float value[] = { (float)SPINDLE_OFF, 0,0,0,0,0 }; - bool flags[] = { 1,0,0,0,0,0 }; - _exec_spindle_control(value, flags); -} + Control: OFF CW CCW PAUSE RESUME + State: |-----------|-----------|-----------|-----------|-----------| + OFF | OFF | CW | CCW | NOP | NOP | + |-----------|-----------|-----------|-----------|-----------| + CW | OFF | NOP | REVERSE | PAUSE | NOP | + |-----------|-----------|-----------|-----------|-----------| + CCW | OFF | REVERSE | NOP | PAUSE | NOP | + |-----------|-----------|-----------|-----------|-----------| + PAUSE | OFF | CW | CCW | NOP | RESUME | + |-----------|-----------|-----------|-----------|-----------| + RESUME | invalid | invalid | invalid | invalid | invalid | + |-----------|-----------|-----------|-----------|-----------| -void cm_spindle_optional_pause(bool option) -{ - if (option && spindle.enable == SPINDLE_ON) { - cm_spindle_off_immediate(); - spindle.enable = SPINDLE_PAUSE; - } -} - -void cm_spindle_resume(float dwell_seconds) -{ - if(spindle.enable == SPINDLE_PAUSE) { - spindle.enable = SPINDLE_ON; - mp_request_out_of_band_dwell(dwell_seconds); - float value[] = { (float)SPINDLE_ON, (float)spindle.direction, 0,0,0,0 }; - bool flags[] = { 1,0,0,0,0,0 }; - _exec_spindle_control(value, flags); - } -} - -/* - * cm_spindle_control() - queue the spindle command to the planner buffer. Observe PAUSE - * _exec_spindle_control() - actually execute the spindle command + Actions: + - OFF Turn spindle off. Even if it's already off (reloads polarities) + - CW Turn spindle on clockwise + - CCW Turn spindle on counterclockwise + - PAUSE Turn off spindle, enter PAUSE state + - RESUME Turn spindle on CW or CCW as before + - NOP No operation, ignore + - REVERSE Reverse spindle direction (Q: need a cycle to spin down then back up again?) */ -stat_t cm_spindle_control(uint8_t control) // requires SPINDLE_CONTROL_xxx style args -{ - if (control == SPINDLE_CONTROL_OFF) { - spindle.enable = SPINDLE_OFF; - } else { - spindle.enable = SPINDLE_ON; - if (control == SPINDLE_CONTROL_CW) { - spindle.direction = SPINDLE_CW; - } else { - spindle.direction = SPINDLE_CCW; - } - } - - float value[] = { (float)spindle.enable, (float)spindle.direction, 0,0,0,0 }; - bool flags[] = { 1,1,0,0,0,0 }; - mp_queue_command(_exec_spindle_control, value, flags); - return(STAT_OK); -} - -// #define _set_spindle_enable_bit_hi() spindle_enable_pin.set() -// #define _set_spindle_enable_bit_lo() spindle_enable_pin.clear() -// #define _set_spindle_direction_bit_hi() spindle_dir_pin.set() -// #define _set_spindle_direction_bit_lo() spindle_dir_pin.clear() - static void _exec_spindle_control(float *value, bool *flag) { + spControl control = (spControl)value[0]; + if (control > SPINDLE_ACTION_MAX) { + return; + } + spControl state = spindle.state; + if (state >= SPINDLE_ACTION_MAX) { +// rpt_exception(STAT_SPINDLE_ASSERTION_FAILURE, "illegal spindle state"); + return; + } + spControl matrix[20] = { + SPINDLE_OFF, SPINDLE_CW, SPINDLE_CCW, SPINDLE_NOP, SPINDLE_NOP, + SPINDLE_OFF, SPINDLE_NOP, SPINDLE_REV, SPINDLE_PAUSE, SPINDLE_NOP, + SPINDLE_OFF, SPINDLE_REV, SPINDLE_NOP, SPINDLE_PAUSE, SPINDLE_NOP, + SPINDLE_OFF, SPINDLE_CW, SPINDLE_CCW, SPINDLE_NOP, SPINDLE_RESUME + }; + spControl action = matrix[(state*5)+control]; + + SPINDLE_DIRECTION_ASSERT; // ensure that the spindle direction is sane + int8_t enable_bit = 0; // default to 0=off + int8_t dir_bit = -1; // -1 will skip setting the direction. 0 & 1 are valid values + bool do_spinup_delay = false; + + switch (action) { + case SPINDLE_NOP: { return; } + + case SPINDLE_OFF: { // enable_bit already set for this case + dir_bit = spindle.direction-1; // spindle direction was stored as '1' & '2' + spindle.state = SPINDLE_OFF; // the control might have been something other than SPINDLE_OFF + break; + } + case SPINDLE_CW: case SPINDLE_CCW: case SPINDLE_REV: { // REV is handled same as CW or CCW for now + enable_bit = 1; + dir_bit = control-1; // adjust direction to be used as a bitmask + spindle.direction = control; + spindle.state = control; + do_spinup_delay = true; + break; + } + case SPINDLE_PAUSE : { + spindle.state = SPINDLE_PAUSE; + break; // enable bit is already set up to stop the move + } + case SPINDLE_RESUME: { + enable_bit = 1; + dir_bit = spindle.direction-1; // spindle direction was stored as '1' & '2' + spindle.state = spindle.direction; + do_spinup_delay = true; + break; + } + default: {} // reversals not handled yet + } + + // Apply the enable and direction bits and adjust the PWM as required + // set the direction first - if (flag[1]) - { - spindle.direction = (cmSpindleDir)value[1]; // record spindle direction in the struct -// if (spindle.direction ^ spindle.dir_polarity) { -// _set_spindle_direction_bit_hi(); -// } else { -// _set_spindle_direction_bit_lo(); -// } + if (dir_bit >= 0) { if (spindle_direction_output != nullptr) { - spindle_direction_output->setValue(spindle.direction); + spindle_direction_output->setValue(dir_bit); } } - if (flag[0]) - { - // set on/off. Mask out PAUSE and consider it OFF - spindle.enable = (cmSpindleEnable)value[0]; // record spindle enable in the struct -// if ((spindle.enable & 0x01) ^ spindle.enable_polarity) { -// _set_spindle_enable_bit_lo(); -// } else { -// _set_spindle_enable_bit_hi(); -// } - if (spindle_enable_output != nullptr) { - spindle_enable_output->setValue(spindle.enable); - } + // set spindle enable + if (spindle_enable_output != nullptr) { + spindle_enable_output->setValue(enable_bit); } - pwm_set_duty(PWM_1, _get_spindle_pwm(spindle.enable, spindle.direction)); + pwm_set_duty(PWM_1, _get_spindle_pwm(spindle, pwm)); + + if (do_spinup_delay) { + mp_request_out_of_band_dwell(spindle.spinup_delay); + } } /* + * spindle_control_immediate() - execute spindle control immediately + * spindle_control_sync() - queue a spindle control to the planner buffer + */ + +stat_t spindle_control_immediate(spControl control) +{ + float value[] = { (float)control }; + _exec_spindle_control(value, nullptr); + return(STAT_OK); +} + +stat_t spindle_control_sync(spControl control) // uses spControl arg: OFF, CW, CCW +{ + // skip the PAUSE operation if pause-enable is not enabled (pause-on-hold) + if ((control == SPINDLE_PAUSE) && (!spindle.pause_enable)) { + return (STAT_OK); + } + + // queue the spindle control + float value[] = { (float)control }; + mp_queue_command(_exec_spindle_control, value, nullptr); + return(STAT_OK); +} + +/**************************************************************************************** + * _exec_spindle_speed() - actually execute the spindle speed command + * spindle_speed_immediate() - execute spindle speed change immediately + * spindle_speed_sync() - queue a spindle speed change to the planner buffer + * + * Setting S0 is considered as turning spindle off. Setting S to non-zero from S0 + * will enable a spinup delay if spinups are npn-zero. + */ + +static void _exec_spindle_speed(float *value, bool *flag) +{ + float previous_speed = spindle.speed; + + spindle.speed = value[0]; + pwm_set_duty(PWM_1, _get_spindle_pwm(spindle, pwm)); + + if (fp_ZERO(previous_speed)) { + mp_request_out_of_band_dwell(spindle.spinup_delay); + } +} + +static stat_t _casey_jones(float speed) +{ + if (speed < spindle.speed_min) { return (STAT_SPINDLE_SPEED_BELOW_MINIMUM); } + if (speed > spindle.speed_max) { return (STAT_SPINDLE_SPEED_MAX_EXCEEDED); } + return (STAT_OK); +} + +stat_t spindle_speed_immediate(float speed) +{ + ritorno(_casey_jones(speed)); + float value[] = { speed }; + _exec_spindle_speed(value, nullptr); + return (STAT_OK); +} + +stat_t spindle_speed_sync(float speed) +{ + ritorno(_casey_jones(speed)); + float value[] = { speed }; + mp_queue_command(_exec_spindle_speed, value, nullptr); + return (STAT_OK); +} + +/**************************************************************************************** * _get_spindle_pwm() - return PWM phase (duty cycle) for dir and speed */ -static float _get_spindle_pwm (cmSpindleEnable enable, cmSpindleDir direction) +static float _get_spindle_pwm (spSpindle_t &_spindle, pwmControl_t &_pwm) { - float speed_lo=0, speed_hi=0, phase_lo=0, phase_hi=0; - if (direction == SPINDLE_CW ) { - speed_lo = pwm.c[PWM_1].cw_speed_lo; - speed_hi = pwm.c[PWM_1].cw_speed_hi; - phase_lo = pwm.c[PWM_1].cw_phase_lo; - phase_hi = pwm.c[PWM_1].cw_phase_hi; + float speed_lo, speed_hi, phase_lo, phase_hi; + if (_spindle.direction == SPINDLE_CW ) { + speed_lo = _pwm.c[PWM_1].cw_speed_lo; + speed_hi = _pwm.c[PWM_1].cw_speed_hi; + phase_lo = _pwm.c[PWM_1].cw_phase_lo; + phase_hi = _pwm.c[PWM_1].cw_phase_hi; } else { // if (direction == SPINDLE_CCW ) { - speed_lo = pwm.c[PWM_1].ccw_speed_lo; - speed_hi = pwm.c[PWM_1].ccw_speed_hi; - phase_lo = pwm.c[PWM_1].ccw_phase_lo; - phase_hi = pwm.c[PWM_1].ccw_phase_hi; + speed_lo = _pwm.c[PWM_1].ccw_speed_lo; + speed_hi = _pwm.c[PWM_1].ccw_speed_hi; + phase_lo = _pwm.c[PWM_1].ccw_phase_lo; + phase_hi = _pwm.c[PWM_1].ccw_phase_hi; } - if (enable == SPINDLE_ON) { + if ((_spindle.state == SPINDLE_CW) || (_spindle.state == SPINDLE_CCW)) { // clamp spindle speed to lo/hi range - if (spindle.speed < speed_lo) { - spindle.speed = speed_lo; + if (_spindle.speed < speed_lo) { + _spindle.speed = speed_lo; } - if (spindle.speed > speed_hi) { - spindle.speed = speed_hi; + if (_spindle.speed > speed_hi) { + _spindle.speed = speed_hi; } // normalize speed to [0..1] - float speed = (spindle.speed - speed_lo) / (speed_hi - speed_lo); - return (speed * (phase_hi - phase_lo)) + phase_lo; + float speed = (_spindle.speed - speed_lo) / (speed_hi - speed_lo); + return ((speed * (phase_hi - phase_lo)) + phase_lo); } else { - return pwm.c[PWM_1].phase_off; + return (_pwm.c[PWM_1].phase_off); } } - -/* - * cm_spindle_override_control() - * cm_start_spindle_override() - * cm_end_spindle_override() +/**************************************************************************************** + * spindle_override_control() + * spindle_start_override() + * spindle_end_override() */ -stat_t cm_sso_control(const float P_word, const bool P_flag) // M51 +stat_t spindle_override_control(const float P_word, const bool P_flag) // M51 { bool new_enable = true; bool new_override = false; @@ -271,27 +364,27 @@ stat_t cm_sso_control(const float P_word, const bool P_flag) // M51 if (P_word > SPINDLE_OVERRIDE_MAX) { return (STAT_INPUT_EXCEEDS_MAX_VALUE); } - spindle.sso_factor = P_word; // P word is valid, store it. + spindle.override_factor = P_word; // P word is valid, store it. new_override = true; } } - if (cm.gmx.m48_enable) { // if master enable is ON - if (new_enable && (new_override || !spindle.sso_enable)) { // 3 cases to start a ramp - cm_start_spindle_override(SPINDLE_OVERRIDE_RAMP_TIME, spindle.sso_factor); - } else if (spindle.sso_enable && !new_enable) { // case to turn off the ramp - cm_end_spindle_override(SPINDLE_OVERRIDE_RAMP_TIME); + if (cm->gmx.m48_enable) { // if master enable is ON + if (new_enable && (new_override || !spindle.override_enable)) { // 3 cases to start a ramp + spindle_start_override(SPINDLE_OVERRIDE_RAMP_TIME, spindle.override_factor); + } else if (spindle.override_enable && !new_enable) { // case to turn off the ramp + spindle_end_override(SPINDLE_OVERRIDE_RAMP_TIME); } } - spindle.sso_enable = new_enable; // always update the enable state + spindle.override_enable = new_enable; // always update the enable state return (STAT_OK); } -void cm_start_spindle_override(const float ramp_time, const float override_factor) +void spindle_start_override(const float ramp_time, const float override_factor) { return; } -void cm_end_spindle_override(const float ramp_time) +void spindle_end_override(const float ramp_time) { return; } @@ -300,69 +393,84 @@ void cm_end_spindle_override(const float ramp_time) * END OF SPINDLE FUNCTIONS * ****************************/ -/*********************************************************************************** +/**************************************************************************************** * CONFIGURATION AND INTERFACE FUNCTIONS * Functions to get and set variables from the cfgArray table - ***********************************************************************************/ + ****************************************************************************************/ -/* - * cm_set_dir() - a cheat to set direction w/o using the M commands - * - * This is provided as a way to set and clear spindle direction without using M commands - * It's here because disabling a spindle (M5) does not change the direction, only the enable. - */ +/**************************************************************************************** + **** Spindle Settings ****************************************************************** + ****************************************************************************************/ -stat_t cm_set_dir(nvObj_t *nv) -{ - set_01(nv); - float value[] = { (float)spindle.enable, (float)spindle.direction, 0,0,0,0 }; - bool flags[] = { 1,1,0,0,0,0 }; - _exec_spindle_control(value, flags); - return (STAT_OK); +stat_t sp_get_spmo(nvObj_t *nv) { return(get_integer(nv, spindle.mode)); } +stat_t sp_set_spmo(nvObj_t *nv) { return(set_integer(nv, (uint8_t &)spindle.mode, SPINDLE_DISABLED, SPINDLE_MODE_MAX)); } + +stat_t sp_get_spep(nvObj_t *nv) { return(get_integer(nv, spindle.enable_polarity)); } +stat_t sp_set_spep(nvObj_t *nv) { + stat_t status = set_integer(nv, (uint8_t &)spindle.enable_polarity, 0, 1); + spindle_enable_output->setPolarity((ioPolarity)spindle.enable_polarity); + spindle_control_immediate(SPINDLE_OFF); // stop spindle and apply new settings + return (status); } - -/* - * cm_set_sso() - set spindle speed feedrate override factor - */ - -stat_t cm_set_sso(nvObj_t *nv) -{ - if (nv->value < SPINDLE_OVERRIDE_MIN) { - return (STAT_INPUT_LESS_THAN_MIN_VALUE); - } - if (nv->value > SPINDLE_OVERRIDE_MAX) { - return (STAT_INPUT_EXCEEDS_MAX_VALUE); - } - set_flt(nv); - return(STAT_OK); +stat_t sp_get_spdp(nvObj_t *nv) { return(get_integer(nv, spindle.dir_polarity)); } +stat_t sp_set_spdp(nvObj_t *nv) { + stat_t status = set_integer(nv, (uint8_t &)spindle.dir_polarity, 0, 1); + spindle_direction_output->setPolarity((ioPolarity)spindle.dir_polarity); + spindle_control_immediate(SPINDLE_OFF); // stop spindle and apply new settings + return (status); } -/*********************************************************************************** +stat_t sp_get_spph(nvObj_t *nv) { return(get_integer(nv, spindle.pause_enable)); } +stat_t sp_set_spph(nvObj_t *nv) { return(set_integer(nv, (uint8_t &)spindle.pause_enable, 0, 1)); } +stat_t sp_get_spde(nvObj_t *nv) { return(get_float(nv, spindle.spinup_delay)); } +stat_t sp_set_spde(nvObj_t *nv) { return(set_float_range(nv, spindle.spinup_delay, 0, SPINDLE_DWELL_MAX)); } + +stat_t sp_get_spsn(nvObj_t *nv) { return(get_float(nv, spindle.speed_min)); } +stat_t sp_set_spsn(nvObj_t *nv) { return(set_float_range(nv, spindle.speed_min, SPINDLE_SPEED_MIN, SPINDLE_SPEED_MAX)); } +stat_t sp_get_spsm(nvObj_t *nv) { return(get_float(nv, spindle.speed_max)); } +stat_t sp_set_spsm(nvObj_t *nv) { return(set_float_range(nv, spindle.speed_max, SPINDLE_SPEED_MIN, SPINDLE_SPEED_MAX)); } + +stat_t sp_get_spoe(nvObj_t *nv) { return(get_integer(nv, spindle.override_enable)); } +stat_t sp_set_spoe(nvObj_t *nv) { return(set_integer(nv, (uint8_t &)spindle.override_enable, 0, 1)); } +stat_t sp_get_spo(nvObj_t *nv) { return(get_float(nv, spindle.override_factor)); } +stat_t sp_set_spo(nvObj_t *nv) { return(set_float_range(nv, spindle.override_factor, SPINDLE_OVERRIDE_MIN, SPINDLE_OVERRIDE_MAX)); } + +// These are provided as a way to view and control spindles without using M commands +stat_t sp_get_spc(nvObj_t *nv) { return(get_integer(nv, spindle.state)); } +stat_t sp_set_spc(nvObj_t *nv) { return(spindle_control_immediate((spControl)nv->value_int)); } +stat_t sp_get_sps(nvObj_t *nv) { return(get_float(nv, spindle.speed)); } +stat_t sp_set_sps(nvObj_t *nv) { return(spindle_speed_immediate(nv->value_flt)); } + +/**************************************************************************************** * TEXT MODE SUPPORT * Functions to print variables from the cfgArray table - ***********************************************************************************/ + ****************************************************************************************/ #ifdef __TEXT_MODE +const char fmt_spc[] = "[spc] spindle control:%12d [0=OFF,1=CW,2=CCW]\n"; +const char fmt_sps[] = "[sps] spindle speed:%14.0f rpm\n"; +const char fmt_spmo[] = "[spmo] spindle mode%16d [0=disabled,1=plan-to-stop,2=continuous]\n"; const char fmt_spep[] = "[spep] spindle enable polarity%5d [0=active_low,1=active_high]\n"; const char fmt_spdp[] = "[spdp] spindle direction polarity%2d [0=CW_low,1=CW_high]\n"; const char fmt_spph[] = "[spph] spindle pause on hold%7d [0=no,1=pause_on_hold]\n"; -const char fmt_spdw[] = "[spdw] spindle dwell time%12.1f seconds\n"; -const char fmt_ssoe[] ="[ssoe] spindle speed override ena%2d [0=disable,1=enable]\n"; -const char fmt_sso[] ="[sso] spindle speed override%11.3f [0.050 < sso < 2.000]\n"; -const char fmt_spe[] = "Spindle Enable:%7d [0=OFF,1=ON,2=PAUSE]\n"; -const char fmt_spd[] = "Spindle Direction:%4d [0=CW,1=CCW]\n"; -const char fmt_sps[] = "Spindle Speed: %7.0f rpm\n"; +const char fmt_spde[] = "[spde] spindle spinup delay%10.1f seconds\n"; +const char fmt_spsn[] = "[spsn] spindle speed min%14.2f rpm\n"; +const char fmt_spsm[] = "[spsm] spindle speed max%14.2f rpm\n"; +const char fmt_spoe[] = "[spoe] spindle speed override ena%2d [0=disable,1=enable]\n"; +const char fmt_spo[] = "[spo] spindle speed override%10.3f [0.050 < spo < 2.000]\n"; -void cm_print_spep(nvObj_t *nv) { text_print(nv, fmt_spep);} // TYPE_INT -void cm_print_spdp(nvObj_t *nv) { text_print(nv, fmt_spdp);} // TYPE_INT -void cm_print_spph(nvObj_t *nv) { text_print(nv, fmt_spph);} // TYPE_INT -void cm_print_spdw(nvObj_t *nv) { text_print(nv, fmt_spdw);} // TYPE_FLOAT -void cm_print_ssoe(nvObj_t *nv) { text_print(nv, fmt_ssoe);} // TYPE INT -void cm_print_sso(nvObj_t *nv) { text_print(nv, fmt_sso);} // TYPE FLOAT -void cm_print_spe(nvObj_t *nv) { text_print(nv, fmt_spe);} // TYPE_INT -void cm_print_spd(nvObj_t *nv) { text_print(nv, fmt_spd);} // TYPE_INT -void cm_print_sps(nvObj_t *nv) { text_print(nv, fmt_sps);} // TYPE_FLOAT +void sp_print_spc(nvObj_t *nv) { text_print(nv, fmt_spc);} // TYPE_INT +void sp_print_sps(nvObj_t *nv) { text_print(nv, fmt_sps);} // TYPE_FLOAT +void sp_print_spmo(nvObj_t *nv) { text_print(nv, fmt_spmo);} // TYPE_INT +void sp_print_spep(nvObj_t *nv) { text_print(nv, fmt_spep);} // TYPE_INT +void sp_print_spdp(nvObj_t *nv) { text_print(nv, fmt_spdp);} // TYPE_INT +void sp_print_spph(nvObj_t *nv) { text_print(nv, fmt_spph);} // TYPE_INT +void sp_print_spde(nvObj_t *nv) { text_print(nv, fmt_spde);} // TYPE_FLOAT +void sp_print_spsn(nvObj_t *nv) { text_print(nv, fmt_spsn);} // TYPE_FLOAT +void sp_print_spsm(nvObj_t *nv) { text_print(nv, fmt_spsm);} // TYPE_FLOAT +void sp_print_spoe(nvObj_t *nv) { text_print(nv, fmt_spoe);} // TYPE INT +void sp_print_spo(nvObj_t *nv) { text_print(nv, fmt_spo);} // TYPE FLOAT #endif // __TEXT_MODE diff --git a/g2core/spindle.h b/g2core/spindle.h index 9e26d585..5b2eb07a 100644 --- a/g2core/spindle.h +++ b/g2core/spindle.h @@ -2,7 +2,7 @@ * spindle.h - spindle driver * This file is part of the g2core project * - * Copyright (c) 2010 - 2016 Alden S. Hart, Jr. + * Copyright (c) 2010 - 2018 Alden S. Hart, Jr. * * This file ("the software") is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License, version 2 as published by the @@ -28,65 +28,83 @@ #ifndef SPINDLE_H_ONCE #define SPINDLE_H_ONCE +#define SPINDLE_OVERRIDE_ENABLE false +#define SPINDLE_OVERRIDE_FACTOR 1.00 +#define SPINDLE_OVERRIDE_MIN 0.05 // 5% +#define SPINDLE_OVERRIDE_MAX 2.00 // 200% +#define SPINDLE_OVERRIDE_RAMP_TIME 1 // change sped in seconds + +typedef enum { + SPINDLE_DISABLED = 0, // spindle will not operate + SPINDLE_PLAN_TO_STOP, // spindle operating, plans to stop + SPINDLE_CONTINUOUS, // spindle operating, does not plan to stop +} spMode; +#define SPINDLE_MODE_MAX SPINDLE_CONTINUOUS + +// spControl enum is used for multiple purposes: +// - request a spindle action (OFF, CW, CCW, PAUSE, RESUME) +// - keep current spindle state in spindle.state +// - store as current direction (CW/CCW) in spindle.direction +// - enumerate internal actions such as NOP, REV that are neither states nor controls typedef enum { // how spindle controls are presented by the Gcode parser - SPINDLE_CONTROL_OFF = 0, // M5 - SPINDLE_CONTROL_CW, // M3 - SPINDLE_CONTROL_CCW // M4 -} cmSpindleControl; + SPINDLE_OFF = 0, // M5 + SPINDLE_CW = 1, // M3 and store CW to spindle.direction + SPINDLE_CCW = 2, // M4 and store CCW to spsindle.direction + SPINDLE_PAUSE = 3, // request PAUSE and store PAUSED state to spindle.state + SPINDLE_RESUME = 4, // request RESUME and revert spindle.state to CW, CCW + SPINDLE_NOP, // no operation + SPINDLE_REV // operation to reverse spindle direction +} spControl; +#define SPINDLE_ACTION_MAX SPINDLE_RESUME -typedef enum { - SPINDLE_OFF = 0, - SPINDLE_ON, - SPINDLE_PAUSE // meaning it was on and now it's off -} cmSpindleEnable; +// *** NOTE: The spindle polarity active hi/low values currently agree with ioMode in gpio.h +// These will all need to be changed to ACTIVE_HIGH = 0, ACTIVE_LOW = 1 +// See: https://github.com/synthetos/g2_private/wiki/GPIO-Design-Discussion#settings-common-to-all-io-types -typedef enum { // spindle direction state - SPINDLE_CW = 0, - SPINDLE_CCW -} cmSpindleDir; +typedef enum { // Note: These values agree with + SPINDLE_ACTIVE_LOW = 0, // Will set output to 0 to enable the spindle or CW direction + SPINDLE_ACTIVE_HIGH = 1, // Will set output to 1 to enable the spindle or CW direction +} spPolarity; -typedef enum { - SPINDLE_ACTIVE_LOW = 0, - SPINDLE_ACTIVE_HIGH -} cmSpindlePolarity; - -typedef enum { +typedef enum { // electronic speed controller for some spindles ESC_ONLINE = 0, ESC_OFFLINE, ESC_LOCKOUT, ESC_REBOOTING, ESC_LOCKOUT_AND_REBOOTING, -} cmESCState; - -#define SPINDLE_OVERRIDE_ENABLE false -#define SPINDLE_OVERRIDE_FACTOR 1.00 -#define SPINDLE_OVERRIDE_MIN 0.05 // 5% -#define SPINDLE_OVERRIDE_MAX 2.00 // 200% -#define SPINDLE_OVERRIDE_RAMP_TIME 1 // change speed in seconds +} ESCState; /* * Spindle control structure */ -typedef struct cmSpindleSingleton { - float speed; // S in RPM - cmSpindleEnable enable; // OFF, ON, PAUSE - cmSpindleDir direction; // CW, CCW +typedef struct spSpindle { - bool pause_on_hold; // pause on feedhold - cmSpindlePolarity enable_polarity; // 0=active low, 1=active high - cmSpindlePolarity dir_polarity; // 0=clockwise low, 1=clockwise high - float dwell_seconds; // dwell on spindle resume + spMode mode; // {spm:} spindle operating mode + spControl state; // {spc:} OFF, ON, PAUSE, RESUME, WAIT + spControl direction; // 1=CW, 2=CCW (subset of above state) - bool sso_enable; // TRUE = spindle speed override enabled (see also m48_enable in canonical machine) - float sso_factor; // 1.0000 x S spindle speed. Go up or down from there + float speed; // {sps:} S in RPM + float speed_min; // {spsn:} minimum settable spindle speed + float speed_max; // {spsm:} maximum settable spindle speed - cmESCState esc_state; // state management for ESC controller - uint32_t esc_boot_timer; // When the ESC last booted up - uint32_t esc_lockout_timer; // When the ESC lockout last triggered + spPolarity enable_polarity; // {spep:} 0=active low, 1=active high + spPolarity dir_polarity; // {spdp:} 0=clockwise low, 1=clockwise high + bool pause_enable; // {spph:} pause on feedhold + float spinup_delay; // {spde:} optional delay on spindle start (set to 0 to disable) +// float spindown_delay; // {spds:} optional delay on spindle stop (set to 0 to disable) -} cmSpindleton_t; -extern cmSpindleton_t spindle; + bool override_enable; // {spoe:} TRUE = spindle speed override enabled (see also m48_enable in canonical machine) + float override_factor; // {spo:} 1.0000 x S spindle speed. Go up or down from there + + // No longer used? + // // Spindle speed controller variables + // ESCState esc_state; // state management for ESC controller + // uint32_t esc_boot_timer; // When the ESC last booted up + // uint32_t esc_lockout_timer; // When the ESC lockout last triggered + +} spSpindle_t; +extern spSpindle_t spindle; /* * Global Scope Functions @@ -94,44 +112,76 @@ extern cmSpindleton_t spindle; void spindle_init(); void spindle_reset(); -stat_t cm_set_spindle_speed(float speed); // S parameter -stat_t cm_spindle_control(uint8_t control); // M3, M4, M5 integrated spindle control -void cm_spindle_off_immediate(void); -void cm_spindle_optional_pause(bool option); // stop spindle based on system options selected -void cm_spindle_resume(float dwell_seconds); // restart spindle after pause based on previous state -stat_t cm_sso_control(const float P_word, const bool P_flag); // M51 -void cm_start_spindle_override(const float ramp_time, const float override_factor); -void cm_end_spindle_override(const float ramp_time); +stat_t spindle_control_immediate(spControl control); +stat_t spindle_control_sync(spControl control); +stat_t spindle_speed_immediate(float speed); // S parameter +stat_t spindle_speed_sync(float speed); // S parameter -stat_t cm_set_dir(nvObj_t* nv); -stat_t cm_set_sso(nvObj_t* nv); +stat_t spindle_override_control(const float P_word, const bool P_flag); // M51 +void spindle_start_override(const float ramp_time, const float override_factor); +void spindle_end_override(const float ramp_time); + +stat_t sp_get_spmo(nvObj_t *nv); +stat_t sp_set_spmo(nvObj_t *nv); +stat_t sp_get_spep(nvObj_t *nv); +stat_t sp_set_spep(nvObj_t *nv); +stat_t sp_get_spdp(nvObj_t *nv); +stat_t sp_set_spdp(nvObj_t *nv); +stat_t sp_get_spph(nvObj_t *nv); +stat_t sp_set_spph(nvObj_t *nv); + +stat_t sp_get_spde(nvObj_t *nv); +stat_t sp_set_spde(nvObj_t *nv); +//stat_t sp_get_spdn(nvObj_t *nv); +//stat_t sp_set_spdn(nvObj_t *nv); + +stat_t sp_get_spsn(nvObj_t *nv); +stat_t sp_set_spsn(nvObj_t *nv); +stat_t sp_get_spsm(nvObj_t *nv); +stat_t sp_set_spsm(nvObj_t *nv); + +stat_t sp_get_spoe(nvObj_t* nv); +stat_t sp_set_spoe(nvObj_t* nv); +stat_t sp_get_spo(nvObj_t* nv); +stat_t sp_set_spo(nvObj_t* nv); + +stat_t sp_get_spc(nvObj_t* nv); +stat_t sp_set_spc(nvObj_t* nv); +stat_t sp_get_sps(nvObj_t* nv); +stat_t sp_set_sps(nvObj_t* nv); /*--- text_mode support functions ---*/ #ifdef __TEXT_MODE - void cm_print_spep(nvObj_t* nv); - void cm_print_spdp(nvObj_t* nv); - void cm_print_spph(nvObj_t* nv); - void cm_print_spdw(nvObj_t* nv); - void cm_print_ssoe(nvObj_t* nv); - void cm_print_sso(nvObj_t* nv); - void cm_print_spe(nvObj_t* nv); - void cm_print_spd(nvObj_t* nv); - void cm_print_sps(nvObj_t* nv); + void sp_print_spmo(nvObj_t* nv); + void sp_print_spep(nvObj_t* nv); + void sp_print_spdp(nvObj_t* nv); + void sp_print_spph(nvObj_t* nv); + void sp_print_spde(nvObj_t* nv); +// void sp_print_spdn(nvObj_t* nv); + void sp_print_spsn(nvObj_t* nv); + void sp_print_spsm(nvObj_t* nv); + void sp_print_spoe(nvObj_t* nv); + void sp_print_spo(nvObj_t* nv); + void sp_print_spc(nvObj_t* nv); + void sp_print_sps(nvObj_t* nv); #else - #define cm_print_spep tx_print_stub - #define cm_print_spdp tx_print_stub - #define cm_print_spph tx_print_stub - #define cm_print_spdw tx_print_stub - #define cm_print_ssoe tx_print_stub - #define cm_print_spe tx_print_stub - #define cm_print_sso tx_print_stub - #define cm_print_spd tx_print_stub - #define cm_print_sps tx_print_stub + #define sp_print_spmo tx_print_stub + #define sp_print_spep tx_print_stub + #define sp_print_spdp tx_print_stub + #define sp_print_spph tx_print_stub + #define sp_print_spde tx_print_stub +// #define sp_print_spdn tx_print_stub + #define sp_print_spsn tx_print_stub + #define sp_print_spsm tx_print_stub + #define sp_print_spoe tx_print_stub + #define sp_print_spo tx_print_stub + #define sp_print_spc tx_print_stub + #define sp_print_sps tx_print_stub #endif // __TEXT_MODE diff --git a/g2core/stepper.cpp b/g2core/stepper.cpp index 2e66aa6f..079b91f9 100644 --- a/g2core/stepper.cpp +++ b/g2core/stepper.cpp @@ -2,8 +2,8 @@ * stepper.cpp - stepper motor controls * This file is part of the g2core project * - * Copyright (c) 2010 - 2016 Alden S. Hart, Jr. - * Copyright (c) 2013 - 2016 Robert Giseburt + * Copyright (c) 2010 - 2019 Alden S. Hart, Jr. + * Copyright (c) 2013 - 2019 Robert Giseburt * * This file ("the software") is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License, version 2 as published by the @@ -40,20 +40,15 @@ #include "controller.h" #include "xio.h" #include "kinematics.h" +#include "gpio.h" /**** Debugging output with semihosting ****/ #include "MotateDebug.h" -// Unless debugging, this should always read "#if 0 && ..." -// DON'T COMMIT with anything else! -#if 0 && (IN_DEBUGGER == 1) -template -void stepper_debug(const char (&str)[len]) { Motate::debug.write(str); }; -#else -template -void stepper_debug(const char (&str)[len]) { ; }; -#endif +/* Note: stepper_debug statements removed 1/16/17 in SHA eb0905ccae03c04f99e6f471cbe029002f0324c6. + * See earlier commits to recover + */ /**** Allocate structures ****/ @@ -65,9 +60,6 @@ static stRunSingleton_t st_run HOT_DATA; static void _load_move(void) HOT_FUNC; -// handy macro -//#define _f_to_period(f) (uint16_t)((float)F_CPU / (float)f) - /**** Setup motate ****/ using namespace Motate; @@ -80,7 +72,7 @@ dda_timer_type dda_timer {kTimerUpToMatch, FREQUENCY_DDA}; // stepper p exec_timer_type exec_timer; // triggers calculation of next+1 stepper segment fwd_plan_timer_type fwd_plan_timer; // triggers planning of next block -// SystickEvent for handling dweels (must be registered before it is active) +// SystickEvent for handling dwells (must be registered before it is active) Motate::SysTickEvent dwell_systick_event {[&] { if (--st_run.dwell_ticks_downcount == 0) { SysTickTimer.unregisterEvent(&dwell_systick_event); @@ -88,6 +80,25 @@ Motate::SysTickEvent dwell_systick_event {[&] { } }, nullptr}; +/* Note on the above: +It's a lambda function creating a closure function. +The full implementation that uses it is small and may help: +https://github.com/synthetos/Motate/blob/41e5b92a98de4b268d1804bf6eadf3333298fc75/MotateProject/motate/Atmel_sam_common/SamTimers.h#L1147-L1218 +It's just like a function, and is used as a function pointer. + +But the closure part means that whatever variables that were in scope where the +[&](parameters){code} is will be captured by the compiler as references in the generated +function and used wherever the function gets called. In this particular use, there isn't +anything that wouldn't be available anywhere in that file, but they're not being called +from that file. They're being called by the systick interrupt which is over in SamTmers.cpp +So this saves a bunch of work exposing bits that the systick would need to call and encapsulates it. +And there's almost no runtime overhead. Just a check for a valid function pointer and then a call of it. +I'd like to get rid of that check but it's more work than its worth. + +See here for some good info on lambda functions in C++ +http://www.cprogramming.com/c++11/c++11-lambda-closures.html +http://en.cppreference.com/w/cpp/language/lambda +*/ /************************************************************************************ **** CODE ************************************************************************** @@ -159,7 +170,7 @@ void stepper_reset() for (uint8_t motor=0; motorstepEnd(); - // } - // loop unrolled version (it's actually faster) motor_1.stepEnd(); motor_2.stepEnd(); #if MOTORS > 2 @@ -364,9 +370,7 @@ void dda_timer_type::interrupt() void st_request_exec_move() { - stepper_debug("e"); exec_timer.setInterruptPending(); - stepper_debug("!\n"); } namespace Motate { // Define timer inside Motate namespace @@ -378,21 +382,22 @@ namespace Motate { // Define timer inside Motate namespace { exec_timer.getInterruptCause(); // clears the interrupt condition if (st_pre.buffer_state == PREP_BUFFER_OWNED_BY_EXEC) { - stepper_debug("E>"); if (mp_exec_move() != STAT_NOOP) { - stepper_debug("E+\n"); st_pre.buffer_state = PREP_BUFFER_OWNED_BY_LOADER; // flip it back st_request_load_move(); return; } - stepper_debug("E-\n"); } } } // namespace Motate +/**************************************************************************************** + * st_request_forward_plan - performs forward planning on penultimate block + * fwd_plan interrupt - interrupt handler for calling forward planning function + */ + void st_request_forward_plan() { - stepper_debug("p"); fwd_plan_timer.setInterruptPending(); } @@ -404,13 +409,10 @@ namespace Motate { // Define timer inside Motate namespace void fwd_plan_timer_type::interrupt() { fwd_plan_timer.getInterruptCause(); // clears the interrupt condition - stepper_debug("P>"); if (mp_forward_plan() != STAT_NOOP) { // We now have a move to exec. - stepper_debug("P+\n"); st_request_exec_move(); return; } - stepper_debug("P-\n"); } } // namespace Motate @@ -429,10 +431,8 @@ void st_request_load_move() if (st_runtime_isbusy()) { // don't request a load if the runtime is busy return; } - stepper_debug("l"); if (st_pre.buffer_state == PREP_BUFFER_OWNED_BY_LOADER) { // bother interrupting - stepper_debug("_"); - _load_move(); + _load_move(); } } @@ -454,45 +454,29 @@ static void _load_move() // Be aware that dda_ticks_downcount must equal zero for the loader to run. // So the initial load must also have this set to zero as part of initialization if (st_runtime_isbusy()) { - return; // exit if the runtime is busy + return; // exit if the runtime is busy } - if (st_pre.buffer_state != PREP_BUFFER_OWNED_BY_LOADER) { // if there are no moves to load... - if (cm.motion_state == MOTION_RUN) { -#if IN_DEBUGGER == 1 -//#warning debbugger REQUIRED for running this firmware! -// __asm__("BKPT"); // attempted to _load_move with PREP_BUFFER_OWNED_BY_EXEC and cm.motion_state == MOTION_RUN -#endif - st_request_exec_move(); - return; - } - - // ...start motor power timeouts - // for (uint8_t motor = MOTOR_1; motor < MOTORS; motor++) { - // Motors[motor]->motionStopped(); - // } - // loop unrolled version + // If there are no moves to load start motor power timeouts + if (st_pre.buffer_state != PREP_BUFFER_OWNED_BY_LOADER) { motor_1.motionStopped(); // ...start motor power timeouts - motor_2.motionStopped(); // ...start motor power timeouts + motor_2.motionStopped(); #if (MOTORS > 2) - motor_3.motionStopped(); // ...start motor power timeouts + motor_3.motionStopped(); #endif #if (MOTORS > 3) - motor_4.motionStopped(); // ...start motor power timeouts + motor_4.motionStopped(); #endif #if (MOTORS > 4) - motor_5.motionStopped(); // ...start motor power timeouts + motor_5.motionStopped(); #endif #if (MOTORS > 5) - motor_6.motionStopped(); // ...start motor power timeouts + motor_6.motionStopped(); #endif - stepper_debug("•"); return; } // if (st_pre.buffer_state != PREP_BUFFER_OWNED_BY_LOADER) - stepper_debug("^"); - - // handle aline loads first (most common case) NB: there are no more lines, only alines + // handle aline loads first (most common case) if (st_pre.block_type == BLOCK_TYPE_ALINE) { //**** setup the new segment **** @@ -664,7 +648,6 @@ static void _load_move() stat_t st_prep_line(const float start_velocity, const float end_velocity, const float travel_steps[], const float following_error[], const float segment_time) { - stepper_debug("😶"); // trap assertion failures and other conditions that would prevent queuing the line if (st_pre.buffer_state != PREP_BUFFER_OWNED_BY_EXEC) { // never supposed to happen return (cm_panic(STAT_INTERNAL_ERROR, "st_prep_line() prep sync error")); @@ -672,8 +655,6 @@ stat_t st_prep_line(const float start_velocity, const float end_velocity, const return (cm_panic(STAT_PREP_LINE_MOVE_TIME_IS_INFINITE, "st_prep_line()")); } else if (isnan(segment_time)) { // never supposed to happen return (cm_panic(STAT_PREP_LINE_MOVE_TIME_IS_NAN, "st_prep_line()")); -// } else if (segment_time < EPSILON) { -// return (STAT_MINIMUM_TIME_MOVE); } // setup segment parameters // - dda_ticks is the integer number of DDA clock ticks needed to play out the segment @@ -771,7 +752,7 @@ stat_t st_prep_line(const float start_velocity, const float end_velocity, const } st_pre.block_type = BLOCK_TYPE_ALINE; st_pre.buffer_state = PREP_BUFFER_OWNED_BY_LOADER; // signal that prep buffer is ready - stepper_debug("👍🏻"); + return (STAT_OK); } @@ -847,7 +828,6 @@ stat_t st_prep_line(const float start_velocities[], const float end_velocities[] } st_pre.block_type = BLOCK_TYPE_ALINE; st_pre.buffer_state = PREP_BUFFER_OWNED_BY_LOADER; // signal that prep buffer is ready - stepper_debug("👍🏻"); return (STAT_OK); } /* @@ -884,48 +864,46 @@ void st_prep_dwell(float milliseconds) } /* - * st_request_out_of_band_dwell() - * (only usable while exec isn't running, e.g. in feedhold or stopped states...) - * add a dwell to the loader without going through the planner buffers + * st_prep_out_of_band_dwell() + * + * Add a dwell to the loader without going through the planner buffers. + * Only usable while exec isn't running, e.g. in feedhold or stopped states. + * Otherwise it is skipped. */ -void st_request_out_of_band_dwell(float milliseconds) + +void st_prep_out_of_band_dwell(float milliseconds) { - st_prep_dwell(milliseconds); - st_pre.buffer_state = PREP_BUFFER_OWNED_BY_LOADER; // signal that prep buffer is ready - st_request_load_move(); + if (!st_runtime_isbusy()) { + st_prep_dwell(milliseconds); + st_pre.buffer_state = PREP_BUFFER_OWNED_BY_LOADER; // signal that prep buffer is ready + st_request_load_move(); + } } /* * _set_hw_microsteps() - set microsteps in hardware */ -static void _set_hw_microsteps(const uint8_t motor, const uint16_t microsteps) + +static void _set_hw_microsteps(const uint8_t motor, const uint8_t microsteps) { - if (motor >= MOTORS) {return;} + if (motor >= MOTORS) { return; } Motors[motor]->setMicrosteps(microsteps); } - /*********************************************************************************** * CONFIGURATION AND INTERFACE FUNCTIONS * Functions to get and set variables from the cfgArray table ***********************************************************************************/ /* HELPERS - * _get_motor() - helper to return motor number as an index or -1 if na + * _motor() - motor number as an index or -1 if na */ -static int8_t _get_motor(const index_t index) +static int8_t _motor(const index_t index) { - char *ptr; - char motors[] = {"123456"}; - char tmp[GROUP_LEN+1]; - - strcpy(tmp, cfgArray[index].group); - if ((ptr = strchr(motors, tmp[0])) == NULL) { - return (-1); - } - return (ptr - motors); + char c = cfgArray[index].token[0]; + return (isdigit(c) ? c-0x31 : -1 ); // 0x30 + 1 offsets motor 1 to == 0 } /* @@ -933,179 +911,187 @@ static int8_t _get_motor(const index_t index) * This function will need to be rethought if microstep morphing is implemented */ -static void _set_motor_steps_per_unit(nvObj_t *nv) +static float _set_motor_steps_per_unit(nvObj_t *nv) { - uint8_t m = _get_motor(nv->index); - st_cfg.mot[m].units_per_step = (st_cfg.mot[m].travel_rev * st_cfg.mot[m].step_angle) / (360 * st_cfg.mot[m].microsteps); + uint8_t m = _motor(nv->index); + st_cfg.mot[m].units_per_step = (st_cfg.mot[m].travel_rev * st_cfg.mot[m].step_angle) / + (360 * st_cfg.mot[m].microsteps); + st_cfg.mot[m].steps_per_unit = 1/st_cfg.mot[m].units_per_step; kn_config_changed(); + + return (st_cfg.mot[m].steps_per_unit); } /* PER-MOTOR FUNCTIONS - * st_set_ma() - map motor to axis + * + * st_get_ma() - get motor axis mapping + * st_set_ma() - set motor axis mapping + * st_get_sa() - get motor step angle * st_set_sa() - set motor step angle + * st_get_tr() - get travel per motor revolution * st_set_tr() - set travel per motor revolution + * st_get_mi() - get motor microsteps * st_set_mi() - set motor microsteps + * * st_set_pm() - set motor power mode * st_get_pm() - get motor power mode * st_set_pl() - set motor power level */ -stat_t st_set_ma(nvObj_t *nv) // map motor to axis +/* + * st_get_ma() - get motor axis mapping + * + * Legacy axis numbers are XYZABC for axis 0-5 + * External axis numbers are XYZABCUVW for axis 0-8 + * Internal axis numbers are XYZUVWABC for axis 0-8 (for various code reasons) + * + * This function retrieves an internal axis number and remaps it to an external axis number + */ +stat_t st_get_ma(nvObj_t *nv) { - if (nv->value < 0) { - nv->valuetype = TYPE_NULL; - return (STAT_INPUT_LESS_THAN_MIN_VALUE); - } - if (nv->value >= AXES) { - nv->valuetype = TYPE_NULL; - return (STAT_INPUT_EXCEEDS_MAX_VALUE); - } - set_ui8(nv); - - kn_config_changed(); - + uint8_t remap_axis[9] = { 0,1,2,6,7,8,3,4,5 }; + ritorno(get_integer(nv, st_cfg.mot[_motor(nv->index)].motor_map)); + nv->value_int = remap_axis[nv->value_int]; return(STAT_OK); } -stat_t st_set_sa(nvObj_t *nv) // motor step angle +/* + * st_set_ma() - set motor axis mapping + * + * Legacy axis numbers are XYZABC for axis 0-5 + * External axis numbers are XYZABCUVW for axis 0-8 + * Internal axis numbers are XYZUVWABC for axis 0-8 (for various code reasons) + * + * This function accepts an external axis number and remaps it to an external axis number, + * writes the internal axis number and returns the external number in the JSON response. + */ +stat_t st_set_ma(nvObj_t *nv) { - if (nv->value <= 0) { + if (nv->value_int < 0) { nv->valuetype = TYPE_NULL; return (STAT_INPUT_LESS_THAN_MIN_VALUE); } - if (nv->value >= 360) { + if (nv->value_int > AXES) { nv->valuetype = TYPE_NULL; return (STAT_INPUT_EXCEEDS_MAX_VALUE); } - set_flt(nv); + uint8_t external_axis = nv->value_int; + uint8_t remap_axis[9] = { 0,1,2,6,7,8,3,4,5 }; + nv->value_int = remap_axis[nv->value_int]; + ritorno(set_integer(nv, st_cfg.mot[_motor(nv->index)].motor_map, 0, AXES)); + nv->value_int = external_axis; + return(STAT_OK); +} + +// step angle +stat_t st_get_sa(nvObj_t *nv) { return(get_float(nv, st_cfg.mot[_motor(nv->index)].step_angle)); } +stat_t st_set_sa(nvObj_t *nv) +{ + ritorno(set_float_range(nv, st_cfg.mot[_motor(nv->index)].step_angle, 0.001, 360)); _set_motor_steps_per_unit(nv); return(STAT_OK); } -stat_t st_set_tr(nvObj_t *nv) // motor travel per revolution +// travel per revolution +stat_t st_get_tr(nvObj_t *nv) { return(get_float(nv, st_cfg.mot[_motor(nv->index)].travel_rev)); } +stat_t st_set_tr(nvObj_t *nv) { - if (nv->value <= 0) { - nv->valuetype = TYPE_NULL; - return (STAT_INPUT_LESS_THAN_MIN_VALUE); - } - set_flu(nv); + ritorno(set_float_range(nv, st_cfg.mot[_motor(nv->index)].travel_rev, 0.0001, 1000000)); _set_motor_steps_per_unit(nv); return(STAT_OK); } -stat_t st_set_mi(nvObj_t *nv) // motor microsteps +// microsteps +stat_t st_get_mi(nvObj_t *nv) { return(get_integer(nv, st_cfg.mot[_motor(nv->index)].microsteps)); } +stat_t st_set_mi(nvObj_t *nv) { - if (nv->value <= 0) { + if (nv->value_int <= 0) { nv->valuetype = TYPE_NULL; return (STAT_INPUT_LESS_THAN_MIN_VALUE); } - uint16_t mi = nv->value; + uint8_t mi = (uint8_t)nv->value_int; if ((mi != 1) && (mi != 2) && (mi != 4) && (mi != 8) && (mi != 16) && (mi != 32)) { nv_add_conditional_message((const char *)"*** WARNING *** Setting non-standard microstep value"); } - set_int(nv); // set it anyway, even if it's unsupported + // set it anyway, even if it's unsupported + ritorno(set_uint32(nv, st_cfg.mot[_motor(nv->index)].microsteps, 1, 255)); _set_motor_steps_per_unit(nv); - _set_hw_microsteps(_get_motor(nv->index), nv->value); + _set_hw_microsteps(_motor(nv->index), nv->value_int); return (STAT_OK); } -stat_t st_get_su(nvObj_t *nv) // motor steps per unit (direct) +// motor steps per unit (direct) +stat_t st_get_su(nvObj_t *nv) { - uint8_t m = _get_motor(nv->index); - nv->value = st_cfg.mot[m].steps_per_unit; - nv->valuetype = TYPE_FLOAT; - nv->precision = cfgArray[nv->index].precision; - return(STAT_OK); + return(get_float(nv, st_cfg.mot[_motor(nv->index)].steps_per_unit)); } -stat_t st_set_su(nvObj_t *nv) // motor steps per unit (direct) +stat_t st_set_su(nvObj_t *nv) { // Don't set a zero or negative value - just calculate based on sa, tr, and mi // This way, if STEPS_PER_UNIT is set to 0 it is unused and we get the computed value - uint8_t m = _get_motor(nv->index); - if(nv->value <= 0) { - nv->value = st_cfg.mot[m].steps_per_unit; - _set_motor_steps_per_unit(nv); + if(nv->value_flt <= 0) { + nv->value_flt = _set_motor_steps_per_unit(nv); return(STAT_OK); } // Do unit conversion here because it's a reciprocal value (rather than process_incoming_float()) if (cm_get_units_mode(MODEL) == INCHES) { - if (cm_get_axis_type(nv->index) == AXIS_TYPE_LINEAR) { - nv->value *= INCHES_PER_MM; + if (cm_get_axis_type(nv) == AXIS_TYPE_LINEAR) { + nv->value_flt *= INCHES_PER_MM; } } - set_flt(nv); + uint8_t m = _motor(nv->index); + st_cfg.mot[m].steps_per_unit = nv->value_flt; st_cfg.mot[m].units_per_step = 1.0/st_cfg.mot[m].steps_per_unit; // Scale TR so all the other values make sense // You could scale any one of the other values, but TR makes the most sense - st_cfg.mot[m].travel_rev = (360.0*st_cfg.mot[m].microsteps)/(st_cfg.mot[m].steps_per_unit*st_cfg.mot[m].step_angle); + st_cfg.mot[m].travel_rev = (360.0 * st_cfg.mot[m].microsteps) / + (st_cfg.mot[m].steps_per_unit * st_cfg.mot[m].step_angle); return(STAT_OK); } -stat_t st_set_pm(nvObj_t *nv) // set motor power mode -{ - if (nv->value < 0) { - nv->valuetype = TYPE_NULL; - return (STAT_INPUT_LESS_THAN_MIN_VALUE); - } - if (nv->value >= MOTOR_POWER_MODE_MAX_VALUE) { - nv->valuetype = TYPE_NULL; - return (STAT_INPUT_EXCEEDS_MAX_VALUE); - } - uint8_t motor = _get_motor(nv->index); - if (motor > MOTORS) { - nv->valuetype = TYPE_NULL; - return STAT_INPUT_VALUE_RANGE_ERROR; - }; +// polarity +stat_t st_get_po(nvObj_t *nv) { return(get_integer(nv, st_cfg.mot[_motor(nv->index)].polarity)); } +stat_t st_set_po(nvObj_t *nv) { return(set_integer(nv, st_cfg.mot[_motor(nv->index)].polarity, 0, 1)); } - // We do this *here* in order for this to take effect immediately. - // setPowerMode() sets the value and also executes it. - Motors[motor]->setPowerMode((stPowerMode)nv->value); +// power management mode +stat_t st_get_pm(nvObj_t *nv) +{ + nv->value_int = (float)Motors[_motor(nv->index)]->getPowerMode(); + nv->valuetype = TYPE_INTEGER; return (STAT_OK); } -stat_t st_get_pm(nvObj_t *nv) // get motor power mode +stat_t st_set_pm(nvObj_t *nv) { - uint8_t motor = _get_motor(nv->index); - if (motor > MOTORS) { - nv->valuetype = TYPE_NULL; - return STAT_INPUT_VALUE_RANGE_ERROR; - }; - - nv->value = (float)Motors[motor]->getPowerMode(); - nv->valuetype = TYPE_INT; + // Test the value without setting it, then setPowerMode() now + // to both set and take effect immediately. + ritorno(set_integer(nv, (uint8_t &)cs.null, 0, MOTOR_POWER_MODE_MAX_VALUE )); + Motors[_motor(nv->index)]->setPowerMode((stPowerMode)nv->value_int); return (STAT_OK); } /* + * st_get_pl() - get motor power level * st_set_pl() - set motor power level * * Input value may vary from 0.000 to 1.000 The setting is scaled to allowable PWM range. * This function sets both the scaled and dynamic power levels, and applies the * scaled value to the vref. */ -stat_t st_set_pl(nvObj_t *nv) // motor power level +stat_t st_get_pl(nvObj_t *nv) { return(get_float(nv, st_cfg.mot[_motor(nv->index)].power_level)); } +stat_t st_set_pl(nvObj_t *nv) { - if (nv->value < (float)0.0) { - nv->valuetype = TYPE_NULL; - return (STAT_INPUT_LESS_THAN_MIN_VALUE); - } - if (nv->value > (float)1.0) { - nv->valuetype = TYPE_NULL; - return (STAT_INPUT_EXCEEDS_MAX_VALUE); - } - set_flt(nv); // set power_setting value in the motor config struct (st) - - uint8_t motor = _get_motor(nv->index); - st_cfg.mot[motor].power_level = nv->value; - st_run.mot[motor].power_level_dynamic = (st_cfg.mot[motor].power_level); - Motors[motor]->setPowerLevel(st_cfg.mot[motor].power_level); - + uint8_t m = _motor(nv->index); + ritorno(set_float_range(nv, st_cfg.mot[m].power_level, 0.0, 1.0)); + st_cfg.mot[m].power_level = nv->value_flt; + st_run.mot[m].power_level_dynamic = st_cfg.mot[m].power_level; + Motors[m]->setPowerLevel(st_cfg.mot[m].power_level); return(STAT_OK); } @@ -1122,43 +1108,87 @@ stat_t st_get_pwr(nvObj_t *nv) uint8_t motor = (cfgArray[nv->index].token[3] & 0x0F) - 1; if (motor > MOTORS) { return STAT_INPUT_VALUE_RANGE_ERROR; }; - nv->value = Motors[motor]->getCurrentPowerLevel(motor); + nv->value_flt = Motors[motor]->getCurrentPowerLevel(motor); nv->valuetype = TYPE_FLOAT; nv->precision = cfgArray[nv->index].precision; return (STAT_OK); } +stat_t st_set_ep(nvObj_t *nv) // set motor enable polarity +{ + if (nv->value_int < IO_ACTIVE_LOW) { return (STAT_INPUT_LESS_THAN_MIN_VALUE); } + if (nv->value_int > IO_ACTIVE_HIGH) { return (STAT_INPUT_EXCEEDS_MAX_VALUE); } + + uint8_t motor = _motor(nv->index); + if (motor > MOTORS) { return STAT_INPUT_VALUE_RANGE_ERROR; }; + + Motors[motor]->setEnablePolarity((ioPolarity)nv->value_int); + return (STAT_OK); +} + +stat_t st_get_ep(nvObj_t *nv) // get motor enable polarity +{ + if (nv->value_int < IO_ACTIVE_LOW) { return (STAT_INPUT_LESS_THAN_MIN_VALUE); } + if (nv->value_int > IO_ACTIVE_HIGH) { return (STAT_INPUT_EXCEEDS_MAX_VALUE); } + + uint8_t motor = _motor(nv->index); + if (motor > MOTORS) { return STAT_INPUT_VALUE_RANGE_ERROR; }; + + nv->value_int = (float)Motors[motor]->getEnablePolarity(); + nv->valuetype = TYPE_INTEGER; + return (STAT_OK); +} + +stat_t st_set_sp(nvObj_t *nv) // set motor step polarity +{ + if (nv->value_int < IO_ACTIVE_LOW) { return (STAT_INPUT_LESS_THAN_MIN_VALUE); } + if (nv->value_int > IO_ACTIVE_HIGH) { return (STAT_INPUT_EXCEEDS_MAX_VALUE); } + + uint8_t motor = _motor(nv->index); + if (motor > MOTORS) { return STAT_INPUT_VALUE_RANGE_ERROR; }; + + Motors[motor]->setStepPolarity((ioPolarity)nv->value_int); + return (STAT_OK); +} + +stat_t st_get_sp(nvObj_t *nv) // get motor step polarity +{ + if (nv->value_int < IO_ACTIVE_LOW) { return (STAT_INPUT_LESS_THAN_MIN_VALUE); } + if (nv->value_int > IO_ACTIVE_HIGH) { return (STAT_INPUT_EXCEEDS_MAX_VALUE); } + + uint8_t motor = _motor(nv->index); + if (motor > MOTORS) { return STAT_INPUT_VALUE_RANGE_ERROR; }; + + nv->value_int = (float)Motors[motor]->getStepPolarity(); + nv->valuetype = TYPE_INTEGER; + return (STAT_OK); +} + /* GLOBAL FUNCTIONS (SYSTEM LEVEL) * - * st_set_mt() - set global motor timeout in seconds + * st_get_mt() - get motor timeout in seconds + * st_set_mt() - set motor timeout in seconds + * st_set_md() - disable motor power * st_set_me() - enable motor power * st_set_md() - disable motor power + * st_get_dw() - get remaining dwell time * * Calling me or md with NULL will enable or disable all motors * Setting a value of 0 will enable or disable all motors * Setting a value from 1 to MOTORS will enable or disable that motor only */ -stat_t st_set_mt(nvObj_t *nv) -{ - if (nv->value < MOTOR_TIMEOUT_SECONDS_MIN) { - nv->valuetype = TYPE_NULL; - return (STAT_INPUT_LESS_THAN_MIN_VALUE); - } - if (nv->value > MOTOR_TIMEOUT_SECONDS_MAX) { - nv->valuetype = TYPE_NULL; - return (STAT_INPUT_EXCEEDS_MAX_VALUE); - } - st_cfg.motor_power_timeout = nv->value; - return (STAT_OK); -} +stat_t st_get_mt(nvObj_t *nv) { return(get_float(nv, st_cfg.motor_power_timeout)); } +stat_t st_set_mt(nvObj_t *nv) { return(set_float_range(nv, st_cfg.motor_power_timeout, + MOTOR_TIMEOUT_SECONDS_MIN, + MOTOR_TIMEOUT_SECONDS_MAX)); } // Make sure this function is not part of initialization --> f00 // nv->value is seconds of timeout stat_t st_set_me(nvObj_t *nv) { for (uint8_t motor = MOTOR_1; motor < MOTORS; motor++) { - Motors[motor]->enable(nv->value); // nv->value is the timeout or 0 for default + Motors[motor]->enable(nv->value_int); // nv->value is the timeout or 0 for default } return (STAT_OK); } @@ -1167,25 +1197,32 @@ stat_t st_set_me(nvObj_t *nv) // nv-value is motor to disable, or 0 for all motors stat_t st_set_md(nvObj_t *nv) { - if (nv->value < 0) { + if (nv->value_int < 0) { nv->valuetype = TYPE_NULL; return (STAT_INPUT_LESS_THAN_MIN_VALUE); } - if (nv->value > MOTORS) { + if (nv->value_int > MOTORS) { nv->valuetype = TYPE_NULL; return (STAT_INPUT_EXCEEDS_MAX_VALUE); } // de-energize all motors - if ((uint8_t)nv->value == 0) { // 0 means all motors + if ((uint8_t)nv->value_int == 0) { // 0 means all motors for (uint8_t motor = MOTOR_1; motor < MOTORS; motor++) { Motors[motor]->disable(); } } else { // otherwise it's just one motor - Motors[(uint8_t)nv->value -1]->disable(); + Motors[(uint8_t)nv->value_int -1]->disable(); } return (STAT_OK); } +stat_t st_get_dw(nvObj_t *nv) +{ + nv->value_int = st_run.dwell_ticks_downcount; + nv->valuetype = TYPE_INTEGER; + return (STAT_OK); +} + /*********************************************************************************** * TEXT MODE SUPPORT * Functions to print variables from the cfgArray table @@ -1193,13 +1230,11 @@ stat_t st_set_md(nvObj_t *nv) #ifdef __TEXT_MODE -#ifndef COMPILING_ALL_AT_ONCE static const char msg_units0[] = " in"; // used by generic print functions static const char msg_units1[] = " mm"; static const char msg_units2[] = " deg"; static const char *const msg_units[] = { msg_units0, msg_units1, msg_units2 }; #define DEGREE_INDEX 2 -#endif // COMPILING_ALL_AT_ONCE static const char fmt_me[] = "motors energized\n"; static const char fmt_md[] = "motors de-energized\n"; @@ -1210,6 +1245,8 @@ static const char fmt_0tr[] = "[%s%s] m%s travel per revolution%10.4f%s\n"; static const char fmt_0mi[] = "[%s%s] m%s microsteps%16d [1,2,4,8,16,32]\n"; static const char fmt_0su[] = "[%s%s] m%s steps per unit %17.5f steps per%s\n"; static const char fmt_0po[] = "[%s%s] m%s polarity%18d [0=normal,1=reverse]\n"; +static const char fmt_0ep[] = "[%s%s] m%s enable polarity%11d [0=active HIGH,1=active LOW]\n"; +static const char fmt_0sp[] = "[%s%s] m%s step polarity%13d [0=active HIGH,1=active LOW]\n"; static const char fmt_0pm[] = "[%s%s] m%s power management%10d [0=disabled,1=always on,2=in cycle,3=when moving]\n"; static const char fmt_0pl[] = "[%s%s] m%s motor power level%13.3f [0.000=minimum, 1.000=maximum]\n"; static const char fmt_pwr[] = "[%s%s] Motor %c power level:%12.3f\n"; @@ -1220,25 +1257,25 @@ void st_print_mt(nvObj_t *nv) { text_print(nv, fmt_mt);} // TYPE_FLOAT static void _print_motor_int(nvObj_t *nv, const char *format) { - sprintf(cs.out_buf, format, nv->group, nv->token, nv->group, (int)nv->value); + sprintf(cs.out_buf, format, nv->group, nv->token, nv->group, (int)nv->value_int); xio_writeline(cs.out_buf); } static void _print_motor_flt(nvObj_t *nv, const char *format) { - sprintf(cs.out_buf, format, nv->group, nv->token, nv->group, nv->value); + sprintf(cs.out_buf, format, nv->group, nv->token, nv->group, nv->value_flt); xio_writeline(cs.out_buf); } static void _print_motor_flt_units(nvObj_t *nv, const char *format, uint8_t units) { - sprintf(cs.out_buf, format, nv->group, nv->token, nv->group, nv->value, GET_TEXT_ITEM(msg_units, units)); + sprintf(cs.out_buf, format, nv->group, nv->token, nv->group, nv->value_flt, GET_TEXT_ITEM(msg_units, units)); xio_writeline(cs.out_buf); } static void _print_motor_pwr(nvObj_t *nv, const char *format) { - sprintf(cs.out_buf, format, nv->group, nv->token, nv->token[0], nv->value); + sprintf(cs.out_buf, format, nv->group, nv->token, nv->token[0], nv->value_flt); xio_writeline(cs.out_buf); } @@ -1248,6 +1285,8 @@ void st_print_tr(nvObj_t *nv) { _print_motor_flt_units(nv, fmt_0tr, cm_get_units void st_print_mi(nvObj_t *nv) { _print_motor_int(nv, fmt_0mi);} void st_print_su(nvObj_t *nv) { _print_motor_flt_units(nv, fmt_0su, cm_get_units_mode(MODEL));} void st_print_po(nvObj_t *nv) { _print_motor_int(nv, fmt_0po);} +void st_print_ep(nvObj_t *nv) { _print_motor_int(nv, fmt_0ep);} +void st_print_sp(nvObj_t *nv) { _print_motor_int(nv, fmt_0sp);} void st_print_pm(nvObj_t *nv) { _print_motor_int(nv, fmt_0pm);} void st_print_pl(nvObj_t *nv) { _print_motor_flt(nv, fmt_0pl);} void st_print_pwr(nvObj_t *nv){ _print_motor_pwr(nv, fmt_pwr);} diff --git a/g2core/stepper.h b/g2core/stepper.h index 1f895b08..9123d87f 100644 --- a/g2core/stepper.h +++ b/g2core/stepper.h @@ -2,8 +2,8 @@ * stepper.h - stepper motor interface * This file is part of g2core project * - * Copyright (c) 2010 - 2016 Alden S. Hart, Jr. - * Copyright (c) 2013 - 2016 Robert Giseburt + * Copyright (c) 2010 - 2019 Alden S. Hart, Jr. + * Copyright (c) 2013 - 2019 Robert Giseburt * * This file ("the software") is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License, version 2 as published by the @@ -255,6 +255,8 @@ #include "planner.h" // planner.h must precede stepper.h for moveType typedef +#include "gpio.h" // for ioPolarity + // Note: "board_stepper.h" is inlcluded at the end of this file /********************************* @@ -276,12 +278,12 @@ typedef enum { // used w/start and stop flags to sequen } stPowerState; typedef enum { - MOTOR_DISABLED = 0, // [0] motor enable is deactivated - MOTOR_ALWAYS_POWERED, // [1] motor is always powered while machine is ON - MOTOR_POWERED_IN_CYCLE, // [2] motor fully powered during cycles, de-powered out of cycle - MOTOR_POWERED_ONLY_WHEN_MOVING, // [3] motor only powered while moving - idles shortly after it's stopped - even in cycle - MOTOR_POWER_MODE_MAX_VALUE // [4] for input range checking + MOTOR_DISABLED = 0, // motor enable is deactivated + MOTOR_ALWAYS_POWERED, // motor is always powered while machine is ON + MOTOR_POWERED_IN_CYCLE, // motor fully powered during cycles, de-powered out of cycle + MOTOR_POWERED_ONLY_WHEN_MOVING // motor only powered while moving - idles shortly after it's stopped - even in cycle } stPowerMode; +#define MOTOR_POWER_MODE_MAX_VALUE MOTOR_POWERED_ONLY_WHEN_MOVING // Min/Max timeouts allowed for motor disable. Allow for inertial stop; must be non-zero #define MOTOR_TIMEOUT_SECONDS_MIN (float)0.1 // seconds !!! SHOULD NEVER BE ZERO !!! @@ -424,6 +426,7 @@ extern stPrepSingleton_t st_pre HOT_DATA; // only used by config_app diagnosti /**** Stepper (base object) ****/ struct Stepper { +protected: Timeout _motor_disable_timeout; // this is the timeout object that will let us know when time is u uint32_t _motor_disable_timeout_ms; // the number of ms that the timeout is reset to stPowerState _power_state; // state machine for managing motor power @@ -431,7 +434,7 @@ struct Stepper { bool _was_enabled; // store if we enabled a motor to handle timout setup outside of load /* stepper default values */ - +public: // sets default pwm freq for all motor vrefs (commented line below also sets HiZ) Stepper(const uint32_t frequency = 500000) { @@ -454,6 +457,26 @@ struct Stepper { } }; + virtual ioPolarity getEnablePolarity() const + { + return IO_ACTIVE_LOW; // we have to say something here + }; + + virtual void setEnablePolarity(ioPolarity new_mp) + { + // do nothing + }; + + virtual ioPolarity getStepPolarity() const + { + return IO_ACTIVE_LOW; // we have to say something here + }; + + virtual void setStepPolarity(ioPolarity new_mp) + { + // do nothing + }; + virtual stPowerMode getPowerMode() { return _power_mode; @@ -574,6 +597,22 @@ struct Stepper { virtual void setPowerLevel(float new_pl) { /* must override */ }; }; +/**** ExternalEncoder (base object) ****/ + +class ExternalEncoder { + public: + using callback_t = std::function; + enum ReturnFormat { ReturnDegrees, ReturnRadians, ReturnFraction }; + + virtual void setCallback(std::function &&handler); + virtual void setCallback(std::function &handler); + + virtual void requestAngleDegrees(); + virtual void requestAngleRadians(); + virtual void requestAngleFraction(); + + virtual float getQuadratureFraction(); +}; /**** FUNCTION PROTOTYPES ****/ @@ -593,25 +632,41 @@ void st_request_load_move(void) HOT_FUNC; void st_prep_null(void); void st_prep_command(void *bf); // use a void pointer since we don't know about mpBuf_t yet) void st_prep_dwell(float milliseconds); -void st_request_out_of_band_dwell(float microseconds); +void st_prep_out_of_band_dwell(float milliseconds); stat_t st_prep_line(const float start_velocity, const float end_velocity, const float travel_steps[], const float following_error[], const float segment_time) HOT_FUNC; // NOTE: this version is the same, except it's passed an array of start/end velocities, one pair per motor stat_t st_prep_line(const float start_velocities[], const float end_velocities[], const float travel_steps[], const float following_error[], const float segment_time) HOT_FUNC; +stat_t st_get_ma(nvObj_t *nv); stat_t st_set_ma(nvObj_t *nv); +stat_t st_get_sa(nvObj_t *nv); stat_t st_set_sa(nvObj_t *nv); +stat_t st_get_tr(nvObj_t *nv); stat_t st_set_tr(nvObj_t *nv); +stat_t st_get_mi(nvObj_t *nv); stat_t st_set_mi(nvObj_t *nv); stat_t st_get_su(nvObj_t *nv); stat_t st_set_su(nvObj_t *nv); -stat_t st_set_pm(nvObj_t *nv); + +stat_t st_get_po(nvObj_t *nv); +stat_t st_set_po(nvObj_t *nv); +stat_t st_set_ep(nvObj_t *nv); +stat_t st_get_ep(nvObj_t *nv); +stat_t st_set_sp(nvObj_t *nv); +stat_t st_get_sp(nvObj_t *nv); + stat_t st_get_pm(nvObj_t *nv); +stat_t st_set_pm(nvObj_t *nv); +stat_t st_get_pl(nvObj_t *nv); stat_t st_set_pl(nvObj_t *nv); + stat_t st_get_pwr(nvObj_t *nv); +stat_t st_get_mt(nvObj_t *nv); stat_t st_set_mt(nvObj_t *nv); stat_t st_set_md(nvObj_t *nv); stat_t st_set_me(nvObj_t *nv); +stat_t st_get_dw(nvObj_t *nv); #ifdef __TEXT_MODE @@ -621,6 +676,8 @@ stat_t st_set_me(nvObj_t *nv); void st_print_mi(nvObj_t *nv); void st_print_su(nvObj_t *nv); void st_print_po(nvObj_t *nv); + void st_print_ep(nvObj_t *nv); + void st_print_sp(nvObj_t *nv); void st_print_pm(nvObj_t *nv); void st_print_pl(nvObj_t *nv); void st_print_pwr(nvObj_t *nv); @@ -636,6 +693,8 @@ stat_t st_set_me(nvObj_t *nv); #define st_print_mi tx_print_stub #define st_print_su tx_print_stub #define st_print_po tx_print_stub + #define st_print_ep tx_print_stub + #define st_print_sp tx_print_stub #define st_print_pm tx_print_stub #define st_print_pl tx_print_stub #define st_print_pwr tx_print_stub diff --git a/g2core/temperature.cpp b/g2core/temperature.cpp old mode 100755 new mode 100644 index ec4c5f02..4e7b1cd3 --- a/g2core/temperature.cpp +++ b/g2core/temperature.cpp @@ -2,8 +2,8 @@ * temperature.cpp - temperature control module - drives heaters or coolers * This file is part of the g2core project * - * Copyright (c) 2016 Robert Giseburt - * Copyright (c) 2016 Alden S. Hart, Jr. + * Copyright (c) 2016 - 2019 Robert Giseburt + * Copyright (c) 2016 - 2019 Alden S. Hart, Jr. * * This file ("the software") is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License, version 2 as published by the @@ -38,6 +38,7 @@ #include "report.h" #include "util.h" #include "settings.h" +#include "gpio.h" // for ValueHistory /**** Local safety/limit settings ****/ @@ -843,7 +844,6 @@ HeaterFan heater_fan1; /* * temperature_init() - * temperature_init() - stop spindle, set speed to zero, and reset values */ void temperature_init() { @@ -887,7 +887,7 @@ const float kTempDiffSRTrigger = 0.25; stat_t temperature_callback() { - if (cm.machine_state == MACHINE_ALARM) { + if (cm->machine_state == MACHINE_ALARM) { // Force the heaters off (redundant with the safety circuit) fet_pin1 = 0.0; fet_pin2 = 0.0; @@ -961,9 +961,13 @@ stat_t temperature_callback() * CONFIGURATION AND INTERFACE FUNCTIONS * Functions to get and set variables from the cfgArray table ***********************************************************************************/ +/* In these function there are usually 3 PIDs, so a simple switch works. + * The default is a failsafe - can only get there if it's set up in config_app, but not here. + */ -// In these functions, nv->group == "he1", "he2", or "he3" -char _get_heater_number(nvObj_t *nv) { +// helpers + +char _get_heater_number(nvObj_t *nv) { // In these functions nv->group == "he1", "he2", or "he3" if (!nv->group[0]) { return nv->token[2]; } @@ -973,32 +977,28 @@ char _get_heater_number(nvObj_t *nv) { stat_t cm_get_heater_enable(nvObj_t *nv) { switch(_get_heater_number(nv)) { - case '1': { nv->value = pid1._enable; break; } - case '2': { nv->value = pid2._enable; break; } - case '3': { nv->value = pid3._enable; break; } - // Failsafe. We can only get here if we set it up in config_app, but not here. + case '1': { nv->value_int = pid1._enable; break; } + case '2': { nv->value_int = pid2._enable; break; } + case '3': { nv->value_int = pid3._enable; break; } default: { return(STAT_INPUT_VALUE_RANGE_ERROR); break; } } - nv->valuetype = TYPE_BOOL; + nv->valuetype = TYPE_BOOLEAN; return (STAT_OK); } stat_t cm_set_heater_enable(nvObj_t *nv) { bool enable = false; - if (nv->value > 1) { // testing a boolean value + if (nv-> value_int > 1) { // testing a boolean value return (STAT_INPUT_VALUE_RANGE_ERROR); } - if (nv->value > 0.1) { + if (nv-> value_int > 0.1) { enable = true; } // The above manipulation of 'enable' was necessary because the compiler won't accept this cast: // pid1._enable = (bool)nv->value; // says it's unsafe to compare ==, != an FP number switch(_get_heater_number(nv)) { - // case '1': { pid1._enable = (bool)nv->value; break; } - // case '2': { pid2._enable = (bool)nv->value; break; } - // case '3': { pid3._enable = (bool)nv->value; break; } case '1': { pid1._enable = enable; break; } case '2': { pid2._enable = enable; break; } case '3': { pid3._enable = enable; break; } @@ -1007,110 +1007,99 @@ stat_t cm_set_heater_enable(nvObj_t *nv) return (STAT_OK); } -/* - * cm_get_heater_p()/cm_set_heater_p() - get/set the P parameter of the PID +/**************************************************************************************** + * cm_get_heater_p() - set the P parameter of the PID + * cm_set_heater_p() - set the P parameter of the PID + * cm_get_heater_i() - set the I parameter of the PID + * cm_set_heater_i() - set the I parameter of the PID + * cm_get_heater_d() - set the D parameter of the PID + * cm_set_heater_d() - set the D parameter of the PID */ + stat_t cm_get_heater_p(nvObj_t *nv) { - // there are three of them, so we can use a simple switch - switch(_get_heater_number(nv)) { - case '1': { nv->value = pid1._p_factor * 100.0; break; } - case '2': { nv->value = pid2._p_factor * 100.0; break; } - case '3': { nv->value = pid3._p_factor * 100.0; break; } - - // Failsafe. We can only get here if we set it up in config_app, but not here. - default: { nv->value = 0.0; break; } + switch(_get_heater_number(nv)) { // there are three of them, so we can use a simple switch + case '1': { nv->value_flt = pid1._p_factor * 100.0; break; } + case '2': { nv->value_flt = pid2._p_factor * 100.0; break; } + case '3': { nv->value_flt = pid3._p_factor * 100.0; break; } + default: { nv->value_flt = 0.0; break; } } nv->precision = GET_TABLE_WORD(precision); nv->valuetype = TYPE_FLOAT; - return (STAT_OK); } stat_t cm_set_heater_p(nvObj_t *nv) { switch(_get_heater_number(nv)) { - case '1': { pid1._p_factor = nv->value / 100.0; break; } - case '2': { pid2._p_factor = nv->value / 100.0; break; } - case '3': { pid3._p_factor = nv->value / 100.0; break; } - + case '1': { pid1._p_factor = nv->value_flt / 100.0; break; } + case '2': { pid2._p_factor = nv->value_flt / 100.0; break; } + case '3': { pid3._p_factor = nv->value_flt / 100.0; break; } default: { break; } } - return (STAT_OK); } -/* - * cm_get_heater_i()/cm_set_heater_i() - get/set the I parameter of the PID - */ stat_t cm_get_heater_i(nvObj_t *nv) { - // there are three of them, so we can use a simple switch switch(_get_heater_number(nv)) { - case '1': { nv->value = pid1._i_factor * 100.0; break; } - case '2': { nv->value = pid2._i_factor * 100.0; break; } - case '3': { nv->value = pid3._i_factor * 100.0; break; } - - default: { nv->value = 0.0; break; } + case '1': { nv->value_flt = pid1._i_factor * 100.0; break; } + case '2': { nv->value_flt = pid2._i_factor * 100.0; break; } + case '3': { nv->value_flt = pid3._i_factor * 100.0; break; } + default: { nv->value_flt = 0.0; break; } } - nv->precision = GET_TABLE_WORD(precision); nv->valuetype = TYPE_FLOAT; - return (STAT_OK); } + stat_t cm_set_heater_i(nvObj_t *nv) { switch(_get_heater_number(nv)) { - case '1': { pid1._i_factor = nv->value / 100.0; break; } - case '2': { pid2._i_factor = nv->value / 100.0; break; } - case '3': { pid3._i_factor = nv->value / 100.0; break; } - + case '1': { pid1._i_factor = nv->value_flt / 100.0; break; } + case '2': { pid2._i_factor = nv->value_flt / 100.0; break; } + case '3': { pid3._i_factor = nv->value_flt / 100.0; break; } default: { break; } } - return (STAT_OK); } -/* - * cm_get_heater_d()/cm_set_heater_d() - get/set the D parameter of the PID - */ stat_t cm_get_heater_d(nvObj_t *nv) { switch(_get_heater_number(nv)) { - case '1': { nv->value = pid1._d_factor * 100.0; break; } - case '2': { nv->value = pid2._d_factor * 100.0; break; } - case '3': { nv->value = pid3._d_factor * 100.0; break; } - - default: { nv->value = 0.0; break; } + case '1': { nv->value_flt = pid1._d_factor * 100.0; break; } + case '2': { nv->value_flt = pid2._d_factor * 100.0; break; } + case '3': { nv->value_flt = pid3._d_factor * 100.0; break; } + default: { nv->value_flt = 0.0; break; } } nv->precision = GET_TABLE_WORD(precision); nv->valuetype = TYPE_FLOAT; - return (STAT_OK); } stat_t cm_set_heater_d(nvObj_t *nv) { switch(_get_heater_number(nv)) { - case '1': { pid1._d_factor = nv->value / 100.0; break; } - case '2': { pid2._d_factor = nv->value / 100.0; break; } - case '3': { pid3._d_factor = nv->value / 100.0; break; } - + case '1': { pid1._d_factor = nv->value_flt / 100.0; break; } + case '2': { pid2._d_factor = nv->value_flt / 100.0; break; } + case '3': { pid3._d_factor = nv->value_flt / 100.0; break; } default: { break; } } return (STAT_OK); } -/* - * cm_get_heater_f()/cm_set_heater_f() - get/set the F parameter of the PIDF +/**************************************************************************************** + * cm_get_heater_f()- get the F parameter of the PIDF + * cm_set_heater_f()- set the F parameter of the PIDF + * + * There are both the file-to-file use version, and the NV-pair form (which uses the other). */ stat_t cm_get_heater_f(nvObj_t *nv) { switch(_get_heater_number(nv)) { - case '1': { nv->value = pid1._f_factor * 100.0; break; } - case '2': { nv->value = pid2._f_factor * 100.0; break; } - case '3': { nv->value = pid3._f_factor * 100.0; break; } + case '1': { nv->value_flt = pid1._f_factor * 100.0; break; } + case '2': { nv->value_flt = pid2._f_factor * 100.0; break; } + case '3': { nv->value_flt = pid3._f_factor * 100.0; break; } - default: { nv->value = 0.0; break; } + default: { nv->value_flt = 0.0; break; } } nv->precision = GET_TABLE_WORD(precision); nv->valuetype = TYPE_FLOAT; @@ -1120,20 +1109,22 @@ stat_t cm_get_heater_f(nvObj_t *nv) stat_t cm_set_heater_f(nvObj_t *nv) { switch(_get_heater_number(nv)) { - case '1': { pid1._f_factor = nv->value / 100.0; break; } - case '2': { pid2._f_factor = nv->value / 100.0; break; } - case '3': { pid3._f_factor = nv->value / 100.0; break; } + case '1': { pid1._f_factor = nv->value_flt / 100.0; break; } + case '2': { pid2._f_factor = nv->value_flt / 100.0; break; } + case '3': { pid3._f_factor = nv->value_flt / 100.0; break; } default: { break; } } return (STAT_OK); } -/* - * cm_get_set_temperature()/cm_set_set_temperature() - get/set the set value of the PID +/**************************************************************************************** + * cm_get_set_temperature() - get the set value of the PID + * cm_set_set_temperature() - set the set value of the PID * - * There are both the file-to-file use version, and the NV-pair form (which uses the other). + * There are both the file-to-file use version, and the NV-pair form (which uses the other). */ + float cm_get_set_temperature(const uint8_t heater) { switch(heater) { @@ -1142,17 +1133,17 @@ float cm_get_set_temperature(const uint8_t heater) case 3: { return pid3._set_point; break; } default: { break; } } - return 0.0; } + stat_t cm_get_set_temperature(nvObj_t *nv) { - nv->value = cm_get_set_temperature(_get_heater_number(nv) - '0'); + nv->value_flt = cm_get_set_temperature(_get_heater_number(nv) - '0'); nv->precision = GET_TABLE_WORD(precision); nv->valuetype = TYPE_FLOAT; - return (STAT_OK); } + void cm_set_set_temperature(const uint8_t heater, const float value) { switch(heater) { @@ -1166,31 +1157,31 @@ void cm_set_set_temperature(const uint8_t heater, const float value) } stat_t cm_set_set_temperature(nvObj_t *nv) { - cm_set_set_temperature(_get_heater_number(nv) - '0', nv->value); + cm_set_set_temperature(_get_heater_number(nv) - '0', nv->value_flt); return (STAT_OK); } -/* - * cm_get_fan_power()/cm_set_fan_power() - get/set the set high-value setting of the heater fan +/**************************************************************************************** + * cm_get_fan_power() - get the set high-value setting of the heater fan + * cm_set_fan_power() - set the set high-value setting of the heater fan */ + float cm_get_fan_power(const uint8_t heater) { switch(heater) { case 1: { return min(1.0f, heater_fan1.max_value); } -// case 2: { return min(1.0f, heater_fan2.max_value); } -// case 3: { return min(1.0f, heater_fan3.max_value); } - +// case 2: { return min(1.0f, heater_fan2.max_value); } +// case 3: { return min(1.0f, heater_fan3.max_value); } default: { break; } } - return 0.0; } + stat_t cm_get_fan_power(nvObj_t *nv) { - nv->value = cm_get_fan_power(_get_heater_number(nv) - '0'); + nv->value_flt = cm_get_fan_power(_get_heater_number(nv) - '0'); nv->precision = GET_TABLE_WORD(precision); nv->valuetype = TYPE_FLOAT; - return (STAT_OK); } @@ -1198,177 +1189,170 @@ void cm_set_fan_power(const uint8_t heater, const float value) { switch(heater) { case 1: { heater_fan1.max_value = max(0.0f, value); break; } -// case 2: { heater_fan2.max_value = max(0.0, value); break; } -// case 3: { heater_fan3.max_value = max(0.0, value); break; } +// case 2: { heater_fan2.max_value = max(0.0, value); break; } +// case 3: { heater_fan3.max_value = max(0.0, value); break; } default: { break; } } } + stat_t cm_set_fan_power(nvObj_t *nv) { - cm_set_fan_power(_get_heater_number(nv) - '0', nv->value); + cm_set_fan_power(_get_heater_number(nv) - '0', nv->value_flt); return (STAT_OK); } -/* - * cm_get_fan_min_power()/cm_set_fan_min_power() - get/set the set high-value setting of the heater fan +/**************************************************************************************** + * cm_get_fan_min_power() - get the set low-value setting of the heater fan + * cm_set_fan_min_power() - set the set low-value setting of the heater fan */ + stat_t cm_get_fan_min_power(nvObj_t *nv) { switch(_get_heater_number(nv)) { - case '1': { nv->value = heater_fan1.min_value; break; } -// case '2': { nv->value = heater_fan2.min_value; break; } -// case '3': { nv->value = heater_fan3.min_value; break; } - - default: { nv->value = 0.0; break; } + case '1': { nv->value_flt = heater_fan1.min_value; break; } +// case '2': { nv->value_flt = heater_fan2.min_value; break; } +// case '3': { nv->value_flt = heater_fan3.min_value; break; } + default: { nv->value_flt = 0.0; break; } } - nv->precision = GET_TABLE_WORD(precision); nv->valuetype = TYPE_FLOAT; - return (STAT_OK); } + stat_t cm_set_fan_min_power(nvObj_t *nv) { switch(_get_heater_number(nv)) { - case '1': { heater_fan1.max_value = min(0.0f, nv->value); break; } -// case '2': { heater_fan2.min_value = min(0.0, nv->value); break; } -// case '3': { heater_fan3.min_value = min(0.0, nv->value); break; } - + case '1': { heater_fan1.max_value = min(0.0f, nv->value_flt); break; } +// case '2': { heater_fan2.min_value = min(0.0, nv->value_flt); break; } +// case '3': { heater_fan3.min_value = min(0.0, nv->value_flt); break; } default: { break; } } - return (STAT_OK); } -/* - * cm_get_fan_low_temp()/cm_set_fan_low_temp() - get/set the set high-value setting of the heater fan +/**************************************************************************************** + * cm_get_fan_low_temp() - get the set low-temp setting of the heater fan + * cm_set_fan_low_temp() - set the set low-temp setting of the heater fan */ + stat_t cm_get_fan_low_temp(nvObj_t *nv) { switch(_get_heater_number(nv)) { - case '1': { nv->value = heater_fan1.low_temp; break; } -// case '2': { nv->value = heater_fan2.low_temp; break; } -// case '3': { nv->value = heater_fan3.low_temp; break; } - - default: { nv->value = 0.0; break; } + case '1': { nv->value_flt = heater_fan1.low_temp; break; } +// case '2': { nv->value_flt = heater_fan2.low_temp; break; } +// case '3': { nv->value_flt = heater_fan3.low_temp; break; } + default: { nv->value_flt = 0.0; break; } } - nv->precision = GET_TABLE_WORD(precision); nv->valuetype = TYPE_FLOAT; - return (STAT_OK); } + stat_t cm_set_fan_low_temp(nvObj_t *nv) { switch(_get_heater_number(nv)) { - case '1': { heater_fan1.low_temp = min(0.0f, nv->value); break; } -// case '2': { heater_fan2.low_temp = min(0.0f, nv->value); break; } -// case '3': { heater_fan3.low_temp = min(0.0f, nv->value); break; } - + case '1': { heater_fan1.low_temp = min(0.0f, nv->value_flt); break; } +// case '2': { heater_fan2.low_temp = min(0.0f, nv->value_flt); break; } +// case '3': { heater_fan3.low_temp = min(0.0f, nv->value_flt); break; } default: { break; } } - return (STAT_OK); } -/* - * cm_get_fan_high_temp()/cm_set_fan_high_temp() - get/set the set high-value setting of the heater fan +/**************************************************************************************** + * cm_get_fan_high_temp() - get the set high-value setting of the heater fan + * cm_set_fan_high_temp() - set the set high-value setting of the heater fan */ + stat_t cm_get_fan_high_temp(nvObj_t *nv) { switch(_get_heater_number(nv)) { - case '1': { nv->value = heater_fan1.high_temp; break; } -// case '2': { nv->value = heater_fan2.high_temp; break; } -// case '3': { nv->value = heater_fan3.high_temp; break; } - - default: { nv->value = 0.0; break; } + case '1': { nv->value_flt = heater_fan1.high_temp; break; } +// case '2': { nv->value_flt = heater_fan2.high_temp; break; } +// case '3': { nv->value_flt = heater_fan3.high_temp; break; } + default: { nv->value_flt = 0.0; break; } } - nv->precision = GET_TABLE_WORD(precision); nv->valuetype = TYPE_FLOAT; - return (STAT_OK); } + stat_t cm_set_fan_high_temp(nvObj_t *nv) { switch(_get_heater_number(nv)) { - case '1': { heater_fan1.high_temp = min(0.0f, nv->value); break; } -// case '2': { heater_fan2.high_temp = min(0.0f, nv->value); break; } -// case '3': { heater_fan3.high_temp = min(0.0f, nv->value); break; } - + case '1': { heater_fan1.high_temp = min(0.0f, nv->value_flt); break; } +// case '2': { heater_fan2.high_temp = min(0.0f, nv->value_flt); break; } +// case '3': { heater_fan3.high_temp = min(0.0f, nv->value_flt); break; } default: { break; } } - return (STAT_OK); } -/* - * cm_get_at_temperature() - get a boolean if the heater has reaced the set value of the PID +/**************************************************************************************** + * cm_get_at_temperature() - get a boolean if the heater has reached the set value of the PID */ + bool cm_get_at_temperature(const uint8_t heater) { switch(heater) { case 1: { return pid1._at_set_point; } case 2: { return pid2._at_set_point; } case 3: { return pid3._at_set_point; } - default: { break; } } - return false; } + stat_t cm_get_at_temperature(nvObj_t *nv) { - nv->value = cm_get_at_temperature(_get_heater_number(nv) - '0'); + nv->value_int = cm_get_at_temperature(_get_heater_number(nv) - '0'); nv->precision = GET_TABLE_WORD(precision); - nv->valuetype = TYPE_BOOL; - + nv->valuetype = TYPE_BOOLEAN; return (STAT_OK); } - -/* +/**************************************************************************************** * cm_get_heater_output() - get the output value (PWM duty cycle) of the PID */ + float cm_get_heater_output(const uint8_t heater) { switch(heater) { - case 1: { return pid1._average_output; } - case 2: { return pid2._average_output; } - case 3: { return pid3._average_output; } - + case 1: { return (float)fet_pin1; } + case 2: { return (float)fet_pin2; } + case 3: { return (float)fet_pin3; } default: { break; } } return 0.0; } + stat_t cm_get_heater_output(nvObj_t *nv) { - nv->value = cm_get_heater_output(_get_heater_number(nv) - '0'); + nv->value_flt = cm_get_heater_output(_get_heater_number(nv) - '0'); nv->precision = GET_TABLE_WORD(precision); nv->valuetype = TYPE_FLOAT; - return (STAT_OK); } -/* +/**************************************************************************************** * cm_get_heater_adc() - get the raw adc value of the PID */ + stat_t cm_get_heater_adc(nvObj_t *nv) { switch(_get_heater_number(nv)) { - case '1': { nv->value = (float)temperature_sensor_1.get_raw_value(); break; } - case '2': { nv->value = (float)temperature_sensor_2.get_raw_value(); break; } - case '3': { nv->value = (float)temperature_sensor_3.get_raw_value(); break; } + case '1': { nv->value_flt = (float)temperature_sensor_1.get_raw_value(); break; } + case '2': { nv->value_flt = (float)temperature_sensor_2.get_raw_value(); break; } + case '3': { nv->value_flt = (float)temperature_sensor_3.get_raw_value(); break; } - default: { nv->value = 0.0; break; } + default: { nv->value_flt = 0.0; break; } } nv->precision = GET_TABLE_WORD(precision); nv->valuetype = TYPE_FLOAT; - return (STAT_OK); } -/* +/**************************************************************************************** * cm_get_temperature() - get the current temperature */ float cm_get_temperature(const uint8_t heater) @@ -1385,24 +1369,24 @@ stat_t cm_get_heater_adc(nvObj_t *nv) } stat_t cm_get_temperature(nvObj_t *nv) { - nv->value = cm_get_temperature(_get_heater_number(nv) - '0'); + nv->value_flt = cm_get_temperature(_get_heater_number(nv) - '0'); nv->precision = GET_TABLE_WORD(precision); nv->valuetype = TYPE_FLOAT; - return (STAT_OK); } -/* +/**************************************************************************************** * cm_get_thermistor_resistance() - get the current temperature */ + stat_t cm_get_thermistor_resistance(nvObj_t *nv) { switch(_get_heater_number(nv)) { - case '1': { nv->value = temperature_sensor_1.get_resistance(); break; } - case '2': { nv->value = temperature_sensor_2.get_resistance(); break; } - case '3': { nv->value = temperature_sensor_3.get_resistance(); break; } + case '1': { nv->value_flt = temperature_sensor_1.get_resistance(); break; } + case '2': { nv->value_flt = temperature_sensor_2.get_resistance(); break; } + case '3': { nv->value_flt = temperature_sensor_3.get_resistance(); break; } - default: { nv->value = 0.0; break; } + default: { nv->value_flt = 0.0; break; } } nv->precision = GET_TABLE_WORD(precision); nv->valuetype = TYPE_FLOAT; @@ -1416,16 +1400,15 @@ stat_t cm_get_thermistor_resistance(nvObj_t *nv) stat_t cm_get_thermistor_voltage(nvObj_t* nv) { switch(_get_heater_number(nv)) { - case '1': { nv->value = temperature_sensor_1.get_voltage(); break; } - case '2': { nv->value = temperature_sensor_2.get_voltage(); break; } - case '3': { nv->value = temperature_sensor_3.get_voltage(); break; } + case '1': { nv->value_flt = temperature_sensor_1.get_voltage(); break; } + case '2': { nv->value_flt = temperature_sensor_2.get_voltage(); break; } + case '3': { nv->value_flt = temperature_sensor_3.get_voltage(); break; } // Failsafe. We can only get here if we set it up in config_app, but not here. - default: { nv->value = 0.0; break; } + default: { nv->value_flt = 0.0; break; } } nv->precision = GET_TABLE_WORD(precision); nv->valuetype = TYPE_FLOAT; - return (STAT_OK); } @@ -1440,71 +1423,66 @@ char _get_pid_number(nvObj_t *nv) { } -/* +/**************************************************************************************** * cm_get_pid_p() - get the active P of the PID (read-only) + * cm_get_pid_i() - get the active I of the PID (read-only) + * cm_get_pid_d() - get the active D of the PID (read-only) + * cm_get_pid_f() - get the active F of the PID (read-only) */ + stat_t cm_get_pid_p(nvObj_t *nv) { switch(_get_pid_number(nv)) { - case '1': { nv->value = pid1._proportional; break; } - case '2': { nv->value = pid2._proportional; break; } - case '3': { nv->value = pid3._proportional; break; } + case '1': { nv->value_flt = pid1._proportional; break; } + case '2': { nv->value_flt = pid2._proportional; break; } + case '3': { nv->value_flt = pid3._proportional; break; } - default: { nv->value = 0.0; break; } + // Failsafe. We can only get here if we set it up in config_app, but not here. + default: { nv->value_flt = 0.0; break; } } nv->precision = GET_TABLE_WORD(precision); nv->valuetype = TYPE_FLOAT; - return (STAT_OK); } -/* - * cm_get_pid_i() - get the active I of the PID (read-only) - */ stat_t cm_get_pid_i(nvObj_t *nv) { switch(_get_pid_number(nv)) { - case '1': { nv->value = pid1._integral; break; } - case '2': { nv->value = pid2._integral; break; } - case '3': { nv->value = pid3._integral; break; } + case '1': { nv->value_flt = pid1._integral; break; } + case '2': { nv->value_flt = pid2._integral; break; } + case '3': { nv->value_flt = pid3._integral; break; } - default: { nv->value = 0.0; break; } + // Failsafe. We can only get here if we set it up in config_app, but not here. + default: { nv->value_flt = 0.0; break; } } nv->precision = GET_TABLE_WORD(precision); nv->valuetype = TYPE_FLOAT; - return (STAT_OK); } -/* - * cm_get_pid_d() - get the active D of the PID (read-only) - */ stat_t cm_get_pid_d(nvObj_t *nv) { switch(_get_pid_number(nv)) { - case '1': { nv->value = pid1._derivative; break; } - case '2': { nv->value = pid2._derivative; break; } - case '3': { nv->value = pid3._derivative; break; } + case '1': { nv->value_flt = pid1._derivative; break; } + case '2': { nv->value_flt = pid2._derivative; break; } + case '3': { nv->value_flt = pid3._derivative; break; } - default: { nv->value = 0.0; break; } + // Failsafe. We can only get here if we set it up in config_app, but not here. + default: { nv->value_flt = 0.0; break; } } nv->precision = GET_TABLE_WORD(precision); nv->valuetype = TYPE_FLOAT; - return (STAT_OK); } -/* - * cm_get_pid_f() - get the active F of the PID (read-only) - */ stat_t cm_get_pid_f(nvObj_t *nv) { switch(_get_pid_number(nv)) { - case '1': { nv->value = pid1._feed_forward; break; } - case '2': { nv->value = pid2._feed_forward; break; } - case '3': { nv->value = pid3._feed_forward; break; } + case '1': { nv->value_flt = pid1._feed_forward; break; } + case '2': { nv->value_flt = pid2._feed_forward; break; } + case '3': { nv->value_flt = pid3._feed_forward; break; } - default: { nv->value = 0.0; break; } + default: { nv->value_flt = 0.0; break; } } nv->precision = GET_TABLE_WORD(precision); nv->valuetype = TYPE_FLOAT; diff --git a/g2core/temperature.h b/g2core/temperature.h old mode 100755 new mode 100644 index d6e6cdf7..83a23976 --- a/g2core/temperature.h +++ b/g2core/temperature.h @@ -37,8 +37,8 @@ void temperature_init(); void temperature_reset(); stat_t temperature_callback(); -stat_t cm_get_heater_enable(nvObj_t* nv); -stat_t cm_set_heater_enable(nvObj_t* nv); +stat_t cm_get_heater_enable(nvObj_t *nv); +stat_t cm_set_heater_enable(nvObj_t *nv); stat_t cm_get_heater_p(nvObj_t* nv); stat_t cm_set_heater_p(nvObj_t* nv); @@ -54,7 +54,7 @@ stat_t cm_get_pid_d(nvObj_t* nv); stat_t cm_get_pid_f(nvObj_t* nv); float cm_get_set_temperature(const uint8_t heater); -stat_t cm_get_set_temperature(nvObj_t* nv); +stat_t cm_get_set_temperature(nvObj_t *nv); void cm_set_set_temperature(const uint8_t heater, const float value); stat_t cm_set_set_temperature(nvObj_t* nv); diff --git a/g2core/text_parser.cpp b/g2core/text_parser.cpp old mode 100755 new mode 100644 index 505858aa..e4aa633c --- a/g2core/text_parser.cpp +++ b/g2core/text_parser.cpp @@ -2,7 +2,7 @@ * text_parser.cpp - text parser * This file is part of the g2core project * - * Copyright (c) 2010 - 2016 Alden S. Hart, Jr. + * Copyright (c) 2010 - 2018 Alden S. Hart, Jr. * * This file ("the software") is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License, version 2 as published by the @@ -121,9 +121,10 @@ static stat_t _text_parser_kernal(char *str, nvObj_t *nv) *rd = NUL; // terminate at end of name strncpy(nv->token, str, TOKEN_LEN); str = ++rd; - nv->value = strtof(str, &rd); // rd used as end pointer + nv->value_int = atol(str); // collect the number as an integer + nv->value_flt = strtof(str, &rd); // collect the number as a float - rd used as end pointer if (rd != str) { - nv->valuetype = TYPE_FLOAT; + nv->valuetype = TYPE_FLOAT; // provisionally set it as a float } } @@ -132,7 +133,8 @@ static stat_t _text_parser_kernal(char *str, nvObj_t *nv) return (STAT_UNRECOGNIZED_NAME); } strcpy(nv->group, cfgArray[nv->index].group); // capture the group string if there is one - + nv_coerce_types(nv); // adjust types based on type fields in configApp table + // see if you need to strip the token - but only if in text mode if ((cs.comm_request_mode == TEXT_MODE) && (nv_group_is_prefixed(nv->group))) { wr = nv->token; @@ -196,7 +198,7 @@ void text_print_multiline_formatted(nvObj_t *nv) { for (uint8_t i=0; ivaluetype != TYPE_PARENT) { - preprocess_float(nv); + convert_outgoing_float(nv); nv_print(nv); } if ((nv = nv->nx) == NULL) return; @@ -218,9 +220,9 @@ void tx_print_flt(nvObj_t *nv) { text_print_flt(nv, fmt_flt);} void tx_print(nvObj_t *nv) { switch ((int8_t)nv->valuetype) { - case TYPE_FLOAT: { text_print_flt(nv, fmt_flt); break;} - case TYPE_INT: { text_print_int(nv, fmt_int); break;} - case TYPE_STRING:{ text_print_str(nv, fmt_str); break;} + case TYPE_FLOAT: { text_print_flt(nv, fmt_flt); break;} + case TYPE_INTEGER: { text_print_int(nv, fmt_int); break;} + case TYPE_STRING: { text_print_str(nv, fmt_str); break;} // TYPE_NULL is not needed in this list as it does nothing } } @@ -244,35 +246,36 @@ void text_print_str(nvObj_t *nv, const char *format) void text_print_int(nvObj_t *nv, const char *format) { - sprintf(cs.out_buf, format, (uint32_t)nv->value); + sprintf(cs.out_buf, format, nv->value_int); xio_writeline(cs.out_buf); } void text_print_flt(nvObj_t *nv, const char *format) { - sprintf(cs.out_buf, format, nv->value); + sprintf(cs.out_buf, format, nv->value_flt); xio_writeline(cs.out_buf); } void text_print_flt_units(nvObj_t *nv, const char *format, const char *units) { - sprintf(cs.out_buf, format, nv->value, units); + sprintf(cs.out_buf, format, nv->value_flt, units); xio_writeline(cs.out_buf); } void text_print_bool(nvObj_t *nv, const char *format) { - sprintf(cs.out_buf, format, !!((uint32_t)nv->value)?"True":"False"); +// sprintf(cs.out_buf, format, !!((uint32_t)nv->value)?"True":"False"); + sprintf(cs.out_buf, format, (nv->value_int ? "True" : "False")); xio_writeline(cs.out_buf); } void text_print(nvObj_t *nv, const char *format) { switch ((int8_t)nv->valuetype) { - case TYPE_NULL: { text_print_nul(nv, format); break;} - case TYPE_FLOAT: { text_print_flt(nv, format); break;} - case TYPE_INT: { text_print_int(nv, format); break;} - case TYPE_STRING:{ text_print_str(nv, format); break;} - case TYPE_BOOL: { text_print_bool(nv, format); break;} + case TYPE_NULL: { text_print_nul(nv, format); break;} + case TYPE_FLOAT: { text_print_flt(nv, format); break;} + case TYPE_INTEGER: { text_print_int(nv, format); break;} + case TYPE_STRING: { text_print_str(nv, format); break;} + case TYPE_BOOLEAN: { text_print_bool(nv, format); break;} } } @@ -282,4 +285,18 @@ void text_print(nvObj_t *nv, const char *format) { static const char fmt_tv[] = "[tv] text verbosity%15d [0=silent,1=verbose]\n"; void tx_print_tv(nvObj_t *nv) { text_print(nv, fmt_tv);} // TYPE_INT + +/*********************************************************************************** + * CONFIGURATION AND INTERFACE FUNCTIONS + * Functions to get and set variables from the cfgArray table + ***********************************************************************************/ + +/* + * txt_get_tv() - get text verbosity setting + * txt_set_tv() - set text verbosity + */ + +stat_t txt_get_tv(nvObj_t *nv) { return (get_integer(nv, txt.text_verbosity)); } +stat_t txt_set_tv(nvObj_t *nv) { return (set_integer(nv, txt.text_verbosity, TV_SILENT, TV_VERBOSE)); } + #endif // __TEXT_MODE diff --git a/g2core/text_parser.h b/g2core/text_parser.h old mode 100755 new mode 100644 index 6c300c94..3743dec8 --- a/g2core/text_parser.h +++ b/g2core/text_parser.h @@ -2,7 +2,7 @@ * text_parser.h - text parser and text mode support * This file is part of the g2core project * - * Copyright (c) 2013 - 2016 Alden S. Hart, Jr. + * Copyright (c) 2013 - 2018 Alden S. Hart, Jr. * * This file ("the software") is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License, version 2 as published by the @@ -95,4 +95,7 @@ stat_t text_parser_stub(char* str); void text_response_stub(const stat_t status, char* buf); void text_print_list_stub(stat_t status, uint8_t flags); +stat_t txt_get_tv(nvObj_t *nv); +stat_t txt_set_tv(nvObj_t *nv); + #endif // End of include guard: TEXT_PARSER_H_ONCE diff --git a/g2core/util.cpp b/g2core/util.cpp index 7c01a136..d1b9e7a1 100644 --- a/g2core/util.cpp +++ b/g2core/util.cpp @@ -2,7 +2,8 @@ * util.cpp - a random assortment of useful functions * This file is part of the g2core project * - * Copyright (c) 2010 - 2016 Alden S. Hart, Jr. + * Copyright (c) 2010 - 2019 Alden S. Hart, Jr. + * Copyright (c) 2016 - 2019 Robert Giseburt * * This file ("the software") is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License, version 2 as published by the @@ -32,13 +33,14 @@ #include "g2core.h" #include "util.h" +#include "canonical_machine.h" // for LAGERs +#include "xio.h" // for LAGERs + bool FLAGS_NONE[AXES] = { false, false, false, false, false, false }; bool FLAGS_ONE[AXES] = { true, false, false, false, false, false }; bool FLAGS_ALL[AXES] = { true, true, true, true, true, true }; -//*** debug utilities *** - /**** Vector utilities **** * copy_vector() - copy vector of arbitrary length * vector_equal() - test if vectors are equal @@ -195,33 +197,6 @@ char *escape_string(char *dst, char *src) return (start_dst); } -/* - * fntoa() - return ASCII string given a float and a decimal precision value - * - * Like sprintf, fntoa returns length of string, less the terminating NUL character - */ -char fntoa(char *str, float n, uint8_t precision) -{ - // handle special cases - if (isnan(n)) { - strcpy(str, "nan"); - return (3); - - } else if (isinf(n)) { - strcpy(str, "inf"); - return (3); - - } else if (precision == 0 ) { return(sprintf(str, "%0.0f", (double) n)); - } else if (precision == 1 ) { return(sprintf(str, "%0.1f", (double) n)); - } else if (precision == 2 ) { return(sprintf(str, "%0.2f", (double) n)); - } else if (precision == 3 ) { return(sprintf(str, "%0.3f", (double) n)); - } else if (precision == 4 ) { return(sprintf(str, "%0.4f", (double) n)); - } else if (precision == 5 ) { return(sprintf(str, "%0.5f", (double) n)); - } else if (precision == 6 ) { return(sprintf(str, "%0.6f", (double) n)); - } else if (precision == 7 ) { return(sprintf(str, "%0.7f", (double) n)); - } else { return(sprintf(str, "%f", (double) n)); } -} - /* * compute_checksum() - calculate the checksum for a string * @@ -243,10 +218,146 @@ uint16_t compute_checksum(char const *string, const uint16_t length) return (h % HASHMASK); } -/*********************************************** - **** Very Fast Number to ASCII Conversions **** - ***********************************************/ -/* +/****************************************** + **** Fast Number to ASCII Conversions **** + ******************************************/ + +/*********************************************************************************** + * floattoa() - integer to float + * + * Floattoa() is a slightly smarter, much faster version of snprintf() + * It suppresses trailing zeros and decimal points, 20.100 --> 20.1, 20.000 --> 20 + * Like sprintf, floattoa returns length of string, less the terminating NUL character + * + * !!! Precision cannot be greater than 10 !!! + */ + +#if 0 // olde version using sprintf. Does not do trailing zero suppression + +static const char _fp0[] = "%1.0f"; +static const char _fp1[] = "%1.1f"; +static const char _fp2[] = "%1.2f"; +static const char _fp3[] = "%1.3f"; +static const char _fp4[] = "%1.4f"; +static const char _fp5[] = "%1.5f"; +static const char _fp6[] = "%1.6f"; +static const char _fp7[] = "%1.7f"; +static const char *const _fmt_precision[] = { _fp0, _fp1, _fp2, _fp3, _fp4, _fp5, _fp6, _fp7 }; + +char floattoa(char *str, float n, int precision, int maxlen /*= 16*/) +{ + // handle special cases + if (isnan(n)) { + strcpy(str, "nan"); + return (3); + } + else if (isinf(n)) { + strcpy(str, "inf"); + return (3); + } + return(snprintf(str, maxlen, _fmt_precision[precision], (double) n)); +} +#endif + +// *** floattoa() starts here *** + +constexpr float round_lookup_[] = { + 0.5, // precision 0 + 0.05, // precision 1 + 0.005, // precision 2 + 0.0005, // precision 3 + 0.00005, // precision 4 + 0.000005, // precision 5 + 0.0000005, // precision 6 + 0.00000005, // precision 7 + 0.000000005, // precision 8 + 0.0000000005, // precision 9 + 0.00000000005 // precision 10 +}; + +// It's assumed that the string buffer contains at least count_ non-\0 chars +int c_strreverse(char * const t, const int count_, char hold = 0) { + return count_>1 + // Note: It always returns the count_, for a consistent interface. + ? (hold=*t, *t=*(t+(count_-1)), *(t+(count_-1))=hold), c_strreverse(t+1, count_-2), count_ + : count_; +} + +char floattoa(char *str, float n, int precision, int maxlen /*= 16*/) // maxlen = 16 +{ + // handle special cases + if (isnan(n)) { + strcpy(str, "nan"); + return (3); + } + else if (isinf(n)) { + strcpy(str, "inf"); + return (3); + } + + int length_ = 0; + char *b_ = str; + + if (n < 0.0) { + *b_++ = '-'; + return floattoa(b_, -n, precision, maxlen-1) + 1; + } + + n += round_lookup_[precision]; + int int_length_ = 0; + int integer_part_ = (int)n; + + // do integer part + while (integer_part_ > 0) { + if (length_++ > maxlen) { + *str = 0; + return 0; + } + int t_ = integer_part_ / 10; + *b_++ = '0' + (integer_part_ - (t_*10)); + integer_part_ = t_; + int_length_++; + } + if (length_ > 0) { + c_strreverse(str, int_length_); + } else { + *b_++ = '0'; + int_length_++; + } + + // do fractional part + *b_++ = '.'; + length_ = int_length_+1; + + float frac_part_ = n; + frac_part_ -= (int)frac_part_; + while (precision-- > 0) { + if (length_++ > maxlen) { + *str = 0; + return 0; + } + frac_part_ *= 10.0; + // if (precision==0) { + // t_ += 0.5; + // } + *b_++ = ('0' + (int)frac_part_); + frac_part_ -= (int)frac_part_; + } + + // right strip trailing zeroes (OPTIONAL) + while (*(b_-1) == '0' && length_>1) { + *(b_--) = 0; + length_--; + } + + if (*(b_-1) == '.') { + *(b_--) = 0; + length_--; + } + return length_; +} + +/*********************************************************************************** * inttoa() - integer to ASCII * * Taking advantage of the fact that most ints we display are 8 bit quantities, @@ -578,89 +689,22 @@ char inttoa(char *str, int n) return (strlen(str)); } -constexpr float round_lookup_[] = { - 0.5, // precision 0 - 0.05, // precision 1 - 0.005, // precision 2 - 0.0005, // precision 3 - 0.00005, // precision 4 - 0.000005, // precision 5 - 0.0000005, // precision 6 - 0.00000005, // precision 7 - 0.000000005, // precision 8 - 0.0000000005, // precision 9 - 0.00000000005 // precision 10 -}; +//*** debug utilities *** -// It's assumed that the string buffer contains at lest count_ non-\0 chars -int c_strreverse(char * const t, const int count_, char hold = 0) { - return count_>1 - // Note: It always returns the count_, for a consistent interface. - ? (hold=*t, *t=*(t+(count_-1)), *(t+(count_-1))=hold), c_strreverse(t+1, count_-2), count_ - : count_; +void LAGER(const char * msg) +{ + char message[64]; + sprintf(message, "%lu: %s\n", SysTickTimer_getValue(), msg); + xio_writeline(message); } -char floattoa(char *buffer, float in, int precision, int maxlen /*= 16*/) { - int length_ = 0; - char *b_ = buffer; - - if (in < 0.0) { - *b_++ = '-'; - return floattoa(b_, -in, precision, maxlen-1) + 1; - } - - in += round_lookup_[precision]; - int int_length_ = 0; - int integer_part_ = (int)in; - - // do integer part - while (integer_part_ > 0) { - if (length_++ > maxlen) { - *buffer = 0; - return 0; - } - int t_ = integer_part_ / 10; - *b_++ = '0' + (integer_part_ - (t_*10)); - integer_part_ = t_; - int_length_++; - } - if (length_ > 0) { - c_strreverse(buffer, int_length_); +void LAGER_cm(const char * msg) +{ + char message[64]; + if (cm == &cm1) { + sprintf(message, "%lu: p1 %s\n", SysTickTimer_getValue(), msg); } else { - *b_++ = '0'; - int_length_++; + sprintf(message, "%lu: p2 %s\n", SysTickTimer_getValue(), msg); } - - // do fractional part - *b_++ = '.'; - length_ = int_length_+1; - - float frac_part_ = in; - frac_part_ -= (int)frac_part_; - while (precision-- > 0) { - if (length_++ > maxlen) { - *buffer = 0; - return 0; - } - frac_part_ *= 10.0; - // if (precision==0) { - // t_ += 0.5; - // } - *b_++ = ('0' + (int)frac_part_); - frac_part_ -= (int)frac_part_; - } - - // right strip trailing zeroes (OPTIONAL) -#if 1 - while (*(b_-1) == '0' && length_>1) { - *(b_--) = 0; - length_--; - } - - if (*(b_-1) == '.') { - *(b_--) = 0; - length_--; - } -#endif - return length_; + xio_writeline(message); } diff --git a/g2core/util.h b/g2core/util.h index b9839d0f..7c18a6ea 100644 --- a/g2core/util.h +++ b/g2core/util.h @@ -2,7 +2,8 @@ * util.h - a random assortment of useful functions * This file is part of the g2core project * - * Copyright (c) 2010 - 2016 Alden S. Hart, Jr. + * Copyright (c) 2010 - 2018 Alden S. Hart, Jr. + * Copyright (c) 2016 - 2018 Robert Giseburt * * This file ("the software") is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License, version 2 as published by the @@ -24,21 +25,17 @@ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -/* util.c/.h contains a dog's breakfast of supporting functions that are - * not specific to g2core: including: - * +/* util.c/.h contains a dog's breakfast of supporting functions that are not specific + * to g2core: including: * - math and min/max utilities and extensions * - vector manipulation utilities * - support for debugging routines */ -#include "hardware.h" // for AXES - #ifndef UTIL_H_ONCE #define UTIL_H_ONCE #include -//#include "sam.h" #include "MotateTimers.h" using Motate::delay; using Motate::SysTickTimer; @@ -48,20 +45,19 @@ using Motate::SysTickTimer; #include // isnan, isinf /****** Global Scope Variables and Functions ******/ - -//*** debug utilities *** - +/* +// +++++ DIAGNOSTIC +++++ #pragma GCC push_options #pragma GCC optimize ("O0") -inline void _debug_trap(const char *reason) { - // We might be able to put a print here, but it MIGHT interrupt other output - // and might be deep in an ISR, so we had better just _NOP() and hope for the best. - __NOP(); -#if IN_DEBUGGER == 1 - __asm__("BKPT"); -#endif +// insert function here +static void _hold_everything (uint32_t n1, uint32_t n2) // example of function +{ + if (n1 == n2) { + cm1.gm.linenum +=1; + } } #pragma GCC reset_options +*/ //*** vector utilities *** @@ -75,6 +71,17 @@ uint8_t vector_equal(const float a[], const float b[]); float *set_vector(float x, float y, float z, float a, float b, float c); float *set_vector_by_axis(float value, uint8_t axis); +// *** canned initializers *** + +#if (AXES == 9) +#define INIT_AXES_ZEROES {0,0,0,0,0,0,0,0,0} +#define INIT_AXES_ONES {1,1,1,1,1,1,1,1,1} +#define INIT_AXES_FALSE INIT_AXES_ZEROES +#define INIT_AXES_TRUE INIT_AXES_ONES +#else +#warning UNSUPPORTED AXES SETTING! +#endif + //*** math utilities *** //float min3(float x1, float x2, float x3); @@ -85,21 +92,11 @@ float *set_vector_by_axis(float value, uint8_t axis); //*** string utilities *** -//#ifdef __ARM -//uint8_t * strcpy_U( uint8_t * dst, const uint8_t * src ); -//#endif - uint8_t isnumber(char c); char *escape_string(char *dst, char *src); -char inttoa(char *str, int n); -char floattoa(char *buffer, float in, int precision, int maxlen = 16); -//char fntoa(char *str, float n, uint8_t precision); - uint16_t compute_checksum(char const *string, const uint16_t length); - -//*** other utilities *** - -uint32_t SysTickTimer_getValue(void); +char floattoa(char *buffer, float in, int precision, int maxlen = 16); +char inttoa(char *str, int n); //**** Math Support ***** @@ -153,6 +150,7 @@ inline T avg(const T a,const T b) {return (a+b)/2; } // Constants #define MAX_LONG (2147483647) #define MAX_ULONG (4294967295) +#define MAX_FP_INTEGER (8388608) // maximum integer 32 bit FP will represent exactly (23 bits) #define MM_PER_INCH (25.4) #define INCHES_PER_MM (1/25.4) #define MICROSECONDS_PER_MINUTE ((float)60000000) @@ -194,6 +192,80 @@ constexpr float c_atof(char *&p_) { return (*p_ == '-') ? (c_atof_int_(++p_, 0) // return count_>1 ? (hold=*t, *t=*(t+(count_-1)), *(t+(count_-1))=hold), c_strreverse(t+1, count_-2), count_ : count_; //} +/*** Debug and DIAGNOSTICS *** + * + * This section collects debug and DIAGNOSTIC functions used by the project. + * + * The debug levels are set in the build line and may be one of: + * - debug is off, IN_DEBUGGER == 0 (See Makefile for the logic) + * DEBUG=0 - debug is off, IN_DEBUGGER == 0 + * DEBUG=1 - debug is on, IN_DEBUGGER == 0 + * DEBUG=2 - debug is on, IN_DEBUGGER == 1. Requires HW debugger to be connected + * DEBUG=3 - debug is on, IN_DEBUGGER == 1. Requires HW debugger and Semihosting to be enabled and running in the debugger + * + * These settings are applied in the Makefile. + * In addition, MotateDebug.h contains the bulk of the Semihosting definitions + * + * The *reason value is provided as it will be shown in the __asm__("BKPT") backtrace, + * or on the __NOP() if a breakpoint is set + * + * Try to use the functions provided below for debug statements to keep the code clean. If these + * are insufficient you can bracket diagnostics like so to enable then for any non-zero debug level: + * + #if IN_DEBUGGER == 1 + if (block->exit_velocity > block->cruise_velocity) { + __asm__("BKPT"); // exit > cruise after calculate_block + } + #endif + * + * ...or add a new debug functions to the ones below + */ + +/* + * debug_trap() - trap unconditionally + * debug_trap_if_zero() - trap if floating point value is zero + * debug_trap_if_true() - trap if condition is true + * + * The 'reason' value will display in GDB (but maybe not in AS7), and can also be passed + * to a downstream logger if these are introduced into the function. + * + * Note that it may be possible to print or generate exceptions in debug_trap(), but + * it MIGHT interrupt other output, or might have been called deep in an ISR, + * so we had better just _NOP() and hope for the best. + */ +#pragma GCC push_options +#pragma GCC optimize ("O0") + +inline void debug_trap(const char *reason) { +#if IN_DEBUGGER == 1 + __NOP(); + __asm__("BKPT"); +#endif +} + +inline void debug_trap_if_zero(float value, const char *reason) { +#if IN_DEBUGGER == 1 + if (fp_ZERO(value)) { + __NOP(); + __asm__("BKPT"); + } +#endif +} + +inline void debug_trap_if_true(bool condition, const char *reason) { +#if IN_DEBUGGER == 1 + if (condition) { + __NOP(); + __asm__("BKPT"); + } +#endif +} + +#pragma GCC reset_options + +void LAGER(const char * msg); +void LAGER_cm(const char * msg); + template void str_concat(char *&dest, const char (&data)[length]) { // length includes the \0 diff --git a/g2core/xio.cpp b/g2core/xio.cpp index 8a8a1fab..fb7e1c26 100644 --- a/g2core/xio.cpp +++ b/g2core/xio.cpp @@ -2,8 +2,8 @@ * xio.cpp - extended IO functions * This file is part of the g2core project * - * Copyright (c) 2013 - 2017 Alden S. Hart Jr. - * Copyright (c) 2013 - 2017 Robert Giseburt + * Copyright (c) 2013 - 2018 Alden S. Hart Jr. + * Copyright (c) 2013 - 2018 Robert Giseburt * * This file ("the software") is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License, version 2 as published by the @@ -413,8 +413,8 @@ extern xio_t xio; // See here for a discussion of what this means if you are not familiar with C++ // https://github.com/synthetos/g2/wiki/Dual-Endpoint-USB-Internals#c-classes-virtual-functions-and-inheritance -// LineRXBuffer takes the Motate RXBuffer (which handles "transfers", usually DMA), and adds G2 line-reading -// semantics to it. +// LineRXBuffer takes the Motate RXBuffer (which handles "transfers", usually DMA), +// and adds G2 line-reading semantics to it. template struct LineRXBuffer : RXBuffer<_size, owner_type, char> { typedef RXBuffer<_size, owner_type, char> parent_type; @@ -440,15 +440,15 @@ struct LineRXBuffer : RXBuffer<_size, owner_type, char> { // * "index" indicates it's in to _headers array // * "offset" means it's a character in the _data array - uint16_t _scan_offset; // offset into data of the last character scanned - uint16_t _line_start_offset; // offset into first character of the line, or the first char to ignore (too-long lines) - uint16_t _last_line_length; // used for ensuring lines aren't too long - bool _ignore_until_next_line; // if we get a too-long-line, we ignore the rest by setting this flag - bool _at_start_of_line; // true if the last character scanned was the end of a line + uint16_t _scan_offset; // offset into data of the last character scanned + uint16_t _line_start_offset; // offset into first character of the line, or the first char to ignore (too-long lines) + uint16_t _last_line_length; // used for ensuring lines aren't too long + bool _ignore_until_next_line; // if we get a too-long-line, we ignore the rest by setting this flag + bool _at_start_of_line; // true if the last character scanned was the end of a line - uint16_t _lines_found; // count of complete non-control lines that were found during scanning. + uint16_t _lines_found; // count of complete non-control lines that were found during scanning. - volatile uint16_t _last_scan_offset; // DEBUGGING + volatile uint16_t _last_scan_offset; // DIAGNOSTIC bool _last_returned_a_control = false; @@ -488,15 +488,15 @@ struct LineRXBuffer : RXBuffer<_size, owner_type, char> { struct SkipSections { struct SkipSection { - uint16_t start_offset; // the offset of the first character to skip - uint16_t end_offset; // the offset of the next character to read after skipping + uint16_t start_offset; // the offset of the first character to skip + uint16_t end_offset; // the offset of the next character to read after skipping }; static constexpr uint16_t _section_count = 16; SkipSection _sections[_section_count]; - uint8_t read_section_idx; // index of the first skip section to skip - uint8_t write_section_idx; // index of the next skip section to populate + uint8_t read_section_idx; // index of the first skip section to skip + uint8_t write_section_idx; // index of the next skip section to populate bool isFull() { return ((write_section_idx+1)&(_section_count-1)) == read_section_idx; @@ -639,7 +639,7 @@ struct LineRXBuffer : RXBuffer<_size, owner_type, char> { // See https://github.com/synthetos/g2/wiki/Marlin-Compatibility#stk500v2 if ((_stk_parser_state == STK500V2_State::Done) && (c == 0)) { - _debug_trap("scan ran into NULL (Marlin-mode)"); + debug_trap("scan ran into NULL (Marlin-mode)"); flush(); // consider the connection and all data trashed return false; } @@ -701,7 +701,7 @@ struct LineRXBuffer : RXBuffer<_size, owner_type, char> { #else // not MARLIN_COMPAT_ENABLED if (c == 0) { - _debug_trap("scan ran into NULL"); + debug_trap("_scanBuffer() scan ran into NULL"); flush(); // consider the connection and all data trashed return false; } @@ -846,14 +846,14 @@ struct LineRXBuffer : RXBuffer<_size, owner_type, char> { // Either way, _scan_offset is one past the end, so we don't care which. if (_data[_line_start_offset] == 0) { - _debug_trap("read ran into NULL"); + debug_trap("readline() read ran into NULL"); } // scan past any leftover CR or LF from the previous line while ((_data[_line_start_offset] == '\n') || (_data[_line_start_offset] == '\r')) { _line_start_offset = (_line_start_offset+1)&(_size-1); if (_scan_offset == _line_start_offset) { - _debug_trap("read ran into scan (1)"); + debug_trap("readline() read ran into scan (1)"); } } @@ -901,7 +901,7 @@ struct LineRXBuffer : RXBuffer<_size, owner_type, char> { if (_data[_read_offset] == 0) { - _debug_trap("read ran into NULL"); + debug_trap("readline() read ran into NULL"); } // scan past any leftover CR or LF from the previous line @@ -909,7 +909,7 @@ struct LineRXBuffer : RXBuffer<_size, owner_type, char> { while ((c == '\n') || (c == '\r')) { _read_offset = (_read_offset+1)&(_size-1); if (_scan_offset == _read_offset) { - _debug_trap("read ran into scan (2)"); + debug_trap("readline() read ran into scan (2)"); } // this also counts as the beginning of a line _skip_sections.skip(_read_offset); diff --git a/g2core/xio.h b/g2core/xio.h index 6ffb0860..f4269d6f 100644 --- a/g2core/xio.h +++ b/g2core/xio.h @@ -2,8 +2,8 @@ * xio.h - extended IO functions * This file is part of the g2core project * - * Copyright (c) 2013 - 2017 Alden S. Hart Jr. - * Copyright (c) 2013 - 2017 Robert Giseburt + * Copyright (c) 2013 - 2018 Alden S. Hart Jr. + * Copyright (c) 2013 - 2018 Robert Giseburt * * This file ("the software") is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License, version 2 as published by the @@ -48,7 +48,7 @@ #ifndef XIO_H_ONCE #define XIO_H_ONCE -//#include "g2core.h" // not required if used in g2core project +//#include "g2core.h" // not required if used in g2core project #include "config.h" // required for nvObj typedef #include "canonical_machine.h" // needed for cm_has_hold() #include "settings.h" // needed for MARLIN_COMPAT_ENABLED @@ -158,13 +158,11 @@ extern "C" { /* Signal character mappings */ -#define CHAR_RESET CAN -#define CHAR_ALARM EOT -#define CHAR_FEEDHOLD (char)'!' -#define CHAR_CYCLE_START (char)'~' -#define CHAR_QUEUE_FLUSH (char)'%' -//#define CHAR_BOOTLOADER ESC - +#define CHAR_RESET CAN // Control X - Reset Board +#define CHAR_ALARM EOT // Control D - Kill Job +#define CHAR_FEEDHOLD (char)'!' // Feedhold +#define CHAR_CYCLE_START (char)'~' // Feedhold Exit and Resume +#define CHAR_QUEUE_FLUSH (char)'%' // Feedhold Exit and Flush /**** xio_flash_file - object to hold in-flash (compiled-in) "files" to run ****/