+++ /dev/null
-# Backup files
-*~
-\#*
-
-build
-
-/*.eep
-/*.hex
-/*.elf
-/*.lss
-/*.map
+++ /dev/null
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2016, Buildbotics LLC
- All rights reserved.
-
- 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/>.
-
- The software is distributed in the hope that it will be useful, but
- WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
- Lesser General Public License for more details.
-
- You should have received a copy of the GNU Lesser General Public
- License along with the software. If not, see
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
+++ /dev/null
-The Buildbotics firmware ("the software") is free software: you can
-redistribute it and/or modify it under the terms of the GNU General
-Public License, version 2 (with out any licensing exceptions) as
-published by the Free Software Foundation. See the full license terms
-below.
-
-***********************************************************************
-
-Substantial effort was made to give credit to all authors of this
-software and of the works it was derived from and to adhere to the
-terms of the applicable license agreements. If you belive any
-mistakes have been made in this regard please contact Joseph Coffland
-<joseph@buildbotics.com>.
-
-Portions of this software are copyrighted by the following entities:
-
- Copyright (c) 2015 - 2016 Buildbotics LLC
- Copyright (c) 2014 Thomas Nixon, Jonathan Heathcote (cpp_magic.h)
- Copyright (c) 2013 - 2015 Robert Giseburt
- Copyright (c) 2010 - 2015 Alden S. Hart, Jr.
- Copyright (c) 2008 Atmel Corporation (part of clock.c)
- All rights reserved.
-
-Each source code file lists the entities which claim copyright to
-parts of those files.
-
-Much of this software was originally written by Alden S. Hart, Jr. and
-Robert Giseburt as part of the TinyG project. The TinyG project was
-in turn derived from grbl/marlin firmwares. All of the original code
-has been reformatted and cleaned up to fit our coding standards.
-Functionality in many areas has been substantially modified.
-
-The original TinyG firmware is licensed under the GPL v2 and includes
-an exception which allows use of the source code by non-GPLed
-libraires and potentially executables linked to those libraries. The
-TinyG project's license did not specify that this exception must be
-offered in derived works, therefore this software's license DOES NOT
-offer any exceptions to the GPL v2 license.
-
-***********************************************************************
-
- GNU GENERAL PUBLIC LICENSE
- Version 2, June 1991
-
- Copyright (C) 1989, 1991 Free Software Foundation, Inc.,
- 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
- Everyone is permitted to copy and distribute verbatim copies
- of this license document, but changing it is not allowed.
-
- Preamble
-
- The licenses for most software are designed to take away your
-freedom to share and change it. By contrast, the GNU General Public
-License is intended to guarantee your freedom to share and change free
-software--to make sure the software is free for all its users. This
-General Public License applies to most of the Free Software
-Foundation's software and to any other program whose authors commit to
-using it. (Some other Free Software Foundation software is covered by
-the GNU Lesser General Public License instead.) You can apply it to
-your programs, too.
-
- When we speak of free software, we are referring to freedom, not
-price. Our General Public Licenses are designed to make sure that you
-have the freedom to distribute copies of free software (and charge for
-this service if you wish), that you receive source code or can get it
-if you want it, that you can change the software or use pieces of it
-in new free programs; and that you know you can do these things.
-
- To protect your rights, we need to make restrictions that forbid
-anyone to deny you these rights or to ask you to surrender the rights.
-These restrictions translate to certain responsibilities for you if you
-distribute copies of the software, or if you modify it.
-
- For example, if you distribute copies of such a program, whether
-gratis or for a fee, you must give the recipients all the rights that
-you have. You must make sure that they, too, receive or can get the
-source code. And you must show them these terms so they know their
-rights.
-
- We protect your rights with two steps: (1) copyright the software, and
-(2) offer you this license which gives you legal permission to copy,
-distribute and/or modify the software.
-
- Also, for each author's protection and ours, we want to make certain
-that everyone understands that there is no warranty for this free
-software. If the software is modified by someone else and passed on, we
-want its recipients to know that what they have is not the original, so
-that any problems introduced by others will not reflect on the original
-authors' reputations.
-
- Finally, any free program is threatened constantly by software
-patents. We wish to avoid the danger that redistributors of a free
-program will individually obtain patent licenses, in effect making the
-program proprietary. To prevent this, we have made it clear that any
-patent must be licensed for everyone's free use or not licensed at all.
-
- The precise terms and conditions for copying, distribution and
-modification follow.
-
- GNU GENERAL PUBLIC LICENSE
- TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION
-
- 0. This License applies to any program or other work which contains
-a notice placed by the copyright holder saying it may be distributed
-under the terms of this General Public License. The "Program", below,
-refers to any such program or work, and a "work based on the Program"
-means either the Program or any derivative work under copyright law:
-that is to say, a work containing the Program or a portion of it,
-either verbatim or with modifications and/or translated into another
-language. (Hereinafter, translation is included without limitation in
-the term "modification".) Each licensee is addressed as "you".
-
-Activities other than copying, distribution and modification are not
-covered by this License; they are outside its scope. The act of
-running the Program is not restricted, and the output from the Program
-is covered only if its contents constitute a work based on the
-Program (independent of having been made by running the Program).
-Whether that is true depends on what the Program does.
-
- 1. You may copy and distribute verbatim copies of the Program's
-source code as you receive it, in any medium, provided that you
-conspicuously and appropriately publish on each copy an appropriate
-copyright notice and disclaimer of warranty; keep intact all the
-notices that refer to this License and to the absence of any warranty;
-and give any other recipients of the Program a copy of this License
-along with the Program.
-
-You may charge a fee for the physical act of transferring a copy, and
-you may at your option offer warranty protection in exchange for a fee.
-
- 2. You may modify your copy or copies of the Program or any portion
-of it, thus forming a work based on the Program, and copy and
-distribute such modifications or work under the terms of Section 1
-above, provided that you also meet all of these conditions:
-
- a) You must cause the modified files to carry prominent notices
- stating that you changed the files and the date of any change.
-
- b) You must cause any work that you distribute or publish, that in
- whole or in part contains or is derived from the Program or any
- part thereof, to be licensed as a whole at no charge to all third
- parties under the terms of this License.
-
- c) If the modified program normally reads commands interactively
- when run, you must cause it, when started running for such
- interactive use in the most ordinary way, to print or display an
- announcement including an appropriate copyright notice and a
- notice that there is no warranty (or else, saying that you provide
- a warranty) and that users may redistribute the program under
- these conditions, and telling the user how to view a copy of this
- License. (Exception: if the Program itself is interactive but
- does not normally print such an announcement, your work based on
- the Program is not required to print an announcement.)
-
-These requirements apply to the modified work as a whole. If
-identifiable sections of that work are not derived from the Program,
-and can be reasonably considered independent and separate works in
-themselves, then this License, and its terms, do not apply to those
-sections when you distribute them as separate works. But when you
-distribute the same sections as part of a whole which is a work based
-on the Program, the distribution of the whole must be on the terms of
-this License, whose permissions for other licensees extend to the
-entire whole, and thus to each and every part regardless of who wrote it.
-
-Thus, it is not the intent of this section to claim rights or contest
-your rights to work written entirely by you; rather, the intent is to
-exercise the right to control the distribution of derivative or
-collective works based on the Program.
-
-In addition, mere aggregation of another work not based on the Program
-with the Program (or with a work based on the Program) on a volume of
-a storage or distribution medium does not bring the other work under
-the scope of this License.
-
- 3. You may copy and distribute the Program (or a work based on it,
-under Section 2) in object code or executable form under the terms of
-Sections 1 and 2 above provided that you also do one of the following:
-
- a) Accompany it with the complete corresponding machine-readable
- source code, which must be distributed under the terms of Sections
- 1 and 2 above on a medium customarily used for software interchange; or,
-
- b) Accompany it with a written offer, valid for at least three
- years, to give any third party, for a charge no more than your
- cost of physically performing source distribution, a complete
- machine-readable copy of the corresponding source code, to be
- distributed under the terms of Sections 1 and 2 above on a medium
- customarily used for software interchange; or,
-
- c) Accompany it with the information you received as to the offer
- to distribute corresponding source code. (This alternative is
- allowed only for noncommercial distribution and only if you
- received the program in object code or executable form with such
- an offer, in accord with Subsection b above.)
-
-The source code for a work means the preferred form of the work for
-making modifications to it. For an executable work, complete source
-code means all the source code for all modules it contains, plus any
-associated interface definition files, plus the scripts used to
-control compilation and installation of the executable. However, as a
-special exception, the source code distributed need not include
-anything that is normally distributed (in either source or binary
-form) with the major components (compiler, kernel, and so on) of the
-operating system on which the executable runs, unless that component
-itself accompanies the executable.
-
-If distribution of executable or object code is made by offering
-access to copy from a designated place, then offering equivalent
-access to copy the source code from the same place counts as
-distribution of the source code, even though third parties are not
-compelled to copy the source along with the object code.
-
- 4. You may not copy, modify, sublicense, or distribute the Program
-except as expressly provided under this License. Any attempt
-otherwise to copy, modify, sublicense or distribute the Program is
-void, and will automatically terminate your rights under this License.
-However, parties who have received copies, or rights, from you under
-this License will not have their licenses terminated so long as such
-parties remain in full compliance.
-
- 5. You are not required to accept this License, since you have not
-signed it. However, nothing else grants you permission to modify or
-distribute the Program or its derivative works. These actions are
-prohibited by law if you do not accept this License. Therefore, by
-modifying or distributing the Program (or any work based on the
-Program), you indicate your acceptance of this License to do so, and
-all its terms and conditions for copying, distributing or modifying
-the Program or works based on it.
-
- 6. Each time you redistribute the Program (or any work based on the
-Program), the recipient automatically receives a license from the
-original licensor to copy, distribute or modify the Program subject to
-these terms and conditions. You may not impose any further
-restrictions on the recipients' exercise of the rights granted herein.
-You are not responsible for enforcing compliance by third parties to
-this License.
-
- 7. If, as a consequence of a court judgment or allegation of patent
-infringement or for any other reason (not limited to patent issues),
-conditions are imposed on you (whether by court order, agreement or
-otherwise) that contradict the conditions of this License, they do not
-excuse you from the conditions of this License. If you cannot
-distribute so as to satisfy simultaneously your obligations under this
-License and any other pertinent obligations, then as a consequence you
-may not distribute the Program at all. For example, if a patent
-license would not permit royalty-free redistribution of the Program by
-all those who receive copies directly or indirectly through you, then
-the only way you could satisfy both it and this License would be to
-refrain entirely from distribution of the Program.
-
-If any portion of this section is held invalid or unenforceable under
-any particular circumstance, the balance of the section is intended to
-apply and the section as a whole is intended to apply in other
-circumstances.
-
-It is not the purpose of this section to induce you to infringe any
-patents or other property right claims or to contest validity of any
-such claims; this section has the sole purpose of protecting the
-integrity of the free software distribution system, which is
-implemented by public license practices. Many people have made
-generous contributions to the wide range of software distributed
-through that system in reliance on consistent application of that
-system; it is up to the author/donor to decide if he or she is willing
-to distribute software through any other system and a licensee cannot
-impose that choice.
-
-This section is intended to make thoroughly clear what is believed to
-be a consequence of the rest of this License.
-
- 8. If the distribution and/or use of the Program is restricted in
-certain countries either by patents or by copyrighted interfaces, the
-original copyright holder who places the Program under this License
-may add an explicit geographical distribution limitation excluding
-those countries, so that distribution is permitted only in or among
-countries not thus excluded. In such case, this License incorporates
-the limitation as if written in the body of this License.
-
- 9. The Free Software Foundation may publish revised and/or new versions
-of the General Public License from time to time. Such new versions will
-be similar in spirit to the present version, but may differ in detail to
-address new problems or concerns.
-
-Each version is given a distinguishing version number. If the Program
-specifies a version number of this License which applies to it and "any
-later version", you have the option of following the terms and conditions
-either of that version or of any later version published by the Free
-Software Foundation. If the Program does not specify a version number of
-this License, you may choose any version ever published by the Free Software
-Foundation.
-
- 10. If you wish to incorporate parts of the Program into other free
-programs whose distribution conditions are different, write to the author
-to ask for permission. For software which is copyrighted by the Free
-Software Foundation, write to the Free Software Foundation; we sometimes
-make exceptions for this. Our decision will be guided by the two goals
-of preserving the free status of all derivatives of our free software and
-of promoting the sharing and reuse of software generally.
-
- NO WARRANTY
-
- 11. BECAUSE THE PROGRAM IS LICENSED FREE OF CHARGE, THERE IS NO WARRANTY
-FOR THE PROGRAM, TO THE EXTENT PERMITTED BY APPLICABLE LAW. EXCEPT WHEN
-OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR OTHER PARTIES
-PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED
-OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
-MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. THE ENTIRE RISK AS
-TO THE QUALITY AND PERFORMANCE OF THE PROGRAM IS WITH YOU. SHOULD THE
-PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF ALL NECESSARY SERVICING,
-REPAIR OR CORRECTION.
-
- 12. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING
-WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY AND/OR
-REDISTRIBUTE THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES,
-INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING
-OUT OF THE USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED
-TO LOSS OF DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY
-YOU OR THIRD PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER
-PROGRAMS), EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE
-POSSIBILITY OF SUCH DAMAGES.
-
- END OF TERMS AND CONDITIONS
-
- How to Apply These Terms to Your New Programs
-
- If you develop a new program, and you want it to be of the greatest
-possible use to the public, the best way to achieve this is to make it
-free software which everyone can redistribute and change under these terms.
-
- To do so, attach the following notices to the program. It is safest
-to attach them to the start of each source file to most effectively
-convey the exclusion of warranty; and each file should have at least
-the "copyright" line and a pointer to where the full notice is found.
-
- <one line to give the program's name and a brief idea of what it does.>
- Copyright (C) <year> <name of author>
-
- This program is free software; you can redistribute it and/or modify
- it under the terms of the GNU General Public License as published by
- the Free Software Foundation; either version 2 of the License, or
- (at your option) any later version.
-
- This program is distributed in the hope that it will be useful,
- but WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- GNU General Public License for more details.
-
- You should have received a copy of the GNU General Public License along
- with this program; if not, write to the Free Software Foundation, Inc.,
- 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
-
-Also add information on how to contact you by electronic and paper mail.
-
-If the program is interactive, make it output a short notice like this
-when it starts in an interactive mode:
-
- Gnomovision version 69, Copyright (C) year name of author
- Gnomovision comes with ABSOLUTELY NO WARRANTY; for details type `show w'.
- This is free software, and you are welcome to redistribute it
- under certain conditions; type `show c' for details.
-
-The hypothetical commands `show w' and `show c' should show the appropriate
-parts of the General Public License. Of course, the commands you use may
-be called something other than `show w' and `show c'; they could even be
-mouse-clicks or menu items--whatever suits your program.
-
-You should also get your employer (if you work as a programmer) or your
-school, if any, to sign a "copyright disclaimer" for the program, if
-necessary. Here is a sample; alter the names:
-
- Yoyodyne, Inc., hereby disclaims all copyright interest in the program
- `Gnomovision' (which makes passes at compilers) written by James Hacker.
-
- <signature of Ty Coon>, 1 April 1989
- Ty Coon, President of Vice
-
-This General Public License does not permit incorporating your program into
-proprietary programs. If your program is a subroutine library, you may
-consider it more useful to permit linking proprietary applications with the
-library. If this is what you want to do, use the GNU Lesser General
-Public License instead of this License.
+++ /dev/null
-# Makefile for the project Bulidbotics firmware
-PROJECT = bbctrl-avr-firmware
-MCU = atxmega192a3u
-CLOCK = 32000000
-VERSION = 0.3.1
-
-TARGET = $(PROJECT).elf
-
-# Compile flags
-CC = avr-gcc
-CPP = avr-g++
-
-COMMON = -mmcu=$(MCU)
-
-CFLAGS += $(COMMON)
-CFLAGS += -Wall -Werror # -Wno-error=unused-function
-CFLAGS += -Wno-error=strict-aliasing # for _invsqrt
-CFLAGS += -std=gnu99 -DF_CPU=$(CLOCK)UL -O3 #-funroll-loops
-CFLAGS += -funsigned-bitfields -fpack-struct -fshort-enums -funsigned-char
-CFLAGS += -MD -MP -MT $@ -MF build/dep/$(@F).d
-CFLAGS += -Isrc -DVERSION=\"$(VERSION)\"
-
-# Linker flags
-LDFLAGS += $(COMMON) -Wl,-u,vfprintf -lprintf_flt -lm
-LIBS += -lm
-
-# EEPROM flags
-EEFLAGS += -j .eeprom
-EEFLAGS += --set-section-flags=.eeprom="alloc,load"
-EEFLAGS += --change-section-lma .eeprom=0 --no-change-warnings
-
-# Programming flags
-PROGRAMMER = avrispmkII
-#PROGRAMMER = jtag3pdi
-PDEV = usb
-AVRDUDE_OPTS = -c $(PROGRAMMER) -p $(MCU) -P $(PDEV)
-
-FUSE0=0xff
-FUSE1=0x00
-FUSE2=0xbe
-FUSE4=0xff
-FUSE5=0xeb
-
-# SRC
-SRC = $(wildcard src/*.c)
-SRC += $(wildcard src/plan/*.c)
-OBJ = $(patsubst src/%.c,build/%.o,$(SRC))
-
-# Boot SRC
-BOOT_SRC = $(wildcard src/xboot/*.S)
-BOOT_SRC += $(wildcard src/xboot/*.c)
-BOOT_OBJ = $(patsubst src/%.c,build/%.o,$(BOOT_SRC))
-BOOT_OBJ := $(patsubst src/%.S,build/%.o,$(BOOT_OBJ))
-
-BOOT_LDFLAGS = $(LDFLAGS) -Wl,--section-start=.text=0x030000
-
-# Build
-all: $(TARGET) $(PROJECT).hex $(PROJECT).eep $(PROJECT).lss xboot.hex \
- boot-size size
-
-# Compile
-build/%.o: src/%.c
- @mkdir -p $(shell dirname $@)
- $(CC) $(INCLUDES) $(CFLAGS) -c -o $@ $<
-
-build/%.o: src/%.S
- @mkdir -p $(shell dirname $@)
- $(CC) $(INCLUDES) $(CFLAGS) -c -o $@ $<
-
-# Link
-$(TARGET): $(OBJ)
- $(CC) $(LDFLAGS) $(OBJ) $(LIBS) -o $@
-
-xboot.elf: $(BOOT_OBJ)
- $(CC) $(BOOT_LDFLAGS) $(BOOT_OBJ) -o $@
-
-%.hex: %.elf
- avr-objcopy -O ihex -R .eeprom -R .fuse -R .lock -R .signature $< $@
-
-%.eep: %.elf
- avr-objcopy $(EEFLAGS) -O ihex $< $@
-
-%.lss: %.elf
- avr-objdump -h -S $< > $@
-
-_size:
- @for X in A B C; do\
- echo '****************************************************************' ;\
- avr-size -$$X --mcu=$(MCU) $(SIZE_TARGET) ;\
- done
-
-boot-size: xboot.elf
- @$(MAKE) SIZE_TARGET=$< _size
-
-size: $(TARGET)
- @$(MAKE) SIZE_TARGET=$< _size
-
-# Program
-init:
- $(MAKE) erase
- -$(MAKE) fuses
- $(MAKE) program-boot
- $(MAKE) program
-
-reset:
- avrdude $(AVRDUDE_OPTS)
-
-erase:
- avrdude $(AVRDUDE_OPTS) -e
-
-program: $(PROJECT).hex
- avrdude $(AVRDUDE_OPTS) -U flash:w:$(PROJECT).hex:i
-
-verify: $(PROJECT).hex
- avrdude $(AVRDUDE_OPTS) -U flash:v:$(PROJECT).hex:i
-
-program-boot: xboot.hex
- avrdude $(AVRDUDE_OPTS) -U flash:w:xboot.hex:i
-
-fuses:
- avrdude $(AVRDUDE_OPTS) -U fuse0:w:$(FUSE0):m -U fuse1:w:$(FUSE1):m \
- -U fuse2:w:$(FUSE2):m -U fuse4:w:$(FUSE4):m -U fuse5:w:$(FUSE5):m
-
-read_fuses:
- avrdude $(AVRDUDE_OPTS) -q -q -U fuse0:r:-:h -U fuse1:r:-:h -U fuse2:r:-:h \
- -U fuse4:r:-:h -U fuse5:r:-:h
-
-signature:
- avrdude $(AVRDUDE_OPTS) -q -q -U signature:r:-:h
-
-prodsig:
- avrdude $(AVRDUDE_OPTS) -q -q -U prodsig:r:-:h
-
-usersig:
- avrdude $(AVRDUDE_OPTS) -q -q -U usersig:r:-:h
-
-# Clean
-tidy:
- rm -f $(shell find -name \*~ -o -name \#\*)
-
-clean: tidy
- rm -rf $(PROJECT).elf $(PROJECT).hex $(PROJECT).eep $(PROJECT).lss \
- $(PROJECT).map build xboot.hex xboot.elf
-
-.PHONY: tidy clean size all reset erase program fuses read_fuses prodsig
-.PHONY: signature usersig
-
-# Dependencies
--include $(shell mkdir -p build/dep) $(wildcard build/dep/*)
+++ /dev/null
-# Notes on the lifecycle of a movement
-
-## Parsing
-A move first starts off as a line of GCode which is parsed by
-``gc_gcode_parser()`` which in turn calls ``_normalize_gcode_block()``
-and ``_parse_gcode_block()``. ``_parse_gcode_block()`` sets flags in
-``mach.gf`` indicating which values have changed and the changed values in
-``mach.gn``. ``_parse_gcode_block()`` then calls ``_execute_gcode_block()``
-which calls ``mach_*()`` functions which modify the state of the GCode machine.
-
-## Queuing
-Some functions such as ``mach_straight_traverse()``, ``mach_straight_feed()``
-and ``mach_arc_feed()`` result in line moves being entered into the planner
-queue. Others enter dwells or commands or into the queue. Planner buffer
-entries encode everything needed to execute a move with out referring back to
-the machine model.
-
-Line moves are entered into the queue by calls to ``mp_aline()``. Arcs are
-converted to short straight line moves and are feed in as buffer room becomes
-available, until complete, via calls to ``mach_arc_callback()`` which results in
-multiple calls to ``mp_aline()``.
-
-``mp_aline()`` plans straight line movements by first calling
-``mach_calc_move_time()`` which uses the current GCode state to estimate the
-total time required to complete the move at the current feed rate and feed rate
-mode. If the move time is long enough, a buffer is allocated. Its jerk, max
-cruise velocity, max entry velocity, max delta velocity, max exit velocity and
-breaking velocity are all computed. The move velocities are planned by a
-call to ``mp_plan_block_list()``. Initially ``bf_func`` is set to
-``mp_exec_aline()`` and the buffer is committed to the queue by calling
-``mp_commit_write_buffer()``.
-
-## Planning
-### Backward planning pass
-``mp_plan_block_list()`` plans movements by first moving backwards through the
-planning buffer until either the last entry is reached or a buffer marked not
-``replannable`` is encountered. The ``breaking_velocity`` is propagated back
-during the backwards pass. Next, begins the forward planning pass.
-
-### Forward planning pass
-During the forward pass the entry velocity, cruise velocity and exit velocity
-are computed and ``mp_calculate_trapezoid()`` is called to (re)compute the
-velocity trapezoids of each buffer being considered. If a buffer's plan is
-deemed optimal then it is marked not ``replannable`` to avoid replanning later.
-
-### Trapezoid planning
-The call to ``mp_calculate_trapezoid()`` computes head, body and tail lengths
-for a single planner buffer. Entry, cruse and exit velocities may be modified
-to make the trapezoid fit with in the move length. Planning may result in a
-degraded trapezoid. I.e. one with out all three sides.
-
-## Execution
-The stepper motor driver interrupt routine calls ``mp_exec_move()`` to prepare
-the next move for execution. ``mp_exec_move()`` access the next buffer in the
-planner queue and calls the function pointed to by ``bf_func`` which is
-initially set to ``mp_exec_aline()`` during planning. Each call to
-``mp_exec_move()`` must prepare one and only one move fragment for the stepper
-driver. The planner buffer is executed repeatedly as long as ``bf_func``
-returns ``STAT_EAGAIN``.
-
-### Move initialization
-On the first call to ``mp_exec_aline()`` a call is made to
-``_exec_aline_init()``. This function may stop processing the move if a
-feedhold is in effect. It may also skip a move if it has zero length.
-Otherwise, it initializes the move runtime state (``mr``) copying over the
-variables set in the planner buffer. In addition, it computes waypoints at
-the ends of each trapezoid section. Waypoints are used later to correct
-position for rounding errors.
-
-### Move execution
-After move initialization ``mp_exec_aline()`` calls ``_exec_aline_head()``,
-``_exec_aline_body()`` and ``exec_aline_tail()`` on successive callbacks.
-These functions are called repeatedly until each section finishes. If any
-sections have zero length they are skipped and execution is passed immediately
-to the next section. During each section, forward differencing is used to map
-the trapezoid computed during the planning stage to a fifth-degree Bezier
-polynomial S-curve. The curve is used to find the appropriate velocity at the
-next target position.
-
-``_exec_aline_segment()`` is called for each non-zero section to convert the
-computed target position to target steps by calling ``mp_kinematics()``. The
-move fragment is then passed to the stepper driver by a call to
-``st_prep_line()``. When a segment is complete ``_exec_aline_segment()``
-returns ``STAT_OK`` indicating the next segment should be loaded. When all
-non-zero segments have been executed, the move is complete.
-
-## Pulse generation
-Calls to ``st_prep_line()`` prepare short (~5ms) move fragements for pluse
-generation by the stepper driver. Move time in clock ticks is computed from
-travel in steps and move duration. Then ``motor_prep_move()`` is called for
-each motor. ``motor_prep_move()`` may perform step correction and enable the
-motor. It then computes the optimal step clock divisor, clock ticks and sets
-the move direction, taking the motor's configuration in to account.
-
-The stepper timer ISR, after ending the previous move, calls
-``motor_load_move()`` on each motor. This sets up and starts the motor clocks,
-sets the motor direction lines and accumulates and resets the step encoders.
-After (re)starting the motor clocks the ISR signals a lower level interrupt to
-call ``mp_exec_move()`` and load the next move fragment.
+++ /dev/null
-The Buildbotics firmware is a 4 axis motion control system designed for
-high-performance on small to mid-sized machines. It was originally
-derived from the [TinyG firmware](https://github.com/synthetos/TinyG).
-
-# Features
- * 4 axis motion
- * jerk controlled motion for acceleration planning (3rd order motion planning)
-
-# Build Instructions
-To build in Linux run:
-
- make
-
-Other make commands are:
-
- * **size** - Display program and data sizes
- * **program** - program using AVR dude and an avrispmkII
- * **erase** - Erase chip
- * **fuses** - Write AVR fuses bytes
- * **read_fuses** - Read and print AVR fuse bytes
- * **clean** - Remove build files
- * **tidy** - Remove backup files
--- /dev/null
+# Backup files
+*~
+\#*
+
+build
+
+/*.eep
+/*.hex
+/*.elf
+/*.lss
+/*.map
--- /dev/null
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2016, Buildbotics LLC
+ All rights reserved.
+
+ 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/>.
+
+ The software is distributed in the hope that it will be useful, but
+ WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public
+ License along with the software. If not, see
+ <http://www.gnu.org/licenses/>.
+
+ For information regarding this software email:
+ "Joseph Coffland" <joseph@buildbotics.com>
--- /dev/null
+The Buildbotics firmware ("the software") is free software: you can
+redistribute it and/or modify it under the terms of the GNU General
+Public License, version 2 (with out any licensing exceptions) as
+published by the Free Software Foundation. See the full license terms
+below.
+
+***********************************************************************
+
+Substantial effort was made to give credit to all authors of this
+software and of the works it was derived from and to adhere to the
+terms of the applicable license agreements. If you belive any
+mistakes have been made in this regard please contact Joseph Coffland
+<joseph@buildbotics.com>.
+
+Portions of this software are copyrighted by the following entities:
+
+ Copyright (c) 2015 - 2016 Buildbotics LLC
+ Copyright (c) 2014 Thomas Nixon, Jonathan Heathcote (cpp_magic.h)
+ Copyright (c) 2013 - 2015 Robert Giseburt
+ Copyright (c) 2010 - 2015 Alden S. Hart, Jr.
+ Copyright (c) 2008 Atmel Corporation (part of clock.c)
+ All rights reserved.
+
+Each source code file lists the entities which claim copyright to
+parts of those files.
+
+Much of this software was originally written by Alden S. Hart, Jr. and
+Robert Giseburt as part of the TinyG project. The TinyG project was
+in turn derived from grbl/marlin firmwares. All of the original code
+has been reformatted and cleaned up to fit our coding standards.
+Functionality in many areas has been substantially modified.
+
+The original TinyG firmware is licensed under the GPL v2 and includes
+an exception which allows use of the source code by non-GPLed
+libraires and potentially executables linked to those libraries. The
+TinyG project's license did not specify that this exception must be
+offered in derived works, therefore this software's license DOES NOT
+offer any exceptions to the GPL v2 license.
+
+***********************************************************************
+
+ GNU GENERAL PUBLIC LICENSE
+ Version 2, June 1991
+
+ Copyright (C) 1989, 1991 Free Software Foundation, Inc.,
+ 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
+ Everyone is permitted to copy and distribute verbatim copies
+ of this license document, but changing it is not allowed.
+
+ Preamble
+
+ The licenses for most software are designed to take away your
+freedom to share and change it. By contrast, the GNU General Public
+License is intended to guarantee your freedom to share and change free
+software--to make sure the software is free for all its users. This
+General Public License applies to most of the Free Software
+Foundation's software and to any other program whose authors commit to
+using it. (Some other Free Software Foundation software is covered by
+the GNU Lesser General Public License instead.) You can apply it to
+your programs, too.
+
+ When we speak of free software, we are referring to freedom, not
+price. Our General Public Licenses are designed to make sure that you
+have the freedom to distribute copies of free software (and charge for
+this service if you wish), that you receive source code or can get it
+if you want it, that you can change the software or use pieces of it
+in new free programs; and that you know you can do these things.
+
+ To protect your rights, we need to make restrictions that forbid
+anyone to deny you these rights or to ask you to surrender the rights.
+These restrictions translate to certain responsibilities for you if you
+distribute copies of the software, or if you modify it.
+
+ For example, if you distribute copies of such a program, whether
+gratis or for a fee, you must give the recipients all the rights that
+you have. You must make sure that they, too, receive or can get the
+source code. And you must show them these terms so they know their
+rights.
+
+ We protect your rights with two steps: (1) copyright the software, and
+(2) offer you this license which gives you legal permission to copy,
+distribute and/or modify the software.
+
+ Also, for each author's protection and ours, we want to make certain
+that everyone understands that there is no warranty for this free
+software. If the software is modified by someone else and passed on, we
+want its recipients to know that what they have is not the original, so
+that any problems introduced by others will not reflect on the original
+authors' reputations.
+
+ Finally, any free program is threatened constantly by software
+patents. We wish to avoid the danger that redistributors of a free
+program will individually obtain patent licenses, in effect making the
+program proprietary. To prevent this, we have made it clear that any
+patent must be licensed for everyone's free use or not licensed at all.
+
+ The precise terms and conditions for copying, distribution and
+modification follow.
+
+ GNU GENERAL PUBLIC LICENSE
+ TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION
+
+ 0. This License applies to any program or other work which contains
+a notice placed by the copyright holder saying it may be distributed
+under the terms of this General Public License. The "Program", below,
+refers to any such program or work, and a "work based on the Program"
+means either the Program or any derivative work under copyright law:
+that is to say, a work containing the Program or a portion of it,
+either verbatim or with modifications and/or translated into another
+language. (Hereinafter, translation is included without limitation in
+the term "modification".) Each licensee is addressed as "you".
+
+Activities other than copying, distribution and modification are not
+covered by this License; they are outside its scope. The act of
+running the Program is not restricted, and the output from the Program
+is covered only if its contents constitute a work based on the
+Program (independent of having been made by running the Program).
+Whether that is true depends on what the Program does.
+
+ 1. You may copy and distribute verbatim copies of the Program's
+source code as you receive it, in any medium, provided that you
+conspicuously and appropriately publish on each copy an appropriate
+copyright notice and disclaimer of warranty; keep intact all the
+notices that refer to this License and to the absence of any warranty;
+and give any other recipients of the Program a copy of this License
+along with the Program.
+
+You may charge a fee for the physical act of transferring a copy, and
+you may at your option offer warranty protection in exchange for a fee.
+
+ 2. You may modify your copy or copies of the Program or any portion
+of it, thus forming a work based on the Program, and copy and
+distribute such modifications or work under the terms of Section 1
+above, provided that you also meet all of these conditions:
+
+ a) You must cause the modified files to carry prominent notices
+ stating that you changed the files and the date of any change.
+
+ b) You must cause any work that you distribute or publish, that in
+ whole or in part contains or is derived from the Program or any
+ part thereof, to be licensed as a whole at no charge to all third
+ parties under the terms of this License.
+
+ c) If the modified program normally reads commands interactively
+ when run, you must cause it, when started running for such
+ interactive use in the most ordinary way, to print or display an
+ announcement including an appropriate copyright notice and a
+ notice that there is no warranty (or else, saying that you provide
+ a warranty) and that users may redistribute the program under
+ these conditions, and telling the user how to view a copy of this
+ License. (Exception: if the Program itself is interactive but
+ does not normally print such an announcement, your work based on
+ the Program is not required to print an announcement.)
+
+These requirements apply to the modified work as a whole. If
+identifiable sections of that work are not derived from the Program,
+and can be reasonably considered independent and separate works in
+themselves, then this License, and its terms, do not apply to those
+sections when you distribute them as separate works. But when you
+distribute the same sections as part of a whole which is a work based
+on the Program, the distribution of the whole must be on the terms of
+this License, whose permissions for other licensees extend to the
+entire whole, and thus to each and every part regardless of who wrote it.
+
+Thus, it is not the intent of this section to claim rights or contest
+your rights to work written entirely by you; rather, the intent is to
+exercise the right to control the distribution of derivative or
+collective works based on the Program.
+
+In addition, mere aggregation of another work not based on the Program
+with the Program (or with a work based on the Program) on a volume of
+a storage or distribution medium does not bring the other work under
+the scope of this License.
+
+ 3. You may copy and distribute the Program (or a work based on it,
+under Section 2) in object code or executable form under the terms of
+Sections 1 and 2 above provided that you also do one of the following:
+
+ a) Accompany it with the complete corresponding machine-readable
+ source code, which must be distributed under the terms of Sections
+ 1 and 2 above on a medium customarily used for software interchange; or,
+
+ b) Accompany it with a written offer, valid for at least three
+ years, to give any third party, for a charge no more than your
+ cost of physically performing source distribution, a complete
+ machine-readable copy of the corresponding source code, to be
+ distributed under the terms of Sections 1 and 2 above on a medium
+ customarily used for software interchange; or,
+
+ c) Accompany it with the information you received as to the offer
+ to distribute corresponding source code. (This alternative is
+ allowed only for noncommercial distribution and only if you
+ received the program in object code or executable form with such
+ an offer, in accord with Subsection b above.)
+
+The source code for a work means the preferred form of the work for
+making modifications to it. For an executable work, complete source
+code means all the source code for all modules it contains, plus any
+associated interface definition files, plus the scripts used to
+control compilation and installation of the executable. However, as a
+special exception, the source code distributed need not include
+anything that is normally distributed (in either source or binary
+form) with the major components (compiler, kernel, and so on) of the
+operating system on which the executable runs, unless that component
+itself accompanies the executable.
+
+If distribution of executable or object code is made by offering
+access to copy from a designated place, then offering equivalent
+access to copy the source code from the same place counts as
+distribution of the source code, even though third parties are not
+compelled to copy the source along with the object code.
+
+ 4. You may not copy, modify, sublicense, or distribute the Program
+except as expressly provided under this License. Any attempt
+otherwise to copy, modify, sublicense or distribute the Program is
+void, and will automatically terminate your rights under this License.
+However, parties who have received copies, or rights, from you under
+this License will not have their licenses terminated so long as such
+parties remain in full compliance.
+
+ 5. You are not required to accept this License, since you have not
+signed it. However, nothing else grants you permission to modify or
+distribute the Program or its derivative works. These actions are
+prohibited by law if you do not accept this License. Therefore, by
+modifying or distributing the Program (or any work based on the
+Program), you indicate your acceptance of this License to do so, and
+all its terms and conditions for copying, distributing or modifying
+the Program or works based on it.
+
+ 6. Each time you redistribute the Program (or any work based on the
+Program), the recipient automatically receives a license from the
+original licensor to copy, distribute or modify the Program subject to
+these terms and conditions. You may not impose any further
+restrictions on the recipients' exercise of the rights granted herein.
+You are not responsible for enforcing compliance by third parties to
+this License.
+
+ 7. If, as a consequence of a court judgment or allegation of patent
+infringement or for any other reason (not limited to patent issues),
+conditions are imposed on you (whether by court order, agreement or
+otherwise) that contradict the conditions of this License, they do not
+excuse you from the conditions of this License. If you cannot
+distribute so as to satisfy simultaneously your obligations under this
+License and any other pertinent obligations, then as a consequence you
+may not distribute the Program at all. For example, if a patent
+license would not permit royalty-free redistribution of the Program by
+all those who receive copies directly or indirectly through you, then
+the only way you could satisfy both it and this License would be to
+refrain entirely from distribution of the Program.
+
+If any portion of this section is held invalid or unenforceable under
+any particular circumstance, the balance of the section is intended to
+apply and the section as a whole is intended to apply in other
+circumstances.
+
+It is not the purpose of this section to induce you to infringe any
+patents or other property right claims or to contest validity of any
+such claims; this section has the sole purpose of protecting the
+integrity of the free software distribution system, which is
+implemented by public license practices. Many people have made
+generous contributions to the wide range of software distributed
+through that system in reliance on consistent application of that
+system; it is up to the author/donor to decide if he or she is willing
+to distribute software through any other system and a licensee cannot
+impose that choice.
+
+This section is intended to make thoroughly clear what is believed to
+be a consequence of the rest of this License.
+
+ 8. If the distribution and/or use of the Program is restricted in
+certain countries either by patents or by copyrighted interfaces, the
+original copyright holder who places the Program under this License
+may add an explicit geographical distribution limitation excluding
+those countries, so that distribution is permitted only in or among
+countries not thus excluded. In such case, this License incorporates
+the limitation as if written in the body of this License.
+
+ 9. The Free Software Foundation may publish revised and/or new versions
+of the General Public License from time to time. Such new versions will
+be similar in spirit to the present version, but may differ in detail to
+address new problems or concerns.
+
+Each version is given a distinguishing version number. If the Program
+specifies a version number of this License which applies to it and "any
+later version", you have the option of following the terms and conditions
+either of that version or of any later version published by the Free
+Software Foundation. If the Program does not specify a version number of
+this License, you may choose any version ever published by the Free Software
+Foundation.
+
+ 10. If you wish to incorporate parts of the Program into other free
+programs whose distribution conditions are different, write to the author
+to ask for permission. For software which is copyrighted by the Free
+Software Foundation, write to the Free Software Foundation; we sometimes
+make exceptions for this. Our decision will be guided by the two goals
+of preserving the free status of all derivatives of our free software and
+of promoting the sharing and reuse of software generally.
+
+ NO WARRANTY
+
+ 11. BECAUSE THE PROGRAM IS LICENSED FREE OF CHARGE, THERE IS NO WARRANTY
+FOR THE PROGRAM, TO THE EXTENT PERMITTED BY APPLICABLE LAW. EXCEPT WHEN
+OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR OTHER PARTIES
+PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED
+OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
+MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. THE ENTIRE RISK AS
+TO THE QUALITY AND PERFORMANCE OF THE PROGRAM IS WITH YOU. SHOULD THE
+PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF ALL NECESSARY SERVICING,
+REPAIR OR CORRECTION.
+
+ 12. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING
+WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY AND/OR
+REDISTRIBUTE THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES,
+INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING
+OUT OF THE USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED
+TO LOSS OF DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY
+YOU OR THIRD PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER
+PROGRAMS), EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE
+POSSIBILITY OF SUCH DAMAGES.
+
+ END OF TERMS AND CONDITIONS
+
+ How to Apply These Terms to Your New Programs
+
+ If you develop a new program, and you want it to be of the greatest
+possible use to the public, the best way to achieve this is to make it
+free software which everyone can redistribute and change under these terms.
+
+ To do so, attach the following notices to the program. It is safest
+to attach them to the start of each source file to most effectively
+convey the exclusion of warranty; and each file should have at least
+the "copyright" line and a pointer to where the full notice is found.
+
+ <one line to give the program's name and a brief idea of what it does.>
+ Copyright (C) <year> <name of author>
+
+ This program is free software; you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation; either version 2 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License along
+ with this program; if not, write to the Free Software Foundation, Inc.,
+ 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
+
+Also add information on how to contact you by electronic and paper mail.
+
+If the program is interactive, make it output a short notice like this
+when it starts in an interactive mode:
+
+ Gnomovision version 69, Copyright (C) year name of author
+ Gnomovision comes with ABSOLUTELY NO WARRANTY; for details type `show w'.
+ This is free software, and you are welcome to redistribute it
+ under certain conditions; type `show c' for details.
+
+The hypothetical commands `show w' and `show c' should show the appropriate
+parts of the General Public License. Of course, the commands you use may
+be called something other than `show w' and `show c'; they could even be
+mouse-clicks or menu items--whatever suits your program.
+
+You should also get your employer (if you work as a programmer) or your
+school, if any, to sign a "copyright disclaimer" for the program, if
+necessary. Here is a sample; alter the names:
+
+ Yoyodyne, Inc., hereby disclaims all copyright interest in the program
+ `Gnomovision' (which makes passes at compilers) written by James Hacker.
+
+ <signature of Ty Coon>, 1 April 1989
+ Ty Coon, President of Vice
+
+This General Public License does not permit incorporating your program into
+proprietary programs. If your program is a subroutine library, you may
+consider it more useful to permit linking proprietary applications with the
+library. If this is what you want to do, use the GNU Lesser General
+Public License instead of this License.
--- /dev/null
+# Makefile for the project Bulidbotics firmware
+PROJECT = bbctrl-avr-firmware
+MCU = atxmega192a3u
+CLOCK = 32000000
+VERSION = 0.3.1
+
+TARGET = $(PROJECT).elf
+
+# Compile flags
+CC = avr-gcc
+CPP = avr-g++
+
+COMMON = -mmcu=$(MCU)
+
+CFLAGS += $(COMMON)
+CFLAGS += -Wall -Werror # -Wno-error=unused-function
+CFLAGS += -Wno-error=strict-aliasing # for _invsqrt
+CFLAGS += -std=gnu99 -DF_CPU=$(CLOCK)UL -O3 #-funroll-loops
+CFLAGS += -funsigned-bitfields -fpack-struct -fshort-enums -funsigned-char
+CFLAGS += -MD -MP -MT $@ -MF build/dep/$(@F).d
+CFLAGS += -Isrc -DVERSION=\"$(VERSION)\"
+
+# Linker flags
+LDFLAGS += $(COMMON) -Wl,-u,vfprintf -lprintf_flt -lm
+LIBS += -lm
+
+# EEPROM flags
+EEFLAGS += -j .eeprom
+EEFLAGS += --set-section-flags=.eeprom="alloc,load"
+EEFLAGS += --change-section-lma .eeprom=0 --no-change-warnings
+
+# Programming flags
+PROGRAMMER = avrispmkII
+#PROGRAMMER = jtag3pdi
+PDEV = usb
+AVRDUDE_OPTS = -c $(PROGRAMMER) -p $(MCU) -P $(PDEV)
+
+FUSE0=0xff
+FUSE1=0x00
+FUSE2=0xbe
+FUSE4=0xff
+FUSE5=0xeb
+
+# SRC
+SRC = $(wildcard src/*.c)
+SRC += $(wildcard src/plan/*.c)
+OBJ = $(patsubst src/%.c,build/%.o,$(SRC))
+
+# Boot SRC
+BOOT_SRC = $(wildcard src/xboot/*.S)
+BOOT_SRC += $(wildcard src/xboot/*.c)
+BOOT_OBJ = $(patsubst src/%.c,build/%.o,$(BOOT_SRC))
+BOOT_OBJ := $(patsubst src/%.S,build/%.o,$(BOOT_OBJ))
+
+BOOT_LDFLAGS = $(LDFLAGS) -Wl,--section-start=.text=0x030000
+
+# Build
+all: $(TARGET) $(PROJECT).hex $(PROJECT).eep $(PROJECT).lss xboot.hex \
+ boot-size size
+
+# Compile
+build/%.o: src/%.c
+ @mkdir -p $(shell dirname $@)
+ $(CC) $(INCLUDES) $(CFLAGS) -c -o $@ $<
+
+build/%.o: src/%.S
+ @mkdir -p $(shell dirname $@)
+ $(CC) $(INCLUDES) $(CFLAGS) -c -o $@ $<
+
+# Link
+$(TARGET): $(OBJ)
+ $(CC) $(LDFLAGS) $(OBJ) $(LIBS) -o $@
+
+xboot.elf: $(BOOT_OBJ)
+ $(CC) $(BOOT_LDFLAGS) $(BOOT_OBJ) -o $@
+
+%.hex: %.elf
+ avr-objcopy -O ihex -R .eeprom -R .fuse -R .lock -R .signature $< $@
+
+%.eep: %.elf
+ avr-objcopy $(EEFLAGS) -O ihex $< $@
+
+%.lss: %.elf
+ avr-objdump -h -S $< > $@
+
+_size:
+ @for X in A B C; do\
+ echo '****************************************************************' ;\
+ avr-size -$$X --mcu=$(MCU) $(SIZE_TARGET) ;\
+ done
+
+boot-size: xboot.elf
+ @$(MAKE) SIZE_TARGET=$< _size
+
+size: $(TARGET)
+ @$(MAKE) SIZE_TARGET=$< _size
+
+# Program
+init:
+ $(MAKE) erase
+ -$(MAKE) fuses
+ $(MAKE) program-boot
+ $(MAKE) program
+
+reset:
+ avrdude $(AVRDUDE_OPTS)
+
+erase:
+ avrdude $(AVRDUDE_OPTS) -e
+
+program: $(PROJECT).hex
+ avrdude $(AVRDUDE_OPTS) -U flash:w:$(PROJECT).hex:i
+
+verify: $(PROJECT).hex
+ avrdude $(AVRDUDE_OPTS) -U flash:v:$(PROJECT).hex:i
+
+program-boot: xboot.hex
+ avrdude $(AVRDUDE_OPTS) -U flash:w:xboot.hex:i
+
+fuses:
+ avrdude $(AVRDUDE_OPTS) -U fuse0:w:$(FUSE0):m -U fuse1:w:$(FUSE1):m \
+ -U fuse2:w:$(FUSE2):m -U fuse4:w:$(FUSE4):m -U fuse5:w:$(FUSE5):m
+
+read_fuses:
+ avrdude $(AVRDUDE_OPTS) -q -q -U fuse0:r:-:h -U fuse1:r:-:h -U fuse2:r:-:h \
+ -U fuse4:r:-:h -U fuse5:r:-:h
+
+signature:
+ avrdude $(AVRDUDE_OPTS) -q -q -U signature:r:-:h
+
+prodsig:
+ avrdude $(AVRDUDE_OPTS) -q -q -U prodsig:r:-:h
+
+usersig:
+ avrdude $(AVRDUDE_OPTS) -q -q -U usersig:r:-:h
+
+# Clean
+tidy:
+ rm -f $(shell find -name \*~ -o -name \#\*)
+
+clean: tidy
+ rm -rf $(PROJECT).elf $(PROJECT).hex $(PROJECT).eep $(PROJECT).lss \
+ $(PROJECT).map build xboot.hex xboot.elf
+
+.PHONY: tidy clean size all reset erase program fuses read_fuses prodsig
+.PHONY: signature usersig
+
+# Dependencies
+-include $(shell mkdir -p build/dep) $(wildcard build/dep/*)
--- /dev/null
+# Notes on the lifecycle of a movement
+
+## Parsing
+A move first starts off as a line of GCode which is parsed by
+``gc_gcode_parser()`` which in turn calls ``_normalize_gcode_block()``
+and ``_parse_gcode_block()``. ``_parse_gcode_block()`` sets flags in
+``mach.gf`` indicating which values have changed and the changed values in
+``mach.gn``. ``_parse_gcode_block()`` then calls ``_execute_gcode_block()``
+which calls ``mach_*()`` functions which modify the state of the GCode machine.
+
+## Queuing
+Some functions such as ``mach_straight_traverse()``, ``mach_straight_feed()``
+and ``mach_arc_feed()`` result in line moves being entered into the planner
+queue. Others enter dwells or commands or into the queue. Planner buffer
+entries encode everything needed to execute a move with out referring back to
+the machine model.
+
+Line moves are entered into the queue by calls to ``mp_aline()``. Arcs are
+converted to short straight line moves and are feed in as buffer room becomes
+available, until complete, via calls to ``mach_arc_callback()`` which results in
+multiple calls to ``mp_aline()``.
+
+``mp_aline()`` plans straight line movements by first calling
+``mach_calc_move_time()`` which uses the current GCode state to estimate the
+total time required to complete the move at the current feed rate and feed rate
+mode. If the move time is long enough, a buffer is allocated. Its jerk, max
+cruise velocity, max entry velocity, max delta velocity, max exit velocity and
+breaking velocity are all computed. The move velocities are planned by a
+call to ``mp_plan_block_list()``. Initially ``bf_func`` is set to
+``mp_exec_aline()`` and the buffer is committed to the queue by calling
+``mp_commit_write_buffer()``.
+
+## Planning
+### Backward planning pass
+``mp_plan_block_list()`` plans movements by first moving backwards through the
+planning buffer until either the last entry is reached or a buffer marked not
+``replannable`` is encountered. The ``breaking_velocity`` is propagated back
+during the backwards pass. Next, begins the forward planning pass.
+
+### Forward planning pass
+During the forward pass the entry velocity, cruise velocity and exit velocity
+are computed and ``mp_calculate_trapezoid()`` is called to (re)compute the
+velocity trapezoids of each buffer being considered. If a buffer's plan is
+deemed optimal then it is marked not ``replannable`` to avoid replanning later.
+
+### Trapezoid planning
+The call to ``mp_calculate_trapezoid()`` computes head, body and tail lengths
+for a single planner buffer. Entry, cruse and exit velocities may be modified
+to make the trapezoid fit with in the move length. Planning may result in a
+degraded trapezoid. I.e. one with out all three sides.
+
+## Execution
+The stepper motor driver interrupt routine calls ``mp_exec_move()`` to prepare
+the next move for execution. ``mp_exec_move()`` access the next buffer in the
+planner queue and calls the function pointed to by ``bf_func`` which is
+initially set to ``mp_exec_aline()`` during planning. Each call to
+``mp_exec_move()`` must prepare one and only one move fragment for the stepper
+driver. The planner buffer is executed repeatedly as long as ``bf_func``
+returns ``STAT_EAGAIN``.
+
+### Move initialization
+On the first call to ``mp_exec_aline()`` a call is made to
+``_exec_aline_init()``. This function may stop processing the move if a
+feedhold is in effect. It may also skip a move if it has zero length.
+Otherwise, it initializes the move runtime state (``mr``) copying over the
+variables set in the planner buffer. In addition, it computes waypoints at
+the ends of each trapezoid section. Waypoints are used later to correct
+position for rounding errors.
+
+### Move execution
+After move initialization ``mp_exec_aline()`` calls ``_exec_aline_head()``,
+``_exec_aline_body()`` and ``exec_aline_tail()`` on successive callbacks.
+These functions are called repeatedly until each section finishes. If any
+sections have zero length they are skipped and execution is passed immediately
+to the next section. During each section, forward differencing is used to map
+the trapezoid computed during the planning stage to a fifth-degree Bezier
+polynomial S-curve. The curve is used to find the appropriate velocity at the
+next target position.
+
+``_exec_aline_segment()`` is called for each non-zero section to convert the
+computed target position to target steps by calling ``mp_kinematics()``. The
+move fragment is then passed to the stepper driver by a call to
+``st_prep_line()``. When a segment is complete ``_exec_aline_segment()``
+returns ``STAT_OK`` indicating the next segment should be loaded. When all
+non-zero segments have been executed, the move is complete.
+
+## Pulse generation
+Calls to ``st_prep_line()`` prepare short (~5ms) move fragements for pluse
+generation by the stepper driver. Move time in clock ticks is computed from
+travel in steps and move duration. Then ``motor_prep_move()`` is called for
+each motor. ``motor_prep_move()`` may perform step correction and enable the
+motor. It then computes the optimal step clock divisor, clock ticks and sets
+the move direction, taking the motor's configuration in to account.
+
+The stepper timer ISR, after ending the previous move, calls
+``motor_load_move()`` on each motor. This sets up and starts the motor clocks,
+sets the motor direction lines and accumulates and resets the step encoders.
+After (re)starting the motor clocks the ISR signals a lower level interrupt to
+call ``mp_exec_move()`` and load the next move fragment.
--- /dev/null
+The Buildbotics firmware is a 4 axis motion control system designed for
+high-performance on small to mid-sized machines. It was originally
+derived from the [TinyG firmware](https://github.com/synthetos/TinyG).
+
+# Features
+ * 4 axis motion
+ * jerk controlled motion for acceleration planning (3rd order motion planning)
+
+# Build Instructions
+To build in Linux run:
+
+ make
+
+Other make commands are:
+
+ * **size** - Display program and data sizes
+ * **program** - program using AVR dude and an avrispmkII
+ * **erase** - Erase chip
+ * **fuses** - Write AVR fuses bytes
+ * **read_fuses** - Read and print AVR fuse bytes
+ * **clean** - Remove build files
+ * **tidy** - Remove backup files
--- /dev/null
+#!/usr/bin/env python3
+
+import os
+import re
+import shlex
+import subprocess
+
+
+lineRE = r'%(addr)s '
+command = 'avr-objdump -j .bss -t buildbotics.elf'
+
+proc = subprocess.Popen(shlex.split(command), stdout = subprocess.PIPE)
+
+out, err = proc.communicate()
+
+if proc.returncode:
+ print(out)
+ raise Exception('command failed')
+
+def get_sizes(data):
+ for line in data.decode().split('\n'):
+ if not re.match(r'^[0-9a-f]{8} .*', line): continue
+
+ size, name = int(line[22:30], 16), line[31:]
+ if not size: continue
+
+ yield (size, name)
+
+sizes = sorted(get_sizes(out))
+total = sum(x[0] for x in sizes)
+
+for size, name in sizes:
+ print('% 6d %5.2f%% %s' % (size, size / total * 100, name))
+
+print('-' * 40)
+print('% 6d Total' % total)
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2016 Buildbotics LLC
+ All rights reserved.
+
+ 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/>.
+
+ The software is distributed in the hope that it will be useful, but
+ WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public
+ License along with the software. If not, see
+ <http://www.gnu.org/licenses/>.
+
+ For information regarding this software email:
+ "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+
+#include "axis.h"
+#include "motor.h"
+#include "plan/planner.h"
+
+#include <math.h>
+#include <string.h>
+
+
+int motor_map[AXES] = {-1, -1, -1, -1, -1, -1};
+
+
+typedef struct {
+ float velocity_max; // max velocity in mm/min or deg/min
+ float feedrate_max; // max velocity in mm/min or deg/min
+ 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 recip_jerk; // reciprocal of current jerk value - with million
+ float junction_dev; // aka cornering delta
+ float radius; // radius in mm for rotary axes
+ float search_velocity; // homing search velocity
+ float latch_velocity; // homing latch velocity
+ float latch_backoff; // backoff from switches prior to homing latch movement
+ float zero_backoff; // backoff from switches for machine zero
+ homing_mode_t homing_mode;
+ bool homed;
+} axis_t;
+
+
+axis_t axes[MOTORS] = {
+ {
+ .velocity_max = X_VELOCITY_MAX,
+ .feedrate_max = X_FEEDRATE_MAX,
+ .travel_min = X_TRAVEL_MIN,
+ .travel_max = X_TRAVEL_MAX,
+ .jerk_max = X_JERK_MAX,
+ .junction_dev = X_JUNCTION_DEVIATION,
+ .search_velocity = X_SEARCH_VELOCITY,
+ .latch_velocity = X_LATCH_VELOCITY,
+ .latch_backoff = X_LATCH_BACKOFF,
+ .zero_backoff = X_ZERO_BACKOFF,
+ .homing_mode = X_HOMING_MODE,
+ }, {
+ .velocity_max = Y_VELOCITY_MAX,
+ .feedrate_max = Y_FEEDRATE_MAX,
+ .travel_min = Y_TRAVEL_MIN,
+ .travel_max = Y_TRAVEL_MAX,
+ .jerk_max = Y_JERK_MAX,
+ .junction_dev = Y_JUNCTION_DEVIATION,
+ .search_velocity = Y_SEARCH_VELOCITY,
+ .latch_velocity = Y_LATCH_VELOCITY,
+ .latch_backoff = Y_LATCH_BACKOFF,
+ .zero_backoff = Y_ZERO_BACKOFF,
+ .homing_mode = Y_HOMING_MODE,
+ }, {
+ .velocity_max = Z_VELOCITY_MAX,
+ .feedrate_max = Z_FEEDRATE_MAX,
+ .travel_min = Z_TRAVEL_MIN,
+ .travel_max = Z_TRAVEL_MAX,
+ .jerk_max = Z_JERK_MAX,
+ .junction_dev = Z_JUNCTION_DEVIATION,
+ .search_velocity = Z_SEARCH_VELOCITY,
+ .latch_velocity = Z_LATCH_VELOCITY,
+ .latch_backoff = Z_LATCH_BACKOFF,
+ .zero_backoff = Z_ZERO_BACKOFF,
+ .homing_mode = Z_HOMING_MODE,
+ }, {
+ .velocity_max = A_VELOCITY_MAX,
+ .feedrate_max = A_FEEDRATE_MAX,
+ .travel_min = A_TRAVEL_MIN,
+ .travel_max = A_TRAVEL_MAX,
+ .jerk_max = A_JERK_MAX,
+ .junction_dev = A_JUNCTION_DEVIATION,
+ .radius = A_RADIUS,
+ .search_velocity = A_SEARCH_VELOCITY,
+ .latch_velocity = A_LATCH_VELOCITY,
+ .latch_backoff = A_LATCH_BACKOFF,
+ .zero_backoff = A_ZERO_BACKOFF,
+ .homing_mode = A_HOMING_MODE,
+ }
+};
+
+
+bool axis_is_enabled(int axis) {
+ int motor = axis_get_motor(axis);
+ return motor != -1 && motor_is_enabled(motor);
+}
+
+
+char axis_get_char(int axis) {
+ return (axis < 0 || AXES <= axis) ? '?' : "XYZABCUVW"[axis];
+}
+
+
+int axis_get_id(char axis) {
+ const char *axes = "XYZABCUVW";
+ char *ptr = strchr(axes, axis);
+ return ptr == 0 ? -1 : (ptr - axes);
+}
+
+
+int axis_get_motor(int axis) {return motor_map[axis];}
+
+
+void axis_set_motor(int axis, int motor) {
+ motor_map[axis] = motor;
+ axis_set_jerk_max(axis, axes[motor].jerk_max); // Init 1/jerk
+}
+
+
+float axis_get_vector_length(const float a[], const float b[]) {
+ return sqrt(square(a[AXIS_X] - b[AXIS_X]) + square(a[AXIS_Y] - b[AXIS_Y]) +
+ square(a[AXIS_Z] - b[AXIS_Z]) + square(a[AXIS_A] - b[AXIS_A]) +
+ square(a[AXIS_B] - b[AXIS_B]) + square(a[AXIS_C] - b[AXIS_C]));
+}
+
+
+#define AXIS_VAR_GET(NAME, TYPE) \
+ TYPE get_##NAME(int axis) {return axes[axis].NAME;}
+
+
+#define AXIS_VAR_SET(NAME, TYPE) \
+ void set_##NAME(int axis, TYPE value) {axes[axis].NAME = value;}
+
+
+#define AXIS_GET(NAME, TYPE, DEFAULT) \
+ TYPE axis_get_##NAME(int axis) { \
+ int motor = axis_get_motor(axis); \
+ return motor == -1 ? DEFAULT : axes[motor].NAME; \
+ } \
+ AXIS_VAR_GET(NAME, TYPE)
+
+
+#define AXIS_SET(NAME, TYPE) \
+ void axis_set_##NAME(int axis, TYPE value) { \
+ int motor = axis_get_motor(axis); \
+ if (motor != -1) axes[motor].NAME = value; \
+ } \
+ AXIS_VAR_SET(NAME, TYPE)
+
+
+AXIS_GET(velocity_max, float, 0)
+AXIS_GET(feedrate_max, float, 0)
+AXIS_GET(homed, bool, false)
+AXIS_SET(homed, bool)
+AXIS_GET(homing_mode, homing_mode_t, HOMING_DISABLED)
+AXIS_SET(homing_mode, homing_mode_t)
+AXIS_GET(radius, float, 0)
+AXIS_GET(travel_min, float, DISABLE_SOFT_LIMIT)
+AXIS_GET(travel_max, float, DISABLE_SOFT_LIMIT)
+AXIS_GET(search_velocity, float, 0)
+AXIS_GET(latch_velocity, float, 0)
+AXIS_GET(zero_backoff, float, 0)
+AXIS_GET(latch_backoff, float, 0)
+AXIS_GET(junction_dev, float, 0)
+AXIS_GET(recip_jerk, float, 0)
+
+
+/* Note on jerk functions
+ *
+ * Jerk values can be rather large. Jerk values are stored in the system in
+ * truncated format; values are divided by 1,000,000 then multiplied before use.
+ *
+ * The axis_jerk() functions expect the jerk in divided by 1,000,000 form.
+ */
+AXIS_GET(jerk_max, float, 0)
+
+
+/// Sets jerk and its reciprocal for axis
+void axis_set_jerk_max(int axis, float jerk) {
+ axes[axis].jerk_max = jerk;
+ axes[axis].recip_jerk = 1 / (jerk * JERK_MULTIPLIER);
+}
+
+
+AXIS_VAR_SET(velocity_max, float)
+AXIS_VAR_SET(feedrate_max, float)
+AXIS_VAR_SET(radius, float)
+AXIS_VAR_SET(travel_min, float)
+AXIS_VAR_SET(travel_max, float)
+AXIS_VAR_SET(search_velocity, float)
+AXIS_VAR_SET(latch_velocity, float)
+AXIS_VAR_SET(zero_backoff, float)
+AXIS_VAR_SET(latch_backoff, float)
+AXIS_VAR_SET(junction_dev, float)
+AXIS_VAR_SET(jerk_max, float)
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2016 Buildbotics LLC
+ Copyright (c) 2010 - 2015 Alden S. Hart, Jr.
+ All rights reserved.
+
+ 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/>.
+
+ The software is distributed in the hope that it will be useful, but
+ WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public
+ License along with the software. If not, see
+ <http://www.gnu.org/licenses/>.
+
+ For information regarding this software email:
+ "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#pragma once
+
+#include "config.h"
+
+#include <stdbool.h>
+
+
+enum {
+ AXIS_X, AXIS_Y, AXIS_Z,
+ AXIS_A, AXIS_B, AXIS_C,
+ AXIS_U, AXIS_V, AXIS_W // reserved
+};
+
+
+typedef enum {
+ HOMING_DISABLED,
+ HOMING_STALL_MIN,
+ HOMING_STALL_MAX,
+ HOMING_SWITCH_MIN,
+ HOMING_SWITCH_MAX,
+} homing_mode_t;
+
+
+bool axis_is_enabled(int axis);
+char axis_get_char(int axis);
+int axis_get_id(char axis);
+int axis_get_motor(int axis);
+void axis_set_motor(int axis, int motor);
+float axis_get_vector_length(const float a[], const float b[]);
+
+float axis_get_velocity_max(int axis);
+float axis_get_feedrate_max(int axis);
+float axis_get_jerk_max(int axis);
+void axis_set_jerk_max(int axis, float jerk);
+bool axis_get_homed(int axis);
+void axis_set_homed(int axis, bool homed);
+homing_mode_t axis_get_homing_mode(int axis);
+void axis_set_homing_mode(int axis, homing_mode_t mode);
+float axis_get_radius(int axis);
+float axis_get_travel_min(int axis);
+float axis_get_travel_max(int axis);
+float axis_get_search_velocity(int axis);
+float axis_get_latch_velocity(int axis);
+float axis_get_zero_backoff(int axis);
+float axis_get_latch_backoff(int axis);
+float axis_get_junction_dev(int axis);
+float axis_get_recip_jerk(int axis);
+float axis_get_jerk_max(int axis);
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2016 Buildbotics LLC
+ All rights reserved.
+
+ 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/>.
+
+ The software is distributed in the hope that it will be useful, but
+ WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public
+ License along with the software. If not, see
+ <http://www.gnu.org/licenses/>.
+
+ For information regarding this software email:
+ "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#include "command.h"
+
+#include "gcode_parser.h"
+#include "usart.h"
+#include "hardware.h"
+#include "report.h"
+#include "vars.h"
+#include "estop.h"
+#include "homing.h"
+#include "probing.h"
+#include "i2c.h"
+#include "plan/jog.h"
+#include "plan/calibrate.h"
+#include "plan/buffer.h"
+#include "plan/arc.h"
+#include "plan/state.h"
+#include "config.h"
+
+#include <avr/pgmspace.h>
+#include <avr/wdt.h>
+
+#include <stdio.h>
+#include <string.h>
+#include <ctype.h>
+#include <stdlib.h>
+
+
+static char *_cmd = 0;
+
+
+static void _reboot() {hw_request_hard_reset();}
+
+
+static unsigned _parse_axis(uint8_t axis) {
+ switch (axis) {
+ case 'x': return 0; case 'y': return 1; case 'z': return 2;
+ case 'a': return 3; case 'b': return 4; case 'c': return 5;
+ case 'X': return 0; case 'Y': return 1; case 'Z': return 2;
+ case 'A': return 3; case 'B': return 4; case 'C': return 5;
+ default: return axis;
+ }
+}
+
+
+static void command_i2c_cb(i2c_cmd_t cmd, uint8_t *data, uint8_t length) {
+ switch (cmd) {
+ case I2C_NULL: break;
+ case I2C_ESTOP: estop_trigger(STAT_ESTOP_USER); break;
+ case I2C_CLEAR: estop_clear(); break;
+ case I2C_PAUSE: mp_request_hold(); break;
+ case I2C_OPTIONAL_PAUSE: mp_request_optional_pause(); break;
+ case I2C_RUN: mp_request_start(); break;
+ case I2C_STEP: mp_request_step(); break;
+ case I2C_FLUSH: mp_request_flush(); break;
+ case I2C_REPORT: report_request_full(); break;
+ case I2C_HOME: break; // TODO
+ case I2C_REBOOT: _reboot(); break;
+ case I2C_ZERO:
+ if (length == 0) mach_zero_all();
+ else if (length == 1) mach_zero_axis(_parse_axis(*data));
+ break;
+ }
+}
+
+
+void command_init() {
+ i2c_set_read_callback(command_i2c_cb);
+}
+
+
+// Command forward declarations
+// (Don't be afraid, X-Macros rock!)
+#define CMD(NAME, ...) \
+ uint8_t command_##NAME(int, char *[]);
+
+#include "command.def"
+#undef CMD
+
+// Command names & help
+#define CMD(NAME, MINARGS, MAXARGS, HELP) \
+ static const char pstr_##NAME[] PROGMEM = #NAME; \
+ static const char NAME##_help[] PROGMEM = HELP;
+
+#include "command.def"
+#undef CMD
+
+// Command table
+#define CMD(NAME, MINARGS, MAXARGS, HELP) \
+ {pstr_##NAME, command_##NAME, MINARGS, MAXARGS, NAME##_help},
+
+static const command_t commands[] PROGMEM = {
+#include "command.def"
+#undef CMD
+ {}, // Sentinel
+};
+
+
+int command_find(const char *match) {
+ for (int i = 0; ; i++) {
+ const char *name = pgm_read_word(&commands[i].name);
+ if (!name) break;
+
+ if (strcmp_P(match, name) == 0) return i;
+ }
+
+ return -1;
+}
+
+
+int command_exec(int argc, char *argv[]) {
+ putchar('\n');
+
+ int i = command_find(argv[0]);
+ if (i != -1) {
+ uint8_t min_args = pgm_read_byte(&commands[i].min_args);
+ uint8_t max_args = pgm_read_byte(&commands[i].max_args);
+
+ if (argc <= min_args) return STAT_TOO_FEW_ARGUMENTS;
+ else if (max_args < argc - 1) return STAT_TOO_MANY_ARGUMENTS;
+ else {
+ command_cb_t cb = pgm_read_word(&commands[i].cb);
+ return cb(argc, argv);
+ }
+
+ } else if (argc != 1)
+ return STAT_INVALID_OR_MALFORMED_COMMAND;
+
+ // Get or set variable
+ char *value = strchr(argv[0], '=');
+ if (value) {
+ *value++ = 0;
+ if (vars_set(argv[0], value)) return STAT_OK;
+
+ } else if (vars_print(argv[0])) {
+ putchar('\n');
+ return STAT_OK;
+ }
+
+ STATUS_ERROR(STAT_UNRECOGNIZED_NAME, "'%s'", argv[0]);
+ return STAT_UNRECOGNIZED_NAME;
+}
+
+
+int command_parser(char *cmd) {
+ // Parse line
+ char *p = cmd + 1; // Skip `$`
+ int argc = 0;
+ char *argv[MAX_ARGS] = {0};
+
+ if (cmd[1] == '$' && !cmd[2]) {
+ report_request_full(); // Full report
+ return STAT_OK;
+ }
+
+ while (argc < MAX_ARGS && *p) {
+ // Skip space
+ while (*p && isspace(*p)) *p++ = 0;
+
+ // Start of token
+ if (*p) argv[argc++] = p;
+
+ // Find end
+ while (*p && !isspace(*p)) p++;
+ }
+
+ // Exec command
+ if (argc) return command_exec(argc, argv);
+
+ return STAT_OK;
+}
+
+
+static char *_command_next() {
+ if (_cmd) return _cmd;
+
+ // Get next command
+ _cmd = usart_readline();
+ if (!_cmd) return 0;
+
+ // Remove leading whitespace
+ while (*_cmd && isspace(*_cmd)) _cmd++;
+
+ // Remove trailing whitespace
+ for (size_t len = strlen(_cmd); len && isspace(_cmd[len - 1]); len--)
+ _cmd[len - 1] = 0;
+
+ return _cmd;
+}
+
+
+void command_callback() {
+ if (!_command_next()) return;
+
+ stat_t status = STAT_OK;
+
+ switch (*_cmd) {
+ case 0: break; // Empty line
+ case '{': status = vars_parser(_cmd); break;
+ case '$': status = command_parser(_cmd); break;
+ case '%': break; // GCode program separator, ignore it
+
+ default:
+ if (estop_triggered()) {status = STAT_MACHINE_ALARMED; break;}
+ else if (mp_is_flushing()) break; // Flush GCode command
+ else if (!mp_queue_get_room() ||
+ mp_is_resuming() ||
+ mach_arc_active() ||
+ mach_is_homing() ||
+ mach_is_probing() ||
+ calibrate_busy() ||
+ mp_jog_busy()) return; // Wait
+
+ // Parse and execute GCode command
+ status = gc_gcode_parser(_cmd);
+ }
+
+ _cmd = 0; // Command consumed
+ report_request();
+
+ if (status) status_error(status);
+}
+
+
+// Command functions
+void static print_command_help(int i) {
+ static const char fmt[] PROGMEM = " $%-12S %S\n";
+ const char *name = pgm_read_word(&commands[i].name);
+ const char *help = pgm_read_word(&commands[i].help);
+
+ printf_P(fmt, name, help);
+}
+
+
+uint8_t command_help(int argc, char *argv[]) {
+ if (argc == 2) {
+ int i = command_find(argv[1]);
+
+ if (i == -1) return STAT_UNRECOGNIZED_NAME;
+ else print_command_help(i);
+
+ return STAT_OK;
+ }
+
+ puts_P(PSTR("\nLine editing:\n"
+ " ENTER Submit current command line.\n"
+ " BS Backspace, delete last character.\n"
+ " CTRL-X Cancel current line entry."));
+
+ puts_P(PSTR("\nCommands:"));
+ for (int i = 0; ; i++) {
+ const char *name = pgm_read_word(&commands[i].name);
+ if (!name) break;
+ print_command_help(i);
+ wdt_reset();
+ }
+
+ puts_P(PSTR("\nVariables:"));
+ vars_print_help();
+
+ return STAT_OK;
+}
+
+
+uint8_t command_reboot(int argc, char *argv[]) {
+ _reboot();
+ return 0;
+}
+
+
+uint8_t command_bootloader(int argc, char *argv[]) {
+ hw_request_bootloader();
+ return 0;
+}
+
+
+uint8_t command_save(int argc, char *argv[]) {
+ vars_save();
+ return 0;
+}
+
+
+uint8_t command_valid(int argc, char *argv[]) {
+ printf_P(vars_valid() ? PSTR("true\n") : PSTR("false\n"));
+ return 0;
+}
+
+
+uint8_t command_restore(int argc, char *argv[]) {
+ return vars_restore();
+}
+
+
+uint8_t command_clear(int argc, char *argv[]) {
+ vars_clear();
+ return 0;
+}
+
+
+uint8_t command_messages(int argc, char *argv[]) {
+ status_help();
+ return 0;
+}
+
+
+uint8_t command_resume(int argc, char *argv[]) {
+ mp_request_resume();
+ return 0;
+}
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2016 Buildbotics LLC
+ All rights reserved.
+
+ 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/>.
+
+ The software is distributed in the hope that it will be useful, but
+ WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public
+ License along with the software. If not, see
+ <http://www.gnu.org/licenses/>.
+
+ For information regarding this software email:
+ "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+// Name Min, Max args, Help
+CMD(help, 0, 1, "Print this help screen")
+CMD(reboot, 0, 0, "Reboot the controller")
+CMD(bootloader, 0, 0, "Load bootloader")
+CMD(save, 0, 0, "Save settings")
+CMD(valid, 0, 0, "Print 'true' if saved settings are valid")
+CMD(restore, 0, 0, "Restore settings")
+CMD(clear, 0, 0, "Clear saved settings")
+CMD(jog, 1, 4, "Jog")
+CMD(mreset, 0, 1, "Reset motor")
+CMD(calibrate, 0, 0, "Calibrate motors")
+CMD(messages, 0, 0, "Dump all possible status messages")
+CMD(resume, 0, 0, "Resume processing after a flush")
+CMD(home, 0, 0, "Home all axes")
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2016 Buildbotics LLC
+ All rights reserved.
+
+ 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/>.
+
+ The software is distributed in the hope that it will be useful, but
+ WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public
+ License along with the software. If not, see
+ <http://www.gnu.org/licenses/>.
+
+ For information regarding this software email:
+ "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#pragma once
+
+
+#include <stdint.h>
+
+
+#define MAX_ARGS 16
+
+typedef uint8_t (*command_cb_t)(int argc, char *argv[]);
+
+typedef struct {
+ const char *name;
+ command_cb_t cb;
+ uint8_t min_args;
+ uint8_t max_args;
+ const char *help;
+} command_t;
+
+
+void command_init();
+int command_find(const char *name);
+int command_exec(int argc, char *argv[]);
+void command_callback();
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2016 Buildbotics LLC
+ All rights reserved.
+
+ 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/>.
+
+ The software is distributed in the hope that it will be useful, but
+ WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public
+ License along with the software. If not, see
+ <http://www.gnu.org/licenses/>.
+
+ For information regarding this software email:
+ "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#pragma once
+
+#include "pins.h"
+
+#include <avr/interrupt.h>
+
+
+// Pins
+enum {
+ STALL_X_PIN = PORT_A << 3,
+ STALL_Y_PIN,
+ STALL_Z_PIN,
+ STALL_A_PIN,
+ SPIN_DIR_PIN,
+ SPIN_ENABLE_PIN,
+ ANALOG_PIN,
+ PROBE_PIN,
+
+ MIN_X_PIN = PORT_B << 3,
+ MAX_X_PIN,
+ MIN_A_PIN,
+ MAX_A_PIN,
+ MIN_Y_PIN,
+ MAX_Y_PIN,
+ MIN_Z_PIN,
+ MAX_Z_PIN,
+
+ SDA_PIN = PORT_C << 3,
+ SCL_PIN,
+ SERIAL_RX_PIN,
+ SERIAL_TX_PIN,
+ SERIAL_CTS_PIN,
+ SPI_CLK_PIN,
+ SPI_MISO_PIN,
+ SPI_MOSI_PIN,
+
+ STEP_X_PIN = PORT_D << 3,
+ SPI_CS_X_PIN,
+ SPI_CS_A_PIN,
+ SPI_CS_Z_PIN,
+ SPIN_PWM_PIN,
+ SWITCH_1_PIN,
+ RS485_RO_PIN,
+ RS485_DI_PIN,
+
+ STEP_Y_PIN = PORT_E << 3,
+ SPI_CS_Y_PIN,
+ DIR_X_PIN,
+ DIR_Y_PIN,
+ STEP_A_PIN,
+ SWITCH_2_PIN,
+ DIR_Z_PIN,
+ DIR_A_PIN,
+
+ STEP_Z_PIN = PORT_F << 3,
+ RS485_RW_PIN,
+ FAULT_PIN,
+ ESTOP_PIN,
+ MOTOR_FAULT_PIN,
+ MOTOR_ENABLE_PIN,
+ NC_0_PIN,
+ NC_1_PIN,
+};
+
+#define SPI_SS_PIN SERIAL_CTS_PIN // Needed for SPI configuration
+
+
+// Compile-time settings
+#define __CLOCK_EXTERNAL_16MHZ // uses PLL to provide 32 MHz system clock
+//#define __CLOCK_INTERNAL_32MHZ
+
+
+#define AXES 6 // number of axes
+#define MOTORS 4 // number of motors on the board
+#define COORDS 6 // number of supported coordinate systems
+#define SWITCHES 9 // number of supported limit switches
+#define PWMS 2 // number of supported PWM channels
+
+#define DISABLE_SOFT_LIMIT -1000000
+
+
+// Motor settings. See motor.c
+#define MOTOR_MAX_CURRENT 1.0 // 1.0 is full power
+#define MOTOR_MIN_CURRENT 0.25 // 1.0 is full power
+#define MOTOR_IDLE_CURRENT 0.05 // 1.0 is full power
+#define MOTOR_STALL_THRESHOLD 0 // 0 -> 1 is least -> most sensitive
+#define MOTOR_MICROSTEPS 32
+#define MOTOR_POWER_MODE MOTOR_POWERED_ONLY_WHEN_MOVING
+#define MOTOR_IDLE_TIMEOUT 0.25 // secs, motor off after this time
+
+#define M1_AXIS AXIS_X
+#define M1_STEP_ANGLE 1.8
+#define M1_TRAVEL_PER_REV 6.35
+#define M1_MICROSTEPS MOTOR_MICROSTEPS
+#define M1_REVERSE false
+#define M1_POWER_MODE MOTOR_POWER_MODE
+
+#define M2_AXIS AXIS_Y
+#define M2_STEP_ANGLE 1.8
+#define M2_TRAVEL_PER_REV 6.35
+#define M2_MICROSTEPS MOTOR_MICROSTEPS
+#define M2_REVERSE false
+#define M2_POWER_MODE MOTOR_POWER_MODE
+
+#define M3_AXIS AXIS_Z
+#define M3_STEP_ANGLE 1.8
+#define M3_TRAVEL_PER_REV (25.4 / 6.0)
+#define M3_MICROSTEPS MOTOR_MICROSTEPS
+#define M3_REVERSE false
+#define M3_POWER_MODE MOTOR_POWER_MODE
+
+#define M4_AXIS AXIS_A
+#define M4_STEP_ANGLE 1.8
+#define M4_TRAVEL_PER_REV 360 // degrees per motor rev
+#define M4_MICROSTEPS MOTOR_MICROSTEPS
+#define M4_REVERSE false
+#define M4_POWER_MODE MOTOR_POWER_MODE
+
+
+// Switch settings. See switch.c
+#define SWITCH_INTLVL PORT_INT0LVL_MED_gc
+#define SW_LOCKOUT_TICKS 250 // ms
+#define SW_DEGLITCH_TICKS 30 // ms
+
+
+// Machine settings
+//#define STEP_CORRECTION // Enable step correction
+#define MAX_STEP_CORRECTION 4 // In steps per segment
+#define CHORDAL_TOLERANCE 0.01 // chordal accuracy for arcs
+#define JERK_MAX 5 // yes, that's km/min^3
+#define JUNCTION_DEVIATION 0.05 // default value, in mm
+#define JUNCTION_ACCELERATION 100000 // centripetal corner accel
+#define JOG_JERK_MULT 4 // Jogging jerk multipler
+#define JOG_MIN_VELOCITY 10 // mm/min
+#define CAL_ACCELERATION 500000 // mm/min^2
+
+// Axis settings
+#define VELOCITY_MAX 13000 // mm/min
+#define FEEDRATE_MAX VELOCITY_MAX
+
+#define X_VELOCITY_MAX VELOCITY_MAX // G0 max velocity in mm/min
+#define X_FEEDRATE_MAX FEEDRATE_MAX // G1 max feed rate in mm/min
+#define X_TRAVEL_MIN 0 // minimum travel for soft limits
+#define X_TRAVEL_MAX 350 // between switches or crashes
+#define X_JERK_MAX JERK_MAX
+#define X_JUNCTION_DEVIATION JUNCTION_DEVIATION
+#define X_SEARCH_VELOCITY 2400 // move in negative direction
+#define X_LATCH_VELOCITY 100 // mm/min
+#define X_LATCH_BACKOFF 5 // mm
+#define X_ZERO_BACKOFF 1 // mm
+#define X_HOMING_MODE HOMING_STALL_MAX
+
+#define Y_VELOCITY_MAX VELOCITY_MAX
+#define Y_FEEDRATE_MAX FEEDRATE_MAX
+#define Y_TRAVEL_MIN 0
+#define Y_TRAVEL_MAX 350
+#define Y_JERK_MAX JERK_MAX
+#define Y_JUNCTION_DEVIATION JUNCTION_DEVIATION
+#define Y_SEARCH_VELOCITY 3000
+#define Y_LATCH_VELOCITY 100
+#define Y_LATCH_BACKOFF 5
+#define Y_ZERO_BACKOFF 1
+#define Y_HOMING_MODE HOMING_STALL_MAX
+
+#define Z_VELOCITY_MAX 2000 // VELOCITY_MAX
+#define Z_FEEDRATE_MAX FEEDRATE_MAX
+#define Z_TRAVEL_MIN 0
+#define Z_TRAVEL_MAX 75
+#define Z_JERK_MAX JERK_MAX
+#define Z_JUNCTION_DEVIATION JUNCTION_DEVIATION
+#define Z_SEARCH_VELOCITY 400
+#define Z_LATCH_VELOCITY 100
+#define Z_LATCH_BACKOFF 5
+#define Z_ZERO_BACKOFF 1
+#define Z_HOMING_MODE HOMING_STALL_MAX
+
+// A values are chosen to make the A motor react the same as X for testing
+// set to the same speed as X axis
+#define A_VELOCITY_MAX (X_VELOCITY_MAX / M1_TRAVEL_PER_REV * 360)
+#define A_FEEDRATE_MAX A_VELOCITY_MAX
+#define A_TRAVEL_MIN -1
+#define A_TRAVEL_MAX -1 // same value means infinite
+#define A_JERK_MAX (X_JERK_MAX * 360 / M1_TRAVEL_PER_REV)
+#define A_JUNCTION_DEVIATION JUNCTION_DEVIATION
+#define A_RADIUS (M1_TRAVEL_PER_REV / 2 / M_PI)
+#define A_SEARCH_VELOCITY 600
+#define A_LATCH_VELOCITY 100
+#define A_LATCH_BACKOFF 5
+#define A_ZERO_BACKOFF 2
+#define A_HOMING_MODE HOMING_DISABLED
+
+
+// Spindle settings
+#define SPINDLE_TYPE SPINDLE_TYPE_HUANYANG
+#define SPINDLE_PWM_FREQUENCY 100 // in Hz
+#define SPINDLE_MIN_RPM 1000
+#define SPINDLE_MAX_RPM 24000
+#define SPINDLE_MIN_DUTY 0.05
+#define SPINDLE_MAX_DUTY 0.99
+#define SPINDLE_REVERSE false
+
+
+// Gcode defaults
+#define GCODE_DEFAULT_UNITS MILLIMETERS // MILLIMETERS or INCHES
+#define GCODE_DEFAULT_PLANE PLANE_XY // See machine.h
+#define GCODE_DEFAULT_COORD_SYSTEM G54 // G54, G55, G56, G57, G58, G59
+#define GCODE_DEFAULT_PATH_CONTROL PATH_CONTINUOUS
+#define GCODE_DEFAULT_DISTANCE_MODE ABSOLUTE_MODE
+#define GCODE_DEFAULT_ARC_DISTANCE_MODE INCREMENTAL_MODE
+
+
+// Motor ISRs
+#define STALL_ISR_vect PORTA_INT1_vect
+#define FAULT_ISR_vect PORTF_INT1_vect
+
+
+/* Interrupt usage:
+ *
+ * HI Stepper timers (set in stepper.h)
+ * LO Segment execution SW interrupt (set in stepper.h)
+ * MED GPIO1 switch port (set in gpio.h)
+ * MED Serial RX (set in usart.c)
+ * MED Serial TX (set in usart.c) (* see note)
+ * LO Real time clock interrupt (set in xmega_rtc.h)
+ *
+ * (*) The TX cannot run at LO level or exception reports and other prints
+ * called from a LO interrupt (as in prep_line()) will kill the system
+ * in a permanent loop call in usart_putc() (usart.c).
+ */
+
+// Timer assignments - see specific modules for details
+// TCC1 free
+#define TIMER_STEP TCC0 // Step timer (see stepper.h)
+#define TIMER_PWM TCD1 // PWM timer (see pwm_spindle.c)
+
+#define M1_TIMER TCD0
+#define M2_TIMER TCE0
+#define M3_TIMER TCF0
+#define M4_TIMER TCE1
+
+#define M1_DMA_CH DMA.CH0
+#define M2_DMA_CH DMA.CH1
+#define M3_DMA_CH DMA.CH2
+#define M4_DMA_CH DMA.CH3
+
+#define M1_DMA_TRIGGER DMA_CH_TRIGSRC_TCD0_CCA_gc
+#define M2_DMA_TRIGGER DMA_CH_TRIGSRC_TCE0_CCA_gc
+#define M3_DMA_TRIGGER DMA_CH_TRIGSRC_TCF0_CCA_gc
+#define M4_DMA_TRIGGER DMA_CH_TRIGSRC_TCE1_CCA_gc
+
+
+// Timer setup for stepper and dwells
+#define STEP_TIMER_DISABLE 0
+#define STEP_TIMER_ENABLE TC_CLKSEL_DIV4_gc
+#define STEP_TIMER_DIV 4
+#define STEP_TIMER_FREQ (F_CPU / STEP_TIMER_DIV)
+#define STEP_TIMER_POLL (STEP_TIMER_FREQ * 0.001)
+#define STEP_TIMER_WGMODE TC_WGMODE_NORMAL_gc // count to TOP & rollover
+#define STEP_TIMER_ISR TCC0_OVF_vect
+#define STEP_TIMER_INTLVL TC_OVFINTLVL_HI_gc
+
+#define MAX_SEGMENT_TIME ((float)0xffff / 60.0 / STEP_TIMER_FREQ)
+#define NOM_SEGMENT_USEC 5000.0 // nominal segment time
+#define MIN_SEGMENT_USEC 2500.0 // minimum segment time
+
+
+// Huanyang settings
+#define HUANYANG_PORT USARTD1
+#define HUANYANG_DRE_vect USARTD1_DRE_vect
+#define HUANYANG_TXC_vect USARTD1_TXC_vect
+#define HUANYANG_RXC_vect USARTD1_RXC_vect
+#define HUANYANG_TIMEOUT 50 // ms. response timeout
+#define HUANYANG_RETRIES 4 // Number of retries before failure
+#define HUANYANG_ID 1 // Default ID
+
+
+// Serial settings
+#define SERIAL_BAUD USART_BAUD_115200
+#define SERIAL_PORT USARTC0
+#define SERIAL_DRE_vect USARTC0_DRE_vect
+#define SERIAL_RXC_vect USARTC0_RXC_vect
+
+
+// Input
+#define INPUT_BUFFER_LEN 255 // text buffer size (255 max)
+
+
+// Arc
+#define ARC_RADIUS_ERROR_MAX 1.0 // max mm diff between start and end radius
+#define ARC_RADIUS_ERROR_MIN 0.005 // min mm where 1% rule applies
+#define ARC_RADIUS_TOLERANCE 0.001 // 0.1% radius variance test
+
+
+// Planner
+/// Should be at least the number of buffers required to support optimal
+/// planning in the case of very short lines or arc segments. Suggest no less
+/// than 12. Maximum is 255 with out also changing the type of mb.space. Must
+/// leave headroom for stack.
+#define PLANNER_BUFFER_POOL_SIZE 32
+
+/// Buffers to reserve in planner before processing new input line
+#define PLANNER_BUFFER_HEADROOM 4
+
+/// Minimum number of filled buffers before timeout until execution starts
+#define PLANNER_EXEC_MIN_FILL 4
+
+/// Delay before executing new buffers unless PLANNER_EXEC_MIN_FILL is met
+/// This gives the planner a chance to make a good plan before execution starts
+#define PLANNER_EXEC_DELAY 250 // In ms
+
+
+// I2C
+#define I2C_DEV TWIC
+#define I2C_ISR TWIC_TWIS_vect
+#define I2C_ADDR 0x2b
+#define I2C_MAX_DATA 8
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2016 Buildbotics LLC
+ All rights reserved.
+
+ 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/>.
+
+ The software is distributed in the hope that it will be useful, but
+ WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public
+ License along with the software. If not, see
+ <http://www.gnu.org/licenses/>.
+
+ For information regarding this software email:
+ "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#include "coolant.h"
+#include "config.h"
+
+#include <avr/io.h>
+
+
+void coolant_init() {
+ OUTSET_PIN(SWITCH_1_PIN); // High
+ DIRSET_PIN(SWITCH_1_PIN); // Output
+ OUTSET_PIN(SWITCH_2_PIN); // High
+ DIRSET_PIN(SWITCH_2_PIN); // Output
+}
+
+
+void coolant_set_mist(bool x) {
+ if (x) OUTCLR_PIN(SWITCH_1_PIN);
+ else OUTSET_PIN(SWITCH_1_PIN);
+}
+
+
+void coolant_set_flood(bool x) {
+ if (x) OUTCLR_PIN(SWITCH_2_PIN);
+ else OUTSET_PIN(SWITCH_2_PIN);
+}
+
+
+bool coolant_get_mist() {return OUT_PIN(SWITCH_1_PIN);}
+bool coolant_get_flood() {return OUT_PIN(SWITCH_2_PIN);}
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2016 Buildbotics LLC
+ All rights reserved.
+
+ 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/>.
+
+ The software is distributed in the hope that it will be useful, but
+ WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public
+ License along with the software. If not, see
+ <http://www.gnu.org/licenses/>.
+
+ For information regarding this software email:
+ "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#pragma once
+
+#include <stdbool.h>
+
+
+void coolant_init();
+void coolant_set_mist(bool x);
+void coolant_set_flood(bool x);
+bool coolant_get_mist();
+bool coolant_get_flood();
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the uSHET library.
+ See https://github.com/18sg/uSHET
+
+ Copyright (c) 2014 Thomas Nixon, Jonathan Heathcote
+ All rights reserved.
+
+ Permission is hereby granted, free of charge, to any person obtaining
+ a copy of this software and associated documentation files (the
+ "Software"), to deal in the Software without restriction, including
+ without limitation the rights to use, copy, modify, merge, publish,
+ distribute, sublicense, and/or sell copies of the Software, and to
+ permit persons to whom the Software is furnished to do so, subject to
+ the following conditions:
+
+ The above copyright notice and this permission notice shall be
+ included in all copies or substantial portions of the Software.
+
+ THE SOFTWARE IS PROVIDED "AS IS", WITHOUT 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.
+
+\******************************************************************************/
+
+/* This header file contains a library of advanced C Pre-Processor (CPP) macros
+ * which implement various useful functions, such as iteration, in the
+ * pre-processor.
+ *
+ * Though the file name (quite validly) labels this as magic, there should be
+ * enough documentation in the comments for a reader only casually familiar
+ * with the CPP to be able to understand how everything works.
+ *
+ * The majority of the magic tricks used in this file are based on those
+ * described by pfultz2 in his "Cloak" library:
+ *
+ * https://github.com/pfultz2/Cloak/wiki/C-Preprocessor-tricks,-tips,-and-idioms
+ *
+ * Major differences are a greater level of detailed explanation in this
+ * implementation and also a refusal to include any macros which require a O(N)
+ * macro definitions to handle O(N) arguments (with the exception of DEFERn).
+ */
+
+#pragma once
+
+/**
+ * Force the pre-processor to expand the macro a large number of times.
+ * Usage:
+ *
+ * EVAL(expression)
+ *
+ * This is useful when you have a macro which evaluates to a valid
+ * macro expression which is not subsequently expanded in the same
+ * pass. A contrived, but easy to understand, example of such a macro
+ * follows. Note that though this example is contrived, this behaviour
+ * is abused to implement bounded recursion in macros such as FOR.
+ *
+ * #define A(x) x+1
+ * #define EMPTY
+ * #define NOT_QUITE_RIGHT(x) A EMPTY (x)
+ * NOT_QUITE_RIGHT(999)
+ *
+ * Here's what happens inside the C preprocessor:
+ *
+ * 1. It sees a macro "NOT_QUITE_RIGHT" and performs a single macro
+ * expansion pass on its arguments. Since the argument is "999" and
+ * this isn't a macro, this is a boring step resulting in no
+ * change.
+ *
+ * 2. The NOT_QUITE_RIGHT macro is substituted for its definition
+ * giving "A EMPTY() (x)".
+ *
+ * 3. The expander moves from left-to-right trying to expand the
+ * macro: The first token, A, cannot be expanded since there are no
+ * brackets immediately following it. The second token EMPTY(),
+ * however, can be expanded (recursively in this manner) and is
+ * replaced with "".
+ *
+ * 4. Expansion continues from the start of the substituted test
+ * (which in this case is just empty), and sees "(999)" but since
+ * no macro name is present, nothing is done. This results in a
+ * final expansion of "A (999)".
+ *
+ * Unfortunately, this doesn't quite meet expectations since you may
+ * expect that "A (999)" would have been expanded into
+ * "999+1". Unfortunately this requires a second expansion pass but
+ * luckily we can force the macro processor to make more passes by
+ * abusing the first step of macro expansion: the preprocessor expands
+ * arguments in their own pass. If we define a macro which does
+ * nothing except produce its arguments e.g.:
+ *
+ * #define PASS_THROUGH(...) __VA_ARGS__
+ *
+ * We can now do "PASS_THROUGH(NOT_QUITE_RIGHT(999))" causing
+ * "NOT_QUITE_RIGHT" to be expanded to "A (999)", as described above,
+ * when the arguments are expanded. Now when the body of PASS_THROUGH
+ * is expanded, "A (999)" gets expanded to "999+1".
+ *
+ * The EVAL defined below is essentially equivalent to a large nesting
+ * of "PASS_THROUGH(PASS_THROUGH(PASS_THROUGH(..." which results in
+ * the preprocessor making a large number of expansion passes over the
+ * given expression.
+ */
+#define EVAL(...) EVAL1024(__VA_ARGS__)
+#define EVAL1024(...) EVAL512(EVAL512(__VA_ARGS__))
+#define EVAL512(...) EVAL256(EVAL256(__VA_ARGS__))
+#define EVAL256(...) EVAL128(EVAL128(__VA_ARGS__))
+#define EVAL128(...) EVAL64(EVAL64(__VA_ARGS__))
+#define EVAL64(...) EVAL32(EVAL32(__VA_ARGS__))
+#define EVAL32(...) EVAL16(EVAL16(__VA_ARGS__))
+#define EVAL16(...) EVAL8(EVAL8(__VA_ARGS__))
+#define EVAL8(...) EVAL4(EVAL4(__VA_ARGS__))
+#define EVAL4(...) EVAL2(EVAL2(__VA_ARGS__))
+#define EVAL2(...) EVAL1(EVAL1(__VA_ARGS__))
+#define EVAL1(...) __VA_ARGS__
+
+
+// Macros which expand to common values
+#define PASS(...) __VA_ARGS__
+#define EMPTY()
+#define COMMA() ,
+#define SEMI() ;
+#define PLUS() +
+#define ZERO() 0
+#define ONE() 1
+
+/**
+ * Causes a function-style macro to require an additional pass to be expanded.
+ *
+ * This is useful, for example, when trying to implement recursion since the
+ * recursive step must not be expanded in a single pass as the pre-processor
+ * will catch it and prevent it.
+ *
+ * Usage:
+ *
+ * DEFER1(IN_NEXT_PASS)(args, to, the, macro)
+ *
+ * How it works:
+ *
+ * 1. When DEFER1 is expanded, first its arguments are expanded which are
+ * simply IN_NEXT_PASS. Since this is a function-style macro and it has no
+ * arguments, nothing will happen.
+ * 2. The body of DEFER1 will now be expanded resulting in EMPTY() being
+ * deleted. This results in "IN_NEXT_PASS (args, to, the macro)". Note that
+ * since the macro expander has already passed IN_NEXT_PASS by the time it
+ * expands EMPTY() and so it won't spot that the brackets which remain can be
+ * applied to IN_NEXT_PASS.
+ * 3. At this point the macro expansion completes. If one more pass is made,
+ * IN_NEXT_PASS(args, to, the, macro) will be expanded as desired.
+ */
+#define DEFER1(id) id EMPTY()
+
+/**
+ * As with DEFER1 except here n additional passes are required for DEFERn.
+ *
+ * The mechanism is analogous.
+ *
+ * Note that there doesn't appear to be a way of combining DEFERn macros in
+ * order to achieve exponentially increasing defers e.g. as is done by EVAL.
+ */
+#define DEFER2(id) id EMPTY EMPTY()()
+#define DEFER3(id) id EMPTY EMPTY EMPTY()()()
+#define DEFER4(id) id EMPTY EMPTY EMPTY EMPTY()()()()
+#define DEFER5(id) id EMPTY EMPTY EMPTY EMPTY EMPTY()()()()()
+#define DEFER6(id) id EMPTY EMPTY EMPTY EMPTY EMPTY EMPTY()()()()()()
+#define DEFER7(id) id EMPTY EMPTY EMPTY EMPTY EMPTY EMPTY EMPTY()()()()()()()
+#define DEFER8(id) \
+ id EMPTY EMPTY EMPTY EMPTY EMPTY EMPTY EMPTY EMPTY()()()()()()()()
+
+
+/**
+ * Indirection around the standard ## concatenation operator. This simply
+ * ensures that the arguments are expanded (once) before concatenation.
+ */
+#define CAT(a, ...) a ## __VA_ARGS__
+#define CAT3(a, b, ...) a ## b ## __VA_ARGS__
+
+
+/**
+ * Get the first argument and ignore the rest.
+ */
+#define FIRST(a, ...) a
+
+/**
+ * Get the second argument and ignore the rest.
+ */
+#define SECOND(a, b, ...) b
+
+/**
+ * Expects a single input (not containing commas). Returns 1 if the input is
+ * PROBE() and otherwise returns 0.
+ *
+ * This can be useful as the basis of a NOT function.
+ *
+ * This macro abuses the fact that PROBE() contains a comma while other valid
+ * inputs must not.
+ */
+#define IS_PROBE(...) SECOND(__VA_ARGS__, 0)
+#define PROBE() ~, 1
+
+
+/**
+ * Logical negation. 0 is defined as false and everything else as true.
+ *
+ * When 0, _NOT_0 will be found which evaluates to the PROBE. When 1
+ * (or any other value) is given, an appropriately named macro won't
+ * be found and the concatenated string will be produced. IS_PROBE
+ * then simply checks to see if the PROBE was returned, cleanly
+ * converting the argument into a 1 or 0.
+ */
+#define NOT(x) IS_PROBE(CAT(_NOT_, x))
+#define _NOT_0 PROBE()
+
+/**
+ * Macro version of C's famous "cast to bool" operator (i.e. !!) which takes
+ * anything and casts it to 0 if it is 0 and 1 otherwise.
+ */
+#define BOOL(x) NOT(NOT(x))
+
+/**
+ * Logical OR. Simply performs a lookup.
+ */
+#define OR(a,b) CAT3(_OR_, a, b)
+#define _OR_00 0
+#define _OR_01 1
+#define _OR_10 1
+#define _OR_11 1
+
+/**
+ * Logical AND. Simply performs a lookup.
+ */
+#define AND(a,b) CAT3(_AND_, a, b)
+#define _AND_00 0
+#define _AND_01 0
+#define _AND_10 0
+#define _AND_11 1
+
+
+/**
+ * Macro if statement. Usage:
+ *
+ * IF(c)(expansion when true)
+ *
+ * Here's how:
+ *
+ * 1. The preprocessor expands the arguments to _IF casting the condition to '0'
+ * or '1'.
+ * 2. The casted condition is concatencated with _IF_ giving _IF_0 or _IF_1.
+ * 3. The _IF_0 and _IF_1 macros either returns the argument or doesn't (e.g.
+ * they implement the "choice selection" part of the macro).
+ * 4. Note that the "true" clause is in the extra set of brackets; thus these
+ * become the arguments to _IF_0 or _IF_1 and thus a selection is made!
+ */
+#define IF(c) _IF(BOOL(c))
+#define _IF(c) CAT(_IF_,c)
+#define _IF_0(...)
+#define _IF_1(...) __VA_ARGS__
+
+/**
+ * Macro if/else statement. Usage:
+ *
+ * IF_ELSE(c)( \
+ * expansion when true, \
+ * expansion when false \
+ * )
+ *
+ * The mechanism is analogous to IF.
+ */
+#define IF_ELSE(c) _IF_ELSE(BOOL(c))
+#define _IF_ELSE(c) CAT(_IF_ELSE_,c)
+#define _IF_ELSE_0(t,f) f
+#define _IF_ELSE_1(t,f) t
+
+
+/**
+ * Macro which checks if it has any arguments. Returns '0' if there are no
+ * arguments, '1' otherwise.
+ *
+ * Limitation: HAS_ARGS(,1,2,3) returns 0 -- this check essentially only checks
+ * that the first argument exists.
+ *
+ * This macro works as follows:
+ *
+ * 1. _END_OF_ARGUMENTS_ is concatenated with the first argument.
+ * 2. If the first argument is not present then only "_END_OF_ARGUMENTS_" will
+ * remain, otherwise "_END_OF_ARGUMENTS something_here" will remain.
+ * 3. In the former case, the _END_OF_ARGUMENTS_() macro expands to a
+ * 0 when it is expanded. In the latter, a non-zero result remains.
+ * 4. BOOL is used to force non-zero results into 1 giving the clean 0 or 1
+ * output required.
+ */
+#define HAS_ARGS(...) BOOL(FIRST(_END_OF_ARGUMENTS_ __VA_ARGS__)())
+#define _END_OF_ARGUMENTS_() 0
+
+
+/**
+ * Macro map/list comprehension. Usage:
+ *
+ * MAP(op, sep, ...)
+ *
+ * Produces a 'sep()'-separated list of the result of op(arg) for each arg.
+ *
+ * Example Usage:
+ *
+ * #define MAKE_HAPPY(x) happy_##x
+ * #define COMMA() ,
+ * MAP(MAKE_HAPPY, COMMA, 1,2,3)
+ *
+ * Which expands to:
+ *
+ * happy_1 , happy_2 , happy_3
+ *
+ * How it works:
+ *
+ * 1. The MAP macro simply maps the inner MAP_INNER function in an EVAL which
+ * forces it to be expanded a large number of times, thus enabling many steps
+ * of iteration (see step 6).
+ * 2. The MAP_INNER macro is substituted for its body.
+ * 3. In the body, op(cur_val) is substituted giving the value for this
+ * iteration.
+ * 4. The IF macro expands according to whether further iterations are required.
+ * This expansion either produces _IF_0 or _IF_1.
+ * 5. Since the IF is followed by a set of brackets containing the "if true"
+ * clause, these become the argument to _IF_0 or _IF_1. At this point, the
+ * macro in the brackets will be expanded giving the separator followed by
+ * _MAP_INNER EMPTY()()(op, sep, __VA_ARGS__).
+ * 5. If the IF was not taken, the above will simply be discarded and everything
+ * stops. If the IF is taken, The expression is then processed a second time
+ * yielding "_MAP_INNER()(op, sep, __VA_ARGS__)". Note that this call looks
+ * very similar to the essentially the same as the original call except the
+ * first argument has been dropped.
+ * 6. At this point expansion of MAP_INNER will terminate. However, since we can
+ * force more rounds of expansion using EVAL1. In the argument-expansion pass
+ * of the EVAL1, _MAP_INNER() is expanded to MAP_INNER which is then expanded
+ * using the arguments which follow it as in step 2-5. This is followed by a
+ * second expansion pass as the substitution of EVAL1() is expanded executing
+ * 2-5 a second time. This results in up to two iterations occurring. Using
+ * many nested EVAL1 macros, i.e. the very-deeply-nested EVAL macro, will in
+ * this manner produce further iterations, hence the outer MAP macro doing
+ * this for us.
+ *
+ * Important tricks used:
+ *
+ * * If we directly produce "MAP_INNER" in an expansion of MAP_INNER,
+ * a special case in the preprocessor will prevent it being expanded
+ * in the future, even if we EVAL. As a result, the MAP_INNER macro
+ * carefully only expands to something containing "_MAP_INNER()"
+ * which requires a further expansion step to invoke MAP_INNER and
+ * thus implementing the recursion.
+ *
+ * * To prevent _MAP_INNER being expanded within the macro we must
+ * first defer its expansion during its initial pass as an argument
+ * to _IF_0 or _IF_1. We must then defer its expansion a second time
+ * as part of the body of the _IF_0. As a result hence the DEFER2.
+ *
+ * * _MAP_INNER seemingly gets away with producing itself because it
+ * actually only produces MAP_INNER. It just happens that when
+ * _MAP_INNER() is expanded in this case it is followed by some
+ * arguments which get consumed by MAP_INNER and produce a
+ * _MAP_INNER. As such, the macro expander never marks _MAP_INNER
+ * as expanding to itself and thus it will still be expanded in
+ * future productions of itself.
+ */
+#define MAP(...) \
+ IF(HAS_ARGS(__VA_ARGS__))(EVAL(MAP_INNER(__VA_ARGS__)))
+#define MAP_INNER(op,sep,cur_val, ...) \
+ op(cur_val) \
+ IF(HAS_ARGS(__VA_ARGS__))( \
+ sep() DEFER2(_MAP_INNER)()(op, sep, ##__VA_ARGS__) \
+ )
+#define _MAP_INNER() MAP_INNER
+
+
+/**
+ * This is a variant of the MAP macro which also includes as an argument to the
+ * operation a valid C variable name which is different for each iteration.
+ *
+ * Usage:
+ * MAP_WITH_ID(op, sep, ...)
+ *
+ * Where op is a macro op(val, id) which takes a list value and an ID. This ID
+ * will simply be a unary number using the digit "I", that is, I, II, III, IIII,
+ * and so on.
+ *
+ * Example:
+ *
+ * #define MAKE_STATIC_VAR(type, name) static type name;
+ * MAP_WITH_ID(MAKE_STATIC_VAR, EMPTY, int, int, int, bool, char)
+ *
+ * Which expands to:
+ *
+ * static int I; static int II; static int III; static bool IIII;
+ * static char IIIII;
+ *
+ * The mechanism is analogous to the MAP macro.
+ */
+#define MAP_WITH_ID(op,sep,...) \
+ IF(HAS_ARGS(__VA_ARGS__))(EVAL(MAP_WITH_ID_INNER(op,sep,I, ##__VA_ARGS__)))
+#define MAP_WITH_ID_INNER(op,sep,id,cur_val, ...) \
+ op(cur_val,id) \
+ IF(HAS_ARGS(__VA_ARGS__))( \
+ sep() DEFER2(_MAP_WITH_ID_INNER)()(op, sep, CAT(id,I), ##__VA_ARGS__) \
+ )
+#define _MAP_WITH_ID_INNER() MAP_WITH_ID_INNER
+
+
+/**
+ * This is a variant of the MAP macro which iterates over pairs rather than
+ * singletons.
+ *
+ * Usage:
+ * MAP_PAIRS(op, sep, ...)
+ *
+ * Where op is a macro op(val_1, val_2) which takes two list values.
+ *
+ * Example:
+ *
+ * #define MAKE_STATIC_VAR(type, name) static type name;
+ * MAP_PAIRS(MAKE_STATIC_VAR, EMPTY, char, my_char, int, my_int)
+ *
+ * Which expands to:
+ *
+ * static char my_char; static int my_int;
+ *
+ * The mechanism is analogous to the MAP macro.
+ */
+#define MAP_PAIRS(op,sep,...) \
+ IF(HAS_ARGS(__VA_ARGS__))(EVAL(MAP_PAIRS_INNER(op,sep,__VA_ARGS__)))
+#define MAP_PAIRS_INNER(op,sep,cur_val_1, cur_val_2, ...) \
+ op(cur_val_1,cur_val_2) \
+ IF(HAS_ARGS(__VA_ARGS__))( \
+ sep() DEFER2(_MAP_PAIRS_INNER)()(op, sep, __VA_ARGS__) \
+ )
+#define _MAP_PAIRS_INNER() MAP_PAIRS_INNER
+
+/**
+ * This is a variant of the MAP macro which iterates over a two-element sliding
+ * window.
+ *
+ * Usage:
+ * MAP_SLIDE(op, last_op, sep, ...)
+ *
+ * Where op is a macro op(val_1, val_2) which takes the two list values
+ * currently in the window. last_op is a macro taking a single value which is
+ * called for the last argument.
+ *
+ * Example:
+ *
+ * #define SIMON_SAYS_OP(simon, next) IF(NOT(simon()))(next)
+ * #define SIMON_SAYS_LAST_OP(val) last_but_not_least_##val
+ * #define SIMON_SAYS() 0
+ *
+ * MAP_SLIDE(SIMON_SAYS_OP, SIMON_SAYS_LAST_OP, EMPTY, wiggle, SIMON_SAYS,
+ * dance, move, SIMON_SAYS, boogie, stop)
+ *
+ * Which expands to:
+ *
+ * dance boogie last_but_not_least_stop
+ *
+ * The mechanism is analogous to the MAP macro.
+ */
+#define MAP_SLIDE(op,last_op,sep,...) \
+ IF(HAS_ARGS(__VA_ARGS__))(EVAL(MAP_SLIDE_INNER(op,last_op,sep,__VA_ARGS__)))
+#define MAP_SLIDE_INNER(op,last_op,sep,cur_val, ...) \
+ IF(HAS_ARGS(__VA_ARGS__))(op(cur_val,FIRST(__VA_ARGS__))) \
+ IF(NOT(HAS_ARGS(__VA_ARGS__)))(last_op(cur_val)) \
+ IF(HAS_ARGS(__VA_ARGS__))( \
+ sep() DEFER2(_MAP_SLIDE_INNER)()(op, last_op, sep, __VA_ARGS__) \
+ )
+#define _MAP_SLIDE_INNER() MAP_SLIDE_INNER
+
+
+/**
+ * Strip any excess commas from a set of arguments.
+ */
+#define REMOVE_TRAILING_COMMAS(...) \
+ MAP(PASS, COMMA, __VA_ARGS__)
+
+
+/**
+ * Evaluates an array of macros passing 1 argument
+ */
+#define EMAP1(...) \
+ IF(HAS_ARGS(__VA_ARGS__))(EVAL(EMAP1_INNER(__VA_ARGS__)))
+
+#define EMAP1_INNER(ARG1, OP, ...) \
+ _##OP(ARG1) \
+ IF(HAS_ARGS(__VA_ARGS__)) \
+ (DEFER2(_EMAP1_INNER)()(ARG1, ##__VA_ARGS__))
+
+#define _EMAP1_INNER() EMAP1_INNER
+
+
+/**
+ * Evaluates an array of macros passing 2 arguments
+ */
+#define EMAP2(...) \
+ IF(HAS_ARGS(__VA_ARGS__))(EVAL(EMAP2_INNER(__VA_ARGS__)))
+
+#define EMAP2_INNER(ARG1, ARG2, OP, ...) \
+ _##OP(ARG1, ARG2) \
+ IF(HAS_ARGS(__VA_ARGS__)) \
+ (DEFER2(_EMAP2_INNER)()(ARG1, ARG2, ##__VA_ARGS__))
+
+#define _EMAP2_INNER() EMAP2_INNER
+
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2016 Buildbotics LLC
+ All rights reserved.
+
+ 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/>.
+
+ The software is distributed in the hope that it will be useful, but
+ WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public
+ License along with the software. If not, see
+ <http://www.gnu.org/licenses/>.
+
+ For information regarding this software email:
+ "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#include "drv8711.h"
+#include "status.h"
+#include "motor.h"
+
+#include <avr/interrupt.h>
+#include <util/delay.h>
+
+#include <string.h>
+#include <stdlib.h>
+#include <stdio.h>
+
+
+#define DRIVERS MOTORS
+#define COMMANDS 10
+
+
+#define DRV8711_WORD_BYTE_PTR(WORD, LOW) \
+ (((uint8_t *)&(WORD)) + ((LOW) ? 0 : 1))
+
+
+bool motor_fault = false;
+
+
+typedef struct {
+ uint8_t status;
+
+ bool active;
+ float idle_current;
+ float drive_current;
+ float stall_threshold;
+ float power;
+
+ uint8_t mode; // microstepping mode
+ stall_callback_t stall_cb;
+
+ uint8_t cs_pin;
+ uint8_t stall_pin;
+} drv8711_driver_t;
+
+
+static drv8711_driver_t drivers[DRIVERS] = {
+ {.cs_pin = SPI_CS_X_PIN, .stall_pin = STALL_X_PIN},
+ {.cs_pin = SPI_CS_Y_PIN, .stall_pin = STALL_Y_PIN},
+ {.cs_pin = SPI_CS_Z_PIN, .stall_pin = STALL_Z_PIN},
+ {.cs_pin = SPI_CS_A_PIN, .stall_pin = STALL_A_PIN},
+};
+
+
+typedef struct {
+ uint8_t *read;
+ bool callback;
+ uint8_t disable_cs_pin;
+
+ uint8_t cmd;
+ uint8_t driver;
+ bool low_byte;
+
+ uint8_t ncmds;
+ uint16_t commands[DRIVERS][COMMANDS];
+ uint16_t responses[DRIVERS];
+} spi_t;
+
+static spi_t spi = {0};
+
+
+static void _driver_check_status(int driver) {
+ uint8_t status = drivers[driver].status;
+ uint8_t mflags = 0;
+
+ if (status & DRV8711_STATUS_OTS_bm) mflags |= MOTOR_FLAG_OVER_TEMP_bm;
+ if (status & DRV8711_STATUS_AOCP_bm) mflags |= MOTOR_FLAG_OVER_CURRENT_bm;
+ if (status & DRV8711_STATUS_BOCP_bm) mflags |= MOTOR_FLAG_OVER_CURRENT_bm;
+ if (status & DRV8711_STATUS_APDF_bm) mflags |= MOTOR_FLAG_DRIVER_FAULT_bm;
+ if (status & DRV8711_STATUS_BPDF_bm) mflags |= MOTOR_FLAG_DRIVER_FAULT_bm;
+ if (status & DRV8711_STATUS_UVLO_bm) mflags |= MOTOR_FLAG_UNDER_VOLTAGE_bm;
+ if (status & DRV8711_STATUS_STD_bm) mflags |= MOTOR_FLAG_STALLED_bm;
+ if (status & DRV8711_STATUS_STDLAT_bm) mflags |= MOTOR_FLAG_STALLED_bm;
+
+ //if (mflags) motor_error_callback(driver, mflags); TODO
+}
+
+
+static float _driver_get_current(int driver) {
+ drv8711_driver_t *drv = &drivers[driver];
+
+#if 1
+ if (!drv->active) return drv->idle_current;
+ return
+ MOTOR_MIN_CURRENT + (drv->drive_current - MOTOR_MIN_CURRENT) * drv->power;
+
+#else
+ return drv->active ? drv->drive_current : drv->idle_current;
+#endif
+}
+
+
+static uint8_t _spi_next_command(uint8_t cmd) {
+ // Process status responses
+ for (int driver = 0; driver < DRIVERS; driver++) {
+ uint16_t command = spi.commands[driver][cmd];
+
+ if (DRV8711_CMD_IS_READ(command) &&
+ DRV8711_CMD_ADDR(command) == DRV8711_STATUS_REG) {
+ drivers[driver].status = spi.responses[driver];
+ _driver_check_status(driver);
+ }
+ }
+
+ // Next command
+ if (++cmd == spi.ncmds) {
+ cmd = 0; // Wrap around
+
+ for (int driver = 0; driver < DRIVERS; driver++)
+ motor_driver_callback(driver);
+ }
+
+ // Prep next command
+ for (int driver = 0; driver < DRIVERS; driver++) {
+ uint16_t *command = &spi.commands[driver][cmd];
+
+ switch (DRV8711_CMD_ADDR(*command)) {
+ case DRV8711_STATUS_REG:
+ if (!DRV8711_CMD_IS_READ(*command))
+ *command = (*command & 0xf000) | (0x0fff & ~(drivers[driver].status));
+ break;
+
+ case DRV8711_TORQUE_REG: // Update motor current setting
+ *command = (*command & 0xff00) |
+ (uint8_t)round(0xff * _driver_get_current(driver));
+ break;
+
+ case DRV8711_CTRL_REG: // Set microsteps
+ *command = (*command & 0xff87) | (drivers[driver].mode << 3);
+ break;
+
+ default: break;
+ }
+ }
+
+ return cmd;
+}
+
+
+static void _spi_send() {
+ // Flush any status errors (TODO check for errors)
+ uint8_t x = SPIC.STATUS;
+ x = x;
+
+ // Disable CS
+ if (spi.disable_cs_pin) {
+ OUTCLR_PIN(spi.disable_cs_pin); // Set low (inactive)
+ _delay_us(1);
+ spi.disable_cs_pin = 0;
+ }
+
+ // Schedule next CS disable or enable next CS now
+ if (spi.low_byte) spi.disable_cs_pin = drivers[spi.driver].cs_pin;
+ else {
+ OUTSET_PIN(drivers[spi.driver].cs_pin); // Set high (active)
+ _delay_us(1);
+ }
+
+ // Read
+ if (spi.read) {
+ *spi.read = SPIC.DATA;
+ spi.read = 0;
+ }
+
+ // Callback, passing current command index, and get next command index
+ if (spi.callback) {
+ spi.cmd = _spi_next_command(spi.cmd);
+ spi.callback = false;
+ }
+
+ // Write byte and prep next read
+ SPIC.DATA =
+ *DRV8711_WORD_BYTE_PTR(spi.commands[spi.driver][spi.cmd], spi.low_byte);
+ spi.read = DRV8711_WORD_BYTE_PTR(spi.responses[spi.driver], spi.low_byte);
+
+ // Check if WORD complete, go to next driver & check if command finished
+ if (spi.low_byte && ++spi.driver == DRIVERS) {
+ spi.driver = 0; // Wrap around
+ spi.callback = true; // Call back after last byte is read
+ }
+
+ // Next byte
+ spi.low_byte = !spi.low_byte;
+}
+
+
+static void _init_spi_commands() {
+ // Setup SPI command sequence
+ for (int driver = 0; driver < DRIVERS; driver++) {
+ uint16_t *commands = spi.commands[driver];
+ spi.ncmds = 0;
+
+ // Set OFF
+ commands[spi.ncmds++] = DRV8711_WRITE(DRV8711_OFF_REG, 12);
+
+ // Set BLANK
+ commands[spi.ncmds++] = DRV8711_WRITE(DRV8711_BLANK_REG, 0x80);
+
+ // Set DECAY
+ commands[spi.ncmds++] =
+ DRV8711_WRITE(DRV8711_DECAY_REG, DRV8711_DECAY_DECMOD_AUTO_OPT | 6);
+
+ // Set STALL
+ commands[spi.ncmds++] =
+ DRV8711_WRITE(DRV8711_STALL_REG,
+ DRV8711_STALL_SDCNT_2 | DRV8711_STALL_VDIV_8 | 200);
+
+ // Set DRIVE
+ commands[spi.ncmds++] =
+ DRV8711_WRITE(DRV8711_DRIVE_REG, DRV8711_DRIVE_IDRIVEP_50 |
+ DRV8711_DRIVE_IDRIVEN_100 | DRV8711_DRIVE_TDRIVEP_500 |
+ DRV8711_DRIVE_TDRIVEN_500 | DRV8711_DRIVE_OCPDEG_2 |
+ DRV8711_DRIVE_OCPTH_500);
+
+ // Set TORQUE
+ commands[spi.ncmds++] =
+ DRV8711_WRITE(DRV8711_TORQUE_REG, DRV8711_TORQUE_SMPLTH_50);
+
+ // Set CTRL enable motor & set ISENSE gain
+ commands[spi.ncmds++] =
+ DRV8711_WRITE(DRV8711_CTRL_REG, DRV8711_CTRL_ENBL_bm |
+ DRV8711_CTRL_ISGAIN_10 | DRV8711_CTRL_DTIME_850);
+
+ // Read STATUS
+ commands[spi.ncmds++] = DRV8711_READ(DRV8711_STATUS_REG);
+
+ // Clear STATUS
+ commands[spi.ncmds++] = DRV8711_WRITE(DRV8711_STATUS_REG, 0);
+ }
+
+ if (COMMANDS < spi.ncmds)
+ STATUS_ERROR(STAT_INTERNAL_ERROR,
+ "SPI command buffer overflow increase COMMANDS in %s",
+ __FILE__);
+
+ _spi_send(); // Kick it off
+}
+
+
+ISR(SPIC_INT_vect) {_spi_send();}
+
+
+ISR(STALL_ISR_vect) {
+ for (int i = 0; i < DRIVERS; i++) {
+ drv8711_driver_t *driver = &drivers[i];
+ if (driver->stall_cb) driver->stall_cb(i);
+ }
+}
+
+
+ISR(FAULT_ISR_vect) {motor_fault = !IN_PIN(MOTOR_FAULT_PIN);} // TODO
+
+
+void drv8711_init() {
+ // Configure drivers
+ for (int i = 0; i < DRIVERS; i++) {
+ drivers[i].idle_current = MOTOR_IDLE_CURRENT;
+ drivers[i].drive_current = MOTOR_MAX_CURRENT;
+ drivers[i].stall_threshold = MOTOR_STALL_THRESHOLD;
+
+ drv8711_disable(i);
+ }
+
+ // Setup pins
+ // Must set the SS pin either in/high or any/output for master mode to work
+ // Note, this pin is also used by the USART as the CTS line
+ DIRSET_PIN(SPI_SS_PIN); // Output
+ OUTSET_PIN(SPI_CLK_PIN); // High
+ DIRSET_PIN(SPI_CLK_PIN); // Output
+ DIRCLR_PIN(SPI_MISO_PIN); // Input
+ OUTSET_PIN(SPI_MOSI_PIN); // High
+ DIRSET_PIN(SPI_MOSI_PIN); // Output
+
+ for (int i = 0; i < DRIVERS; i++) {
+ uint8_t cs_pin = drivers[i].cs_pin;
+ uint8_t stall_pin = drivers[i].stall_pin;
+
+ OUTSET_PIN(cs_pin); // High
+ DIRSET_PIN(cs_pin); // Output
+ DIRCLR_PIN(stall_pin); // Input
+
+ // Stall interrupt
+ PINCTRL_PIN(stall_pin) = PORT_ISC_FALLING_gc;
+ PORT(stall_pin)->INT1MASK |= BM(stall_pin);
+ PORT(stall_pin)->INTCTRL |= PORT_INT1LVL_HI_gc;
+ }
+
+ // Fault interrupt
+ DIRCLR_PIN(MOTOR_FAULT_PIN);
+ PINCTRL_PIN(MOTOR_FAULT_PIN) = PORT_ISC_RISING_gc;
+ PORT(MOTOR_FAULT_PIN)->INT1MASK |= BM(MOTOR_FAULT_PIN);
+ PORT(MOTOR_FAULT_PIN)->INTCTRL |= PORT_INT1LVL_HI_gc;
+
+ // Configure SPI
+ PR.PRPC &= ~PR_SPI_bm; // Disable power reduction
+ SPIC.CTRL = SPI_ENABLE_bm | SPI_MASTER_bm | SPI_MODE_0_gc |
+ SPI_PRESCALER_DIV16_gc; // enable, big endian, master, mode, clock div
+ PORT(SPI_CLK_PIN)->REMAP = PORT_SPI_bm; // Swap SCK and MOSI
+ SPIC.INTCTRL = SPI_INTLVL_LO_gc; // interupt level
+
+ _init_spi_commands();
+}
+
+
+void drv8711_enable(int driver) {
+ if (driver < 0 || DRIVERS <= driver) return;
+ drivers[driver].active = true;
+}
+
+
+void drv8711_disable(int driver) {
+ if (driver < 0 || DRIVERS <= driver) return;
+ drivers[driver].active = false;
+}
+
+
+void drv8711_set_power(int driver, float power) {
+ if (driver < 0 || DRIVERS <= driver) return;
+ drivers[driver].power = power < 0 ? 0 : (1 < power ? 1 : power);
+}
+
+
+void drv8711_set_microsteps(int driver, uint16_t msteps) {
+ if (driver < 0 || DRIVERS <= driver) return;
+ switch (msteps) {
+ case 1: case 2: case 4: case 8: case 16: case 32: case 64: case 128: case 256:
+ break;
+ default: return; // Invalid
+ }
+
+ drivers[driver].mode = round(logf(msteps) / logf(2));
+}
+
+
+void drv8711_set_stall_callback(int driver, stall_callback_t cb) {
+ drivers[driver].stall_cb = cb;
+}
+
+
+float get_drive_power(int driver) {
+ if (driver < 0 || DRIVERS <= driver) return 0;
+ return drivers[driver].drive_current;
+}
+
+
+void set_drive_power(int driver, float value) {
+ if (driver < 0 || DRIVERS <= driver || value < 0 || 1 < value) return;
+ drivers[driver].drive_current = value;
+}
+
+
+float get_idle_power(int driver) {
+ if (driver < 0 || DRIVERS <= driver) return 0;
+ return drivers[driver].idle_current;
+}
+
+
+void set_idle_power(int driver, float value) {
+ if (driver < 0 || DRIVERS <= driver || value < 0 || 1 < value) return;
+ drivers[driver].idle_current = value;
+}
+
+
+float get_current_power(int driver) {
+ if (driver < 0 || DRIVERS <= driver) return 0;
+ return _driver_get_current(driver);
+}
+
+
+bool get_motor_fault() {return motor_fault;}
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2016 Buildbotics LLC
+ All rights reserved.
+
+ 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/>.
+
+ The software is distributed in the hope that it will be useful, but
+ WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public
+ License along with the software. If not, see
+ <http://www.gnu.org/licenses/>.
+
+ For information regarding this software email:
+ "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#pragma once
+
+
+#include "config.h"
+#include "status.h"
+#include "motor.h"
+
+#include <stdint.h>
+#include <stdbool.h>
+
+enum {
+ DRV8711_CTRL_REG,
+ DRV8711_TORQUE_REG,
+ DRV8711_OFF_REG,
+ DRV8711_BLANK_REG,
+ DRV8711_DECAY_REG,
+ DRV8711_STALL_REG,
+ DRV8711_DRIVE_REG,
+ DRV8711_STATUS_REG,
+};
+
+
+enum {
+ DRV8711_CTRL_ENBL_bm = 1 << 0,
+ DRV8711_CTRL_RDIR_bm = 1 << 1,
+ DRV8711_CTRL_RSTEP_bm = 1 << 2,
+ DRV8711_CTRL_MODE_1 = 0 << 3,
+ DRV8711_CTRL_MODE_2 = 1 << 3,
+ DRV8711_CTRL_MODE_4 = 2 << 3,
+ DRV8711_CTRL_MODE_8 = 3 << 3,
+ DRV8711_CTRL_MODE_16 = 4 << 3,
+ DRV8711_CTRL_MODE_32 = 5 << 3,
+ DRV8711_CTRL_MODE_64 = 6 << 3,
+ DRV8711_CTRL_MODE_128 = 7 << 3,
+ DRV8711_CTRL_MODE_256 = 8 << 3,
+ DRV8711_CTRL_EXSTALL_bm = 1 << 7,
+ DRV8711_CTRL_ISGAIN_5 = 0 << 8,
+ DRV8711_CTRL_ISGAIN_10 = 1 << 8,
+ DRV8711_CTRL_ISGAIN_20 = 2 << 8,
+ DRV8711_CTRL_ISGAIN_40 = 3 << 8,
+ DRV8711_CTRL_DTIME_400 = 0 << 10,
+ DRV8711_CTRL_DTIME_450 = 1 << 10,
+ DRV8711_CTRL_DTIME_650 = 2 << 10,
+ DRV8711_CTRL_DTIME_850 = 3 << 10,
+};
+
+
+enum {
+ DRV8711_TORQUE_SMPLTH_50 = 0 << 8,
+ DRV8711_TORQUE_SMPLTH_100 = 1 << 8,
+ DRV8711_TORQUE_SMPLTH_200 = 2 << 8,
+ DRV8711_TORQUE_SMPLTH_300 = 3 << 8,
+ DRV8711_TORQUE_SMPLTH_400 = 4 << 8,
+ DRV8711_TORQUE_SMPLTH_600 = 5 << 8,
+ DRV8711_TORQUE_SMPLTH_800 = 6 << 8,
+ DRV8711_TORQUE_SMPLTH_1000 = 7 << 8,
+};
+
+
+enum {
+ DRV8711_OFF_PWMMODE_bm = 1 << 8,
+};
+
+
+enum {
+ DRV8711_BLANK_ABT_bm = 1 << 8,
+};
+
+
+enum {
+ DRV8711_DECAY_DECMOD_SLOW = 0 << 8,
+ DRV8711_DECAY_DECMOD_OPT = 1 << 8,
+ DRV8711_DECAY_DECMOD_FAST = 2 << 8,
+ DRV8711_DECAY_DECMOD_MIXED = 3 << 8,
+ DRV8711_DECAY_DECMOD_AUTO_OPT = 4 << 8,
+ DRV8711_DECAY_DECMOD_AUTO_MIXED = 5 << 8,
+};
+
+
+enum {
+ DRV8711_STALL_SDCNT_1 = 0 << 8,
+ DRV8711_STALL_SDCNT_2 = 1 << 8,
+ DRV8711_STALL_SDCNT_4 = 2 << 8,
+ DRV8711_STALL_SDCNT_8 = 3 << 8,
+ DRV8711_STALL_VDIV_32 = 0 << 10,
+ DRV8711_STALL_VDIV_16 = 1 << 10,
+ DRV8711_STALL_VDIV_8 = 2 << 10,
+ DRV8711_STALL_VDIV_4 = 3 << 10,
+};
+
+
+enum {
+ DRV8711_DRIVE_OCPTH_250 = 0 << 0,
+ DRV8711_DRIVE_OCPTH_500 = 1 << 0,
+ DRV8711_DRIVE_OCPTH_750 = 2 << 0,
+ DRV8711_DRIVE_OCPTH_1000 = 3 << 0,
+ DRV8711_DRIVE_OCPDEG_1 = 0 << 2,
+ DRV8711_DRIVE_OCPDEG_2 = 1 << 2,
+ DRV8711_DRIVE_OCPDEG_4 = 2 << 2,
+ DRV8711_DRIVE_OCPDEG_8 = 3 << 2,
+ DRV8711_DRIVE_TDRIVEN_250 = 0 << 4,
+ DRV8711_DRIVE_TDRIVEN_500 = 1 << 4,
+ DRV8711_DRIVE_TDRIVEN_1000 = 2 << 4,
+ DRV8711_DRIVE_TDRIVEN_2000 = 3 << 4,
+ DRV8711_DRIVE_TDRIVEP_250 = 0 << 6,
+ DRV8711_DRIVE_TDRIVEP_500 = 1 << 6,
+ DRV8711_DRIVE_TDRIVEP_1000 = 2 << 6,
+ DRV8711_DRIVE_TDRIVEP_2000 = 3 << 6,
+ DRV8711_DRIVE_IDRIVEN_100 = 0 << 8,
+ DRV8711_DRIVE_IDRIVEN_200 = 1 << 8,
+ DRV8711_DRIVE_IDRIVEN_300 = 2 << 8,
+ DRV8711_DRIVE_IDRIVEN_400 = 3 << 8,
+ DRV8711_DRIVE_IDRIVEP_50 = 0 << 10,
+ DRV8711_DRIVE_IDRIVEP_100 = 1 << 10,
+ DRV8711_DRIVE_IDRIVEP_150 = 2 << 10,
+ DRV8711_DRIVE_IDRIVEP_200 = 3 << 10,
+};
+
+enum {
+ DRV8711_STATUS_OTS_bm = 1 << 0,
+ DRV8711_STATUS_AOCP_bm = 1 << 1,
+ DRV8711_STATUS_BOCP_bm = 1 << 2,
+ DRV8711_STATUS_APDF_bm = 1 << 3,
+ DRV8711_STATUS_BPDF_bm = 1 << 4,
+ DRV8711_STATUS_UVLO_bm = 1 << 5,
+ DRV8711_STATUS_STD_bm = 1 << 6,
+ DRV8711_STATUS_STDLAT_bm = 1 << 7,
+};
+
+
+#define DRV8711_READ(ADDR) ((1 << 15) | ((ADDR) << 12))
+#define DRV8711_WRITE(ADDR, DATA) (((ADDR) << 12) | ((DATA) & 0xfff))
+#define DRV8711_CMD_ADDR(CMD) (((CMD) >> 12) & 7)
+#define DRV8711_CMD_IS_READ(CMD) ((1 << 15) & (CMD))
+
+
+void drv8711_init();
+void drv8711_enable(int driver);
+void drv8711_disable(int driver);
+void drv8711_set_power(int driver, float power);
+void drv8711_set_microsteps(int driver, uint16_t msteps);
+void drv8711_set_stall_callback(int driver, stall_callback_t cb);
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2016 Buildbotics LLC
+ All rights reserved.
+
+ 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/>.
+
+ The software is distributed in the hope that it will be useful, but
+ WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public
+ License along with the software. If not, see
+ <http://www.gnu.org/licenses/>.
+
+ For information regarding this software email:
+ "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#include "estop.h"
+#include "motor.h"
+#include "stepper.h"
+#include "spindle.h"
+#include "switch.h"
+#include "report.h"
+#include "hardware.h"
+#include "homing.h"
+#include "config.h"
+
+#include "plan/planner.h"
+#include "plan/state.h"
+
+#include <avr/eeprom.h>
+
+
+typedef struct {
+ bool triggered;
+} estop_t;
+
+
+static estop_t estop = {0};
+
+static uint16_t estop_reason_eeprom EEMEM;
+
+
+static void _set_reason(stat_t reason) {
+ eeprom_update_word(&estop_reason_eeprom, reason);
+}
+
+
+static stat_t _get_reason() {
+ return eeprom_read_word(&estop_reason_eeprom);
+}
+
+
+static void _switch_callback(switch_id_t id, bool active) {
+ if (active) estop_trigger(STAT_ESTOP_SWITCH);
+ else estop_clear();
+}
+
+
+void estop_init() {
+ if (switch_is_active(SW_ESTOP)) _set_reason(STAT_ESTOP_SWITCH);
+ if (STAT_MAX <= _get_reason()) _set_reason(STAT_OK);
+ estop.triggered = _get_reason() != STAT_OK;
+
+ switch_set_callback(SW_ESTOP, _switch_callback);
+
+ if (estop.triggered) mp_state_estop();
+
+ // Fault signal
+ if (estop.triggered) OUTSET_PIN(FAULT_PIN); // High
+ else OUTCLR_PIN(FAULT_PIN); // Low
+ DIRSET_PIN(FAULT_PIN); // Output
+}
+
+
+bool estop_triggered() {
+ return estop.triggered || switch_is_active(SW_ESTOP);
+}
+
+
+void estop_trigger(stat_t reason) {
+ if (estop.triggered) return;
+ estop.triggered = true;
+
+ // Hard stop the motors and the spindle
+ st_shutdown();
+ spindle_estop();
+
+ // Set machine state
+ mp_state_estop();
+
+ // Set axes not homed
+ mach_set_not_homed();
+
+ // Save reason
+ _set_reason(reason);
+
+ report_request();
+}
+
+
+void estop_clear() {
+ // Check if estop switch is set
+ if (switch_is_active(SW_ESTOP)) {
+ if (_get_reason() != STAT_ESTOP_SWITCH) _set_reason(STAT_ESTOP_SWITCH);
+ return; // Can't clear while estop switch is still active
+ }
+
+ // Clear fault signal
+ OUTCLR_PIN(FAULT_PIN); // Low
+
+ estop.triggered = false;
+
+ // Clear reason
+ _set_reason(STAT_OK);
+
+ // Reboot
+ // Note, hardware.c waits until any spindle stop command has been delivered
+ hw_request_hard_reset();
+}
+
+
+bool get_estop() {
+ return estop_triggered();
+}
+
+
+void set_estop(bool value) {
+ if (value == estop_triggered()) return;
+ if (value) estop_trigger(STAT_ESTOP_USER);
+ else estop_clear();
+}
+
+
+PGM_P get_estop_reason() {
+ return status_to_pgmstr(_get_reason());
+}
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2016 Buildbotics LLC
+ All rights reserved.
+
+ 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/>.
+
+ The software is distributed in the hope that it will be useful, but
+ WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public
+ License along with the software. If not, see
+ <http://www.gnu.org/licenses/>.
+
+ For information regarding this software email:
+ "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#pragma once
+
+#include "status.h"
+
+#include <stdbool.h>
+
+
+void estop_init();
+bool estop_triggered();
+void estop_trigger(stat_t reason);
+void estop_clear();
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2016 Buildbotics LLC
+ Copyright (c) 2010 - 2015 Alden S. Hart, Jr.
+ All rights reserved.
+
+ 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/>.
+
+ The software is distributed in the hope that it will be useful, but
+ WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public
+ License along with the software. If not, see
+ <http://www.gnu.org/licenses/>.
+
+ For information regarding this software email:
+ "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#include "gcode_parser.h"
+
+#include "machine.h"
+#include "plan/arc.h"
+#include "probing.h"
+#include "homing.h"
+#include "axis.h"
+#include "util.h"
+
+#include <stdbool.h>
+#include <string.h>
+#include <ctype.h>
+#include <stdlib.h>
+#include <math.h>
+
+
+parser_t parser = {{0}};
+
+
+#define SET_MODAL(m, parm, val) \
+ {parser.gn.parm = val; parser.gf.parm = true; modals[m] += 1; break;}
+#define SET_NON_MODAL(parm, val) \
+ {parser.gn.parm = val; parser.gf.parm = true; break;}
+#define EXEC_FUNC(f, parm) if (parser.gf.parm) f(parser.gn.parm)
+
+
+static uint8_t modals[MODAL_GROUP_COUNT]; // collects modal groups in a block
+
+
+/* Normalize a block (line) of gcode in place
+ *
+ * Normalization functions:
+ * - 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
+ *
+ * So this: " g1 x100 Y100 f400" becomes this: "G1X100Y100F400"
+ *
+ * Comment and message handling:
+ * - Comments field start with a '(' char or alternately a semicolon ';'
+ * - Comments and messages are not normalized - they are left alone
+ * - The 'MSG' specifier in comment can have mixed case but cannot cannot
+ * have embedded white spaces
+ * - Normalization returns true if there was a message to display, false
+ * otherwise
+ * - Comments always terminate the block - i.e. leading or embedded comments
+ * are not supported
+ * - Valid cases (examples) Notes:
+ * G0X10 - command only - no comment
+ * (comment text) - There is no command on this line
+ * G0X10 (comment text)
+ * G0X10 (comment text - It's OK to drop the trailing paren
+ * G0X10 ;comment text - It's OK to drop the trailing paren
+ *
+ * - Invalid cases (examples) Notes:
+ * G0X10 comment text - Comment with no separator
+ * N10 (comment) G0X10 - embedded comment. G0X10 will be ignored
+ * (comment) G0X10 - leading comment. G0X10 will be ignored
+ * G0X10 # comment - invalid separator
+ *
+ * Returns:
+ * - com points to comment string or to 0 if no comment
+ * - msg points to message string or to 0 if no comment
+ * - block_delete_flag is set true if block delete encountered, false otherwise
+ */
+
+static void _normalize_gcode_block(char *str, char **com, char **msg,
+ uint8_t *block_delete_flag) {
+ char *rd = str; // read pointer
+ char *wr = str; // write pointer
+
+ // mark block deletes
+ *block_delete_flag = *rd == '/';
+
+ // normalize the command block & find the comment (if any)
+ for (; *wr; rd++)
+ if (!*rd) *wr = 0;
+ else if (*rd == '(' || *rd == ';') {*wr = 0; *com = rd + 1;}
+ else if (isalnum(*rd) || strchr("-.", *rd)) // all valid characters
+ *wr++ = toupper(*rd);
+
+ // Perform Octal stripping - remove invalid leading zeros in number strings
+ rd = str;
+ while (*rd) {
+ if (*rd == '.') break; // don't strip past a decimal point
+ if (!isdigit(*rd) && *(rd + 1) == '0' && isdigit(*(rd + 2))) {
+ wr = rd + 1;
+ while (*wr) {*wr = *(wr + 1); wr++;} // copy forward w/overwrite
+ continue;
+ }
+
+ rd++;
+ }
+
+ // process comments and messages
+ if (**com) {
+ rd = *com;
+ while (isspace(*rd)) rd++; // skip any leading spaces before "msg"
+
+ if (tolower(*rd) == 'm' && tolower(*(rd + 1)) == 's' &&
+ tolower(*(rd + 2)) == 'g')
+ *msg = rd + 3;
+
+ for (; *rd; rd++)
+ // 0 terminate on trailing parenthesis, if any
+ if (*rd == ')') *rd = 0;
+ }
+}
+
+
+/* Get gcode word consisting of a letter and a value
+ *
+ * This function requires the Gcode string to be normalized.
+ * Normalization must remove any leading zeros or they will be converted to
+ * octal. G0X... is not interpreted as hexadecimal. This is trapped.
+ */
+static stat_t _get_next_gcode_word(char **pstr, char *letter, float *value) {
+ if (!**pstr) return STAT_COMPLETE; // no more words to process
+
+ // get letter part
+ if (!isupper(**pstr)) return STAT_INVALID_OR_MALFORMED_COMMAND;
+ *letter = **pstr;
+ (*pstr)++;
+
+ // 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
+ }
+
+ // get-value general case
+ char *end;
+ *value = strtod(*pstr, &end);
+ // more robust test then checking for value == 0
+ if (end == *pstr) return STAT_GCODE_COMMAND_UNSUPPORTED;
+ *pstr = end; // pointer points to next character after the word
+
+ return STAT_OK;
+}
+
+
+/// Isolate the decimal point value as an integer
+static uint8_t _point(float value) {return value * 10 - trunc(value) * 10;}
+
+
+#if 0
+static bool _axis_changed() {
+ for (int axis = 0; axis < AXES; axis++)
+ if (parser.gf.target[axis]) return true;
+ return false;
+}
+#endif
+
+
+/// Check for some gross Gcode block semantic violations
+static stat_t _validate_gcode_block() {
+ // 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 use axis words. If an
+ // axis word-using G-code from group 1 is implicitly in effect on
+ // a line (by having been activated on an earlier line), and a
+ // group 0 G-code that 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 (modals[MODAL_GROUP_G0] && modals[MODAL_GROUP_G1])
+ return STAT_MODAL_GROUP_VIOLATION;
+
+#if 0 // This check fails for arcs which may have offsets but no axis word
+ // look for commands that require an axis word to be present
+ if (modals[MODAL_GROUP_G0] || modals[MODAL_GROUP_G1])
+ if (!_axis_changed()) return STAT_GCODE_AXIS_IS_MISSING;
+#endif
+
+ return STAT_OK;
+}
+
+
+/* Execute parsed block
+ *
+ * Conditionally (based on whether a flag is set in gf) call the
+ * machining functions in order of execution as per RS274NGC_3 table 8
+ * (below, with modifications):
+ *
+ * 0. record the line number
+ * 1. comment (includes message) [handled during block normalization]
+ * 2. set feed rate mode (G93, G94 - inverse time or per minute)
+ * 3. set feed rate (F)
+ * 3a. set feed override rate (M50)
+ * 4. set spindle speed (S)
+ * 4a. set spindle override rate (M51)
+ * 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)
+ * 10. dwell (G4)
+ * 11. set active plane (G17, G18, G19)
+ * 12. set length units (G20, G21)
+ * 13. cutter radius compensation on or off (G40, G41, G42)
+ * 14. cutter length compensation on or off (G43, G49)
+ * 15. coordinate system selection (G54, G55, G56, G57, G58, G59)
+ * 16. set path control mode (G61, G61.1, G64)
+ * 17. set distance mode (G90, G91, G90.1, G91.1)
+ * 18. set retract mode (G98, G99)
+ * 19a. homing functions (G28.2, G28.3, G28.1, G28, G30)
+ * 19b. update system data (G10)
+ * 19c. set axis offsets (G92, G92.1, G92.2, G92.3)
+ * 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
+ * to calling the machine functions (which does the unit conversions)
+ */
+static stat_t _execute_gcode_block() {
+ stat_t status = STAT_OK;
+
+ mach_set_line(parser.gn.line);
+ EXEC_FUNC(mach_set_feed_mode, feed_mode);
+ EXEC_FUNC(mach_set_feed_rate, feed_rate);
+ EXEC_FUNC(mach_feed_override_enable, feed_override_enable);
+ EXEC_FUNC(mach_set_spindle_speed, spindle_speed);
+ EXEC_FUNC(mach_spindle_override_enable, spindle_override_enable);
+ EXEC_FUNC(mach_select_tool, tool);
+ EXEC_FUNC(mach_change_tool, tool_change);
+ EXEC_FUNC(mach_set_spindle_mode, spindle_mode);
+ EXEC_FUNC(mach_mist_coolant_control, mist_coolant);
+ EXEC_FUNC(mach_flood_coolant_control, flood_coolant);
+ EXEC_FUNC(mach_override_enables, override_enables);
+
+ if (parser.gn.next_action == NEXT_ACTION_DWELL) // G4 - dwell
+ RITORNO(mach_dwell(parser.gn.parameter));
+
+ EXEC_FUNC(mach_set_plane, plane);
+ EXEC_FUNC(mach_set_units, units);
+ //--> cutter radius compensation goes here
+ //--> cutter length compensation goes here
+ EXEC_FUNC(mach_set_coord_system, coord_system);
+ EXEC_FUNC(mach_set_path_mode, path_mode);
+ EXEC_FUNC(mach_set_distance_mode, distance_mode);
+ EXEC_FUNC(mach_set_arc_distance_mode, arc_distance_mode);
+ //--> set retract mode goes here
+
+ switch (parser.gn.next_action) {
+ case NEXT_ACTION_SET_G28_POSITION: // G28.1
+ mach_set_g28_position();
+ break;
+ case NEXT_ACTION_GOTO_G28_POSITION: // G28
+ status = mach_goto_g28_position(parser.gn.target, parser.gf.target);
+ break;
+ case NEXT_ACTION_SET_G30_POSITION: // G30.1
+ mach_set_g30_position();
+ break;
+ case NEXT_ACTION_GOTO_G30_POSITION: // G30
+ status = mach_goto_g30_position(parser.gn.target, parser.gf.target);
+ break;
+ case NEXT_ACTION_SEARCH_HOME: // G28.2
+ mach_homing_cycle_start();
+ break;
+ case NEXT_ACTION_SET_ABSOLUTE_ORIGIN: // G28.3
+ mach_set_absolute_origin(parser.gn.target, parser.gf.target);
+ break;
+ case NEXT_ACTION_HOMING_NO_SET: // G28.4
+ mach_homing_cycle_start_no_set();
+ break;
+ case NEXT_ACTION_STRAIGHT_PROBE: // G38.2
+ status = mach_probe(parser.gn.target, parser.gf.target);
+ break;
+ case NEXT_ACTION_SET_COORD_DATA:
+ mach_set_coord_offsets(parser.gn.parameter, parser.gn.target,
+ parser.gf.target);
+ break;
+ case NEXT_ACTION_SET_ORIGIN_OFFSETS:
+ mach_set_origin_offsets(parser.gn.target, parser.gf.target);
+ break;
+ case NEXT_ACTION_RESET_ORIGIN_OFFSETS:
+ mach_reset_origin_offsets();
+ break;
+ case NEXT_ACTION_SUSPEND_ORIGIN_OFFSETS:
+ mach_suspend_origin_offsets();
+ break;
+ case NEXT_ACTION_RESUME_ORIGIN_OFFSETS:
+ mach_resume_origin_offsets();
+ break;
+ case NEXT_ACTION_DWELL: break; // Handled above
+
+ case NEXT_ACTION_DEFAULT:
+ // apply override setting to gm struct
+ mach_set_absolute_mode(parser.gn.absolute_mode);
+
+ switch (parser.gn.motion_mode) {
+ case MOTION_MODE_CANCEL_MOTION_MODE:
+ mach_set_motion_mode(parser.gn.motion_mode);
+ break;
+ case MOTION_MODE_RAPID:
+ status = mach_rapid(parser.gn.target, parser.gf.target);
+ break;
+ case MOTION_MODE_FEED:
+ status = mach_feed(parser.gn.target, parser.gf.target);
+ break;
+ case MOTION_MODE_CW_ARC: case MOTION_MODE_CCW_ARC:
+ // gf.radius sets radius mode if radius was collected in gn
+ status = mach_arc_feed(parser.gn.target, parser.gf.target,
+ parser.gn.arc_offset, parser.gf.arc_offset,
+ parser.gn.arc_radius, parser.gf.arc_radius,
+ parser.gn.parameter, parser.gf.parameter,
+ modals[MODAL_GROUP_G1], parser.gn.motion_mode);
+ break;
+ default: break; // Should not get here
+ }
+ }
+ // un-set absolute override once the move is planned
+ mach_set_absolute_mode(false);
+
+ // do the program stops and ends : M0, M1, M2, M30, M60
+ if (parser.gf.program_flow)
+ switch (parser.gn.program_flow) {
+ case PROGRAM_STOP: mach_program_stop(); break;
+ case PROGRAM_OPTIONAL_STOP: mach_optional_program_stop(); break;
+ case PROGRAM_PALLET_CHANGE_STOP: mach_pallet_change_stop(); break;
+ case PROGRAM_END: mach_program_end(); break;
+ }
+
+ return status;
+}
+
+
+/* Parses one line of 0 terminated G-Code.
+ *
+ * All the parser does is load the state values in gn (next model
+ * state) and set flags in gf (model state flags). The execute
+ * routine applies them. The buffer is assumed to contain only
+ * uppercase characters and signed floats (no whitespace).
+ *
+ * A number of implicit things happen when the gn struct is zeroed:
+ * - inverse feed rate mode is canceled - set back to units_per_minute mode
+ */
+static stat_t _parse_gcode_block(char *buf) {
+ char *pstr = buf; // persistent pointer 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)
+ stat_t status = STAT_OK;
+
+ // set initial state for new move
+ memset(modals, 0, sizeof(modals)); // clear all parser values
+ memset(&parser.gf, 0, sizeof(gcode_flags_t)); // clear all next-state flags
+ memset(&parser.gn, 0, sizeof(gcode_state_t)); // clear all next-state values
+
+ // get motion mode from previous block
+ parser.gn.motion_mode = mach_get_motion_mode();
+
+ // extract commands and parameters
+ while ((status = _get_next_gcode_word(&pstr, &letter, &value)) == STAT_OK) {
+ switch (letter) {
+ case 'G':
+ switch ((uint8_t)value) {
+ case 0:
+ SET_MODAL(MODAL_GROUP_G1, motion_mode, MOTION_MODE_RAPID);
+ case 1:
+ SET_MODAL(MODAL_GROUP_G1, motion_mode, MOTION_MODE_FEED);
+ case 2: SET_MODAL(MODAL_GROUP_G1, motion_mode, MOTION_MODE_CW_ARC);
+ case 3: SET_MODAL(MODAL_GROUP_G1, motion_mode, MOTION_MODE_CCW_ARC);
+ case 4: SET_NON_MODAL(next_action, NEXT_ACTION_DWELL);
+ case 10:
+ SET_MODAL(MODAL_GROUP_G0, next_action, NEXT_ACTION_SET_COORD_DATA);
+ case 17: SET_MODAL(MODAL_GROUP_G2, plane, PLANE_XY);
+ case 18: SET_MODAL(MODAL_GROUP_G2, plane, PLANE_XZ);
+ case 19: SET_MODAL(MODAL_GROUP_G2, plane, PLANE_YZ);
+ case 20: SET_MODAL(MODAL_GROUP_G6, units, INCHES);
+ case 21: SET_MODAL(MODAL_GROUP_G6, units, MILLIMETERS);
+ case 28:
+ switch (_point(value)) {
+ case 0:
+ SET_MODAL(MODAL_GROUP_G0, next_action, NEXT_ACTION_GOTO_G28_POSITION);
+ case 1:
+ SET_MODAL(MODAL_GROUP_G0, next_action, NEXT_ACTION_SET_G28_POSITION);
+ case 2: SET_NON_MODAL(next_action, NEXT_ACTION_SEARCH_HOME);
+ case 3: SET_NON_MODAL(next_action, NEXT_ACTION_SET_ABSOLUTE_ORIGIN);
+ case 4: SET_NON_MODAL(next_action, NEXT_ACTION_HOMING_NO_SET);
+ default: status = STAT_GCODE_COMMAND_UNSUPPORTED;
+ }
+ break;
+
+ case 30:
+ switch (_point(value)) {
+ case 0:
+ SET_MODAL(MODAL_GROUP_G0, next_action, NEXT_ACTION_GOTO_G30_POSITION);
+ case 1:
+ SET_MODAL(MODAL_GROUP_G0, next_action, NEXT_ACTION_SET_G30_POSITION);
+ default: status = STAT_GCODE_COMMAND_UNSUPPORTED;
+ }
+ break;
+
+ case 38:
+ switch (_point(value)) {
+ case 2: SET_NON_MODAL(next_action, NEXT_ACTION_STRAIGHT_PROBE);
+ default: status = STAT_GCODE_COMMAND_UNSUPPORTED;
+ }
+ break;
+
+ case 40: break; // ignore cancel cutter radius compensation
+ case 49: break; // ignore cancel tool length offset comp.
+ case 53: SET_NON_MODAL(absolute_mode, true);
+ 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);
+ case 57: SET_MODAL(MODAL_GROUP_G12, coord_system, G57);
+ case 58: SET_MODAL(MODAL_GROUP_G12, coord_system, G58);
+ case 59: SET_MODAL(MODAL_GROUP_G12, coord_system, G59);
+ case 61:
+ switch (_point(value)) {
+ case 0: SET_MODAL(MODAL_GROUP_G13, path_mode, PATH_EXACT_PATH);
+ case 1: SET_MODAL(MODAL_GROUP_G13, path_mode, PATH_EXACT_STOP);
+ default: status = STAT_GCODE_COMMAND_UNSUPPORTED;
+ }
+ break;
+
+ case 64: SET_MODAL(MODAL_GROUP_G13,path_mode, PATH_CONTINUOUS);
+ case 80: SET_MODAL(MODAL_GROUP_G1, motion_mode,
+ MOTION_MODE_CANCEL_MOTION_MODE);
+ // case 90: SET_MODAL(MODAL_GROUP_G3, distance_mode, ABSOLUTE_MODE);
+ // case 91: SET_MODAL(MODAL_GROUP_G3, distance_mode, INCREMENTAL_MODE);
+ case 90:
+ switch (_point(value)) {
+ case 0: SET_MODAL(MODAL_GROUP_G3, distance_mode, ABSOLUTE_MODE);
+ case 1: SET_MODAL(MODAL_GROUP_G3, arc_distance_mode, ABSOLUTE_MODE);
+ default: status = STAT_GCODE_COMMAND_UNSUPPORTED;
+ }
+ break;
+
+ case 91:
+ switch (_point(value)) {
+ case 0: SET_MODAL(MODAL_GROUP_G3, distance_mode, INCREMENTAL_MODE);
+ case 1: SET_MODAL(MODAL_GROUP_G3, arc_distance_mode, INCREMENTAL_MODE);
+ default: status = STAT_GCODE_COMMAND_UNSUPPORTED;
+ }
+ break;
+
+ 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);
+ default: status = STAT_GCODE_COMMAND_UNSUPPORTED;
+ }
+ break;
+
+ case 93: SET_MODAL(MODAL_GROUP_G5, feed_mode, INVERSE_TIME_MODE);
+ case 94: SET_MODAL(MODAL_GROUP_G5, feed_mode, UNITS_PER_MINUTE_MODE);
+ // case 95:
+ // SET_MODAL(MODAL_GROUP_G5, feed_mode, UNITS_PER_REVOLUTION_MODE);
+ // case 96: // Spindle Constant Surface Speed (not currently supported)
+ case 97: break; // Spindle RPM mode (only mode curently supported)
+ default: status = STAT_GCODE_COMMAND_UNSUPPORTED;
+ }
+ break;
+
+ case 'M':
+ switch ((uint8_t)value) {
+ case 0:
+ SET_MODAL(MODAL_GROUP_M4, program_flow, PROGRAM_STOP);
+ case 1:
+ SET_MODAL(MODAL_GROUP_M4, program_flow, PROGRAM_OPTIONAL_STOP);
+ case 60:
+ SET_MODAL(MODAL_GROUP_M4, program_flow, PROGRAM_PALLET_CHANGE_STOP);
+ case 2: case 30:
+ SET_MODAL(MODAL_GROUP_M4, program_flow, PROGRAM_END);
+ case 3: SET_MODAL(MODAL_GROUP_M7, spindle_mode, SPINDLE_CW);
+ case 4: SET_MODAL(MODAL_GROUP_M7, spindle_mode, SPINDLE_CCW);
+ case 5: SET_MODAL(MODAL_GROUP_M7, spindle_mode, 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); // Also mist
+ case 48: SET_MODAL(MODAL_GROUP_M9, override_enables, true);
+ case 49: SET_MODAL(MODAL_GROUP_M9, override_enables, false);
+ case 50: SET_MODAL(MODAL_GROUP_M9, feed_override_enable, true);
+ case 51: SET_MODAL(MODAL_GROUP_M9, spindle_override_enable, true);
+ default: status = STAT_MCODE_COMMAND_UNSUPPORTED;
+ }
+ break;
+
+ case 'T': SET_NON_MODAL(tool, (uint8_t)trunc(value));
+ case 'F': SET_NON_MODAL(feed_rate, value);
+ // used for dwell time, G10 coord select, rotations
+ case 'P': SET_NON_MODAL(parameter, value);
+ case 'S': SET_NON_MODAL(spindle_speed, value);
+ case 'X': SET_NON_MODAL(target[AXIS_X], value);
+ case 'Y': SET_NON_MODAL(target[AXIS_Y], value);
+ case 'Z': SET_NON_MODAL(target[AXIS_Z], value);
+ 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 '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 'R': SET_NON_MODAL(arc_radius, value);
+ case 'N': SET_NON_MODAL(line, (uint32_t)value); // line number
+ case 'L': break; // not used for anything
+ case 0: break;
+ default: status = STAT_GCODE_COMMAND_UNSUPPORTED;
+ }
+
+ if (status != STAT_OK) break;
+ }
+
+ if (status != STAT_OK && status != STAT_COMPLETE) return status;
+ RITORNO(_validate_gcode_block());
+
+ return _execute_gcode_block(); // if successful execute the block
+}
+
+
+/// Parse a block (line) of gcode
+/// Top level of gcode parser. Normalizes block and looks for special cases
+stat_t gc_gcode_parser(char *block) {
+ char *str = block; // gcode command or 0 string
+ char none = 0;
+ char *com = &none; // gcode comment or 0 string
+ char *msg = &none; // gcode message or 0 string
+ uint8_t block_delete_flag;
+
+ _normalize_gcode_block(str, &com, &msg, &block_delete_flag);
+
+ // Block delete omits the line if a / char is present in the first space
+ // For now this is unconditional and will always delete
+ if (block_delete_flag) return STAT_NOOP;
+
+ // queue a "(MSG" response
+ if (*msg) mach_message(msg); // queue the message
+
+ return _parse_gcode_block(block);
+}
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2016 Buildbotics LLC
+ Copyright (c) 2010 - 2015 Alden S. Hart, Jr.
+ All rights reserved.
+
+ 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/>.
+
+ The software is distributed in the hope that it will be useful, but
+ WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public
+ License along with the software. If not, see
+ <http://www.gnu.org/licenses/>.
+
+ For information regarding this software email:
+ "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#pragma once
+
+
+#include "status.h"
+#include "machine.h"
+
+
+typedef enum { // Used for detecting gcode errors. See NIST section 3.4
+ MODAL_GROUP_G0, // {G10,G28,G28.1,G92} non-modal axis commands
+ MODAL_GROUP_G1, // {G0,G1,G2,G3,G80} motion
+ MODAL_GROUP_G2, // {G17,G18,G19} plane selection
+ MODAL_GROUP_G3, // {G90,G91} distance mode
+ MODAL_GROUP_G5, // {G93,G94} feed rate mode
+ MODAL_GROUP_G6, // {G20,G21} units
+ MODAL_GROUP_G7, // {G40,G41,G42} cutter radius compensation
+ MODAL_GROUP_G8, // {G43,G49} tool length offset
+ MODAL_GROUP_G9, // {G98,G99} return mode in canned cycles
+ MODAL_GROUP_G12, // {G54,G55,G56,G57,G58,G59} coordinate system selection
+ MODAL_GROUP_G13, // {G61,G61.1,G64} path control mode
+ MODAL_GROUP_M4, // {M0,M1,M2,M30,M60} stopping
+ MODAL_GROUP_M6, // {M6} tool change
+ MODAL_GROUP_M7, // {M3,M4,M5} spindle turning
+ MODAL_GROUP_M8, // {M7,M8,M9} coolant
+ MODAL_GROUP_M9, // {M48,M49} speed/feed override switches
+} modal_group_t;
+
+#define MODAL_GROUP_COUNT (MODAL_GROUP_M9 + 1)
+
+
+typedef struct {
+ gcode_state_t gn; // gcode input values
+ gcode_flags_t gf; // gcode input flags
+} parser_t;
+
+
+extern parser_t parser;
+
+
+stat_t gc_gcode_parser(char *block);
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2016 Buildbotics LLC
+ All rights reserved.
+
+ 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/>.
+
+ The software is distributed in the hope that it will be useful, but
+ WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public
+ License along with the software. If not, see
+ <http://www.gnu.org/licenses/>.
+
+ For information regarding this software email:
+ "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#include "gcode_state.h"
+
+
+PGM_P gs_get_units_pgmstr(units_t mode) {
+ switch (mode) {
+ case INCHES: return PSTR("IN");
+ case MILLIMETERS: return PSTR("MM");
+ case DEGREES: return PSTR("DEG");
+ }
+
+ return PSTR("INVALID");
+}
+
+
+PGM_P gs_get_feed_mode_pgmstr(feed_mode_t mode) {
+ switch (mode) {
+ case INVERSE_TIME_MODE: return PSTR("INVERSE TIME");
+ case UNITS_PER_MINUTE_MODE: return PSTR("PER MIN");
+ case UNITS_PER_REVOLUTION_MODE: return PSTR("PER REV");
+ }
+
+ return PSTR("INVALID");
+}
+
+
+PGM_P gs_get_plane_pgmstr(plane_t plane) {
+ switch (plane) {
+ case PLANE_XY: return PSTR("XY");
+ case PLANE_XZ: return PSTR("XZ");
+ case PLANE_YZ: return PSTR("YZ");
+ }
+
+ return PSTR("INVALID");
+}
+
+
+PGM_P gs_get_coord_system_pgmstr(coord_system_t cs) {
+ switch (cs) {
+ case ABSOLUTE_COORDS: return PSTR("ABS");
+ case G54: return PSTR("G54");
+ case G55: return PSTR("G55");
+ case G56: return PSTR("G56");
+ case G57: return PSTR("G57");
+ case G58: return PSTR("G58");
+ case G59: return PSTR("G59");
+ }
+
+ return PSTR("INVALID");
+}
+
+
+PGM_P gs_get_path_mode_pgmstr(path_mode_t mode) {
+ switch (mode) {
+ case PATH_EXACT_PATH: return PSTR("EXACT PATH");
+ case PATH_EXACT_STOP: return PSTR("EXACT STOP");
+ case PATH_CONTINUOUS: return PSTR("CONTINUOUS");
+ }
+
+ return PSTR("INVALID");
+}
+
+
+PGM_P gs_get_distance_mode_pgmstr(distance_mode_t mode) {
+ switch (mode) {
+ case ABSOLUTE_MODE: return PSTR("ABSOLUTE");
+ case INCREMENTAL_MODE: return PSTR("INCREMENTAL");
+ }
+
+ return PSTR("INVALID");
+}
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2016 Buildbotics LLC
+ Copyright (c) 2010 - 2015 Alden S. Hart, Jr.
+ All rights reserved.
+
+ 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/>.
+
+ The software is distributed in the hope that it will be useful, but
+ WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public
+ License along with the software. If not, see
+ <http://www.gnu.org/licenses/>.
+
+ For information regarding this software email:
+ "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+MEMBER(uint32_t, line) // Gcode block line number
+
+MEMBER(uint8_t, tool) // T - sets this value
+
+MEMBER(float, feed_rate) // F - mm/min or inverse time mode
+MEMBER(feed_mode_t, feed_mode)
+MEMBER(float, feed_override) // 1.0000 x F feed rate
+MEMBER(bool, feed_override_enable) // M48, M49
+
+MEMBER(float, spindle_speed) // in RPM
+MEMBER(spindle_mode_t, spindle_mode)
+MEMBER(float, spindle_override) // 1.0000 x S spindle speed
+MEMBER(bool, spindle_override_enable) // true = override enabled
+
+MEMBER(motion_mode_t, motion_mode) // Group 1 modal motion
+MEMBER(plane_t, plane) // G17, G18, G19
+MEMBER(units_t, units) // G20, G21
+MEMBER(coord_system_t, coord_system) // G54-G59 - select coord system 1-9
+MEMBER(bool, absolute_mode) // G53 move in machine coordinates
+MEMBER(path_mode_t, path_mode) // G61
+MEMBER(distance_mode_t, distance_mode) // G91
+MEMBER(distance_mode_t, arc_distance_mode) // G91.1
+
+MEMBER(bool, mist_coolant) // mist on (M7), off (M9)
+MEMBER(bool, flood_coolant) // mist on (M8), off (M9)
+
+MEMBER(next_action_t, next_action) // G group 1 moves & non-modals
+MEMBER(program_flow_t, program_flow) // used only by the gcode_parser
+
+// TODO unimplemented gcode parameters
+// MEMBER(float cutter_radius) // D - cutter radius compensation (0 is off)
+// MEMBER(float cutter_length) // H - cutter length compensation (0 is off)
+
+// Used for input only
+MEMBER(float, target[AXES]) // XYZABC where the move should go
+MEMBER(bool, override_enables) // feed and spindle enable
+MEMBER(bool, tool_change) // M6 tool change flag
+
+MEMBER(float, parameter) // P - dwell & G10 coord select
+MEMBER(float, arc_radius) // R - in arc radius mode
+MEMBER(float, arc_offset[3]) // IJK - used by arc commands
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2016 Buildbotics LLC
+ Copyright (c) 2010 - 2015 Alden S. Hart, Jr.
+ All rights reserved.
+
+ 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/>.
+
+ The software is distributed in the hope that it will be useful, but
+ WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public
+ License along with the software. If not, see
+ <http://www.gnu.org/licenses/>.
+
+ For information regarding this software email:
+ "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#pragma once
+
+#include "config.h"
+
+#include <avr/pgmspace.h>
+
+#include <stdint.h>
+#include <stdbool.h>
+
+
+/* The difference between next_action_t and motion_mode_t is that
+ * next_action_t is used by the current block, and may carry non-modal
+ * commands, whereas motion_mode_t persists across blocks as G modal group 1
+ */
+
+/// these are in order to optimized CASE statement
+typedef enum {
+ NEXT_ACTION_DEFAULT, // Must be zero (invokes motion modes)
+ NEXT_ACTION_SEARCH_HOME, // G28.2 homing cycle
+ NEXT_ACTION_SET_ABSOLUTE_ORIGIN, // G28.3 origin set
+ NEXT_ACTION_HOMING_NO_SET, // G28.4 homing cycle no coord setting
+ NEXT_ACTION_SET_G28_POSITION, // G28.1 set position in abs coordinates
+ NEXT_ACTION_GOTO_G28_POSITION, // G28 go to machine position
+ NEXT_ACTION_SET_G30_POSITION, // G30.1
+ NEXT_ACTION_GOTO_G30_POSITION, // G30
+ NEXT_ACTION_SET_COORD_DATA, // G10
+ 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_DWELL, // G4
+ NEXT_ACTION_STRAIGHT_PROBE, // G38.2
+} next_action_t;
+
+
+typedef enum { // G Modal Group 1
+ MOTION_MODE_RAPID, // G0 - rapid
+ MOTION_MODE_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
+} motion_mode_t;
+
+
+typedef enum { // plane - translates to:
+ // axis_0 axis_1 axis_2
+ PLANE_XY, // G17 X Y Z
+ PLANE_XZ, // G18 X Z Y
+ PLANE_YZ, // G19 Y Z X
+} plane_t;
+
+
+typedef enum {
+ INCHES, // G20
+ MILLIMETERS, // G21
+ DEGREES, // ABC axes (this value used for displays only)
+} units_t;
+
+
+typedef enum {
+ ABSOLUTE_COORDS, // machine coordinate system
+ G54, G55, G56, G57, G58, G59,
+} coord_system_t;
+
+
+/// G Modal Group 13
+typedef enum {
+ PATH_EXACT_PATH, // G61 hits corners but stops only if needed
+ PATH_EXACT_STOP, // G61.1 stops at all corners
+ PATH_CONTINUOUS, // G64 and typically the default mode
+} path_mode_t;
+
+
+typedef enum {
+ ABSOLUTE_MODE, // G90
+ INCREMENTAL_MODE, // G91
+} distance_mode_t;
+
+
+typedef enum {
+ INVERSE_TIME_MODE, // G93
+ UNITS_PER_MINUTE_MODE, // G94
+ UNITS_PER_REVOLUTION_MODE, // G95 (unimplemented)
+} feed_mode_t;
+
+
+typedef enum {
+ ORIGIN_OFFSET_SET, // G92 - set origin offsets
+ ORIGIN_OFFSET_CANCEL, // G92.1 - zero out origin offsets
+ ORIGIN_OFFSET_SUSPEND, // G92.2 - do not apply offsets, but preserve values
+ ORIGIN_OFFSET_RESUME, // G92.3 - resume application of the suspended offsets
+} origin_offset_t;
+
+
+typedef enum {
+ PROGRAM_STOP,
+ PROGRAM_OPTIONAL_STOP,
+ PROGRAM_PALLET_CHANGE_STOP,
+ PROGRAM_END,
+} program_flow_t;
+
+
+/// spindle state settings
+typedef enum {
+ SPINDLE_OFF,
+ SPINDLE_CW,
+ SPINDLE_CCW,
+} spindle_mode_t;
+
+
+/// mist and flood coolant states
+typedef enum {
+ COOLANT_OFF, // all coolant off
+ COOLANT_ON, // request coolant on or indicate both coolants are on
+ COOLANT_MIST, // indicates mist coolant on
+ COOLANT_FLOOD, // indicates flood coolant on
+} coolant_state_t;
+
+
+/// used for spindle and arc dir
+typedef enum {
+ DIRECTION_CW,
+ DIRECTION_CCW,
+} direction_t;
+
+
+/* Gcode model
+ *
+ * - mach.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 machine layer and
+ * should be accessed only through mach_*() routines.
+ *
+ * - parser.gn is used by the gcode parser 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.
+ *
+ * - parser.gf is used by the gcode parser to hold flags for any data that has
+ * changed in gn during the parse.
+ */
+
+
+/// Gcode model state
+typedef struct {
+#define MEMBER(TYPE, VAR) TYPE VAR;
+#include "gcode_state.def"
+#undef MEMBER
+} gcode_state_t;
+
+
+typedef struct {
+#define MEMBER(TYPE, VAR) bool VAR;
+#include "gcode_state.def"
+#undef MEMBER
+} gcode_flags_t;
+
+
+PGM_P gs_get_units_pgmstr(units_t mode);
+PGM_P gs_get_feed_mode_pgmstr(feed_mode_t mode);
+PGM_P gs_get_plane_pgmstr(plane_t plane);
+PGM_P gs_get_coord_system_pgmstr(coord_system_t cs);
+PGM_P gs_get_path_mode_pgmstr(path_mode_t mode);
+PGM_P gs_get_distance_mode_pgmstr(distance_mode_t mode);
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2016 Buildbotics LLC
+ Copyright (c) 2010 - 2015 Alden S. Hart, Jr.
+ All rights reserved.
+
+ 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/>.
+
+ The software is distributed in the hope that it will be useful, but
+ WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public
+ License along with the software. If not, see
+ <http://www.gnu.org/licenses/>.
+
+ For information regarding this software email:
+ "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#include "hardware.h"
+#include "rtc.h"
+#include "usart.h"
+#include "huanyang.h"
+#include "config.h"
+
+#include <avr/interrupt.h>
+#include <avr/pgmspace.h>
+#include <avr/eeprom.h>
+#include <avr/wdt.h>
+
+#include <stdbool.h>
+#include <stddef.h>
+
+
+typedef struct {
+ char id[26];
+ bool hard_reset; // flag to perform a hard reset
+ bool bootloader; // flag to enter the bootloader
+} hw_t;
+
+static hw_t hw = {{0}};
+
+
+#define PROD_SIGS (*(NVM_PROD_SIGNATURES_t *)0x0000)
+#define HEXNIB(x) "0123456789abcdef"[(x) & 0xf]
+
+
+/// This routine is lifted and modified from Boston Android and from
+/// http://www.avrfreaks.net/index.php?name=PNphpBB2&file=viewtopic&p=711659
+static void _init_clock() {
+#if defined(__CLOCK_EXTERNAL_8MHZ) // external 8 Mhx Xtal w/ 4x PLL = 32 Mhz
+ // 2-9 MHz crystal; 0.4-16 MHz XTAL w/ 16K CLK startup
+ OSC.XOSCCTRL = OSC_FRQRANGE_2TO9_gc | OSC_XOSCSEL_XTAL_16KCLK_gc;
+ OSC.CTRL = OSC_XOSCEN_bm; // enable external crystal oscillator
+ while (!(OSC.STATUS & OSC_XOSCRDY_bm)); // wait for oscillator ready
+
+ OSC.PLLCTRL = OSC_PLLSRC_XOSC_gc | 4; // PLL source, 4x (32 MHz sys clock)
+ OSC.CTRL = OSC_PLLEN_bm | OSC_XOSCEN_bm; // Enable PLL & External Oscillator
+ while (!(OSC.STATUS & OSC_PLLRDY_bm)); // wait for PLL ready
+
+ CCP = CCP_IOREG_gc;
+ CLK.CTRL = CLK_SCLKSEL_PLL_gc; // switch to PLL clock
+
+ OSC.CTRL &= ~OSC_RC2MEN_bm; // disable internal 2 MHz clock
+
+#elif defined(__CLOCK_EXTERNAL_16MHZ) // external 16Mhz Xtal w/ 2x PLL = 32 Mhz
+ // 12-16 MHz crystal; 0.4-16 MHz XTAL w/ 16K CLK startup
+ OSC.XOSCCTRL = OSC_FRQRANGE_12TO16_gc | OSC_XOSCSEL_XTAL_16KCLK_gc;
+ OSC.CTRL = OSC_XOSCEN_bm; // enable external crystal oscillator
+ while (!(OSC.STATUS & OSC_XOSCRDY_bm)); // wait for oscillator ready
+
+ OSC.PLLCTRL = OSC_PLLSRC_XOSC_gc | 2; // PLL source, 2x (32 MHz sys clock)
+ OSC.CTRL = OSC_PLLEN_bm | OSC_XOSCEN_bm; // Enable PLL & External Oscillator
+ while (!(OSC.STATUS & OSC_PLLRDY_bm)); // wait for PLL ready
+
+ CCP = CCP_IOREG_gc;
+ CLK.CTRL = CLK_SCLKSEL_PLL_gc; // switch to PLL clock
+
+ OSC.CTRL &= ~OSC_RC2MEN_bm; // disable internal 2 MHz clock
+
+#elif defined(__CLOCK_INTERNAL_32MHZ) // 32 MHz internal clock
+ OSC.CTRL = OSC_RC32MEN_bm; // enable internal 32MHz oscillator
+ while (!(OSC.STATUS & OSC_RC32MRDY_bm)); // wait for oscillator ready
+
+ CCP = CCP_IOREG_gc; // Security Signature to modify clk
+ CLK.CTRL = CLK_SCLKSEL_RC32M_gc; // select sysclock 32MHz osc
+
+#else
+#error No clock defined
+#endif
+}
+
+
+static void _load_hw_id_byte(int i, register8_t *reg) {
+ NVM.CMD = NVM_CMD_READ_CALIB_ROW_gc;
+ uint8_t byte = pgm_read_byte(reg);
+ NVM.CMD = NVM_CMD_NO_OPERATION_gc;
+
+ hw.id[i] = HEXNIB(byte >> 4);
+ hw.id[i + 1] = HEXNIB(byte);
+}
+
+
+static void _read_hw_id() {
+ int i = 0;
+ _load_hw_id_byte(i, &PROD_SIGS.LOTNUM5); i += 2;
+ _load_hw_id_byte(i, &PROD_SIGS.LOTNUM4); i += 2;
+ _load_hw_id_byte(i, &PROD_SIGS.LOTNUM3); i += 2;
+ _load_hw_id_byte(i, &PROD_SIGS.LOTNUM2); i += 2;
+ _load_hw_id_byte(i, &PROD_SIGS.LOTNUM1); i += 2;
+ _load_hw_id_byte(i, &PROD_SIGS.LOTNUM0); i += 2;
+ hw.id[i++] = '-';
+ _load_hw_id_byte(i, &PROD_SIGS.WAFNUM); i += 2;
+ hw.id[i++] = '-';
+ _load_hw_id_byte(i, &PROD_SIGS.COORDX1); i += 2;
+ _load_hw_id_byte(i, &PROD_SIGS.COORDX0); i += 2;
+ hw.id[i++] = '-';
+ _load_hw_id_byte(i, &PROD_SIGS.COORDY1); i += 2;
+ _load_hw_id_byte(i, &PROD_SIGS.COORDY0); i += 2;
+ hw.id[i] = 0;
+}
+
+
+/// Lowest level hardware init
+void hardware_init() {
+ _init_clock(); // set system clock
+ rtc_init(); // real time counter
+ _read_hw_id();
+
+ // Round-robin, interrupts in application section, all interupts levels
+ CCP = CCP_IOREG_gc;
+ PMIC.CTRL =
+ PMIC_RREN_bm | PMIC_HILVLEN_bm | PMIC_MEDLVLEN_bm | PMIC_LOLVLEN_bm;
+}
+
+
+void hw_request_hard_reset() {hw.hard_reset = true;}
+
+
+/// Hard reset using watchdog timer
+/// software hard reset using the watchdog timer
+void hw_hard_reset() {
+ usart_flush();
+ cli();
+ CCP = CCP_IOREG_gc;
+ RST.CTRL = RST_SWRST_bm;
+}
+
+
+/// Controller's rest handler
+void hw_reset_handler() {
+ if (hw.hard_reset) {
+ while (huanyang_stopping() || !usart_tx_empty() || !eeprom_is_ready())
+ continue;
+
+ hw_hard_reset();
+ }
+
+ if (hw.bootloader) {
+ // TODO enable bootloader interrupt vectors and jump to BOOT_SECTION_START
+ hw.bootloader = false;
+ }
+}
+
+
+/// Executes a software reset using CCPWrite
+void hw_request_bootloader() {hw.bootloader = true;}
+
+
+uint8_t hw_disable_watchdog() {
+ uint8_t state = WDT.CTRL;
+ wdt_disable();
+ return state;
+}
+
+
+void hw_restore_watchdog(uint8_t state) {
+ cli();
+ CCP = CCP_IOREG_gc;
+ WDT.CTRL = state | WDT_CEN_bm;
+ sei();
+}
+
+
+const char *get_hw_id() {
+ return hw.id;
+}
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2016 Buildbotics LLC
+ Copyright (c) 2010 - 2015 Alden S. Hart, Jr.
+ Copyright (c) 2012 - 2015 Rob Giseburt
+ All rights reserved.
+
+ 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/>.
+
+ The software is distributed in the hope that it will be useful, but
+ WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public
+ License along with the software. If not, see
+ <http://www.gnu.org/licenses/>.
+
+ For information regarding this software email:
+ "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#pragma once
+
+#include "status.h"
+
+#include <stdint.h>
+
+
+void hardware_init();
+void hw_request_hard_reset();
+void hw_hard_reset();
+void hw_reset_handler();
+
+void hw_request_bootloader();
+
+uint8_t hw_disable_watchdog();
+void hw_restore_watchdog(uint8_t state);
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2016 Buildbotics LLC
+ All rights reserved.
+
+ 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/>.
+
+ The software is distributed in the hope that it will be useful, but
+ WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public
+ License along with the software. If not, see
+ <http://www.gnu.org/licenses/>.
+
+ For information regarding this software email:
+ "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#include "home.h"
+
+#include "plan/runtime.h"
+#include "plan/line.h"
+#include "plan/state.h"
+#include "plan/exec.h"
+#include "axis.h"
+#include "motor.h"
+#include "util.h"
+#include "config.h"
+
+#include <stdint.h>
+#include <string.h>
+
+
+typedef enum {
+ STATE_INIT,
+ STATE_HOMING,
+ STATE_STALLED,
+ STATE_BACKOFF,
+ STATE_DONE,
+} home_state_t;
+
+
+typedef struct {
+ bool homed[AXES];
+ unsigned axis;
+ home_state_t state;
+ float velocity;
+ uint16_t microsteps;
+} home_t;
+
+static home_t home;
+
+
+void _stall_callback(int motor) {
+ if (home.velocity == mp_runtime_get_velocity()) {
+ mp_exec_abort();
+ home.state = STATE_STALLED;
+ }
+}
+
+
+void _move_axis(float travel) {
+ float target[AXES];
+ float *position = mp_runtime_get_position();
+ copy_vector(target, position);
+ target[home.axis] += travel;
+ mp_aline(target, false, false, false, home.velocity, 1, -1);
+}
+
+
+void home_callback() {
+ if (mp_get_cycle() != CYCLE_HOMING || !mp_is_quiescent() ||
+ !mp_queue_is_empty()) return;
+
+ while (true) {
+ int motor = axis_get_motor(home.axis);
+ homing_mode_t mode = axis_get_homing_mode(home.axis);
+ int direction =
+ (mode == HOMING_STALL_MIN || mode == HOMING_SWITCH_MIN) ? -1 : 1;
+ float min = axis_get_travel_min(home.axis);
+ float max = axis_get_travel_max(home.axis);
+
+ switch (home.state) {
+ case STATE_INIT: {
+ if (motor == -1 || mode == HOMING_DISABLED) {
+ home.state = STATE_DONE;
+ break;
+ }
+
+ STATUS_INFO("Homing %c axis", axis_get_char(home.axis));
+
+ // Set axis not homed
+ home.homed[home.axis] = false;
+
+ // Determine homing type: switch or stall
+
+ // Configure driver, set stall callback and compute homing velocity
+ home.microsteps = motor_get_microsteps(motor);
+ motor_set_microsteps(motor, 8);
+ motor_set_stall_callback(motor, _stall_callback);
+ //home.velocity = axis_get_stall_homing_velocity(home.axis);
+ home.velocity = axis_get_search_velocity(home.axis);
+
+ // Move in home direction
+ float travel = max - min; // TODO consider ramping distance
+ _move_axis(travel * direction);
+
+ home.state = STATE_HOMING;
+ return;
+ }
+
+ case STATE_HOMING:
+ case STATE_STALLED:
+ // Restore motor driver config
+ motor_set_microsteps(motor, home.microsteps);
+ motor_set_stall_callback(motor, 0);
+
+ if (home.state == STATE_HOMING) {
+ STATUS_ERROR(STAT_HOMING_CYCLE_FAILED, "Failed to find %c axis home",
+ axis_get_char(home.axis));
+ mp_set_cycle(CYCLE_MACHINING);
+
+ } else {
+ STATUS_INFO("Backing off %c axis", axis_get_char(home.axis));
+ mach_set_axis_position(home.axis, direction < 0 ? min : max);
+ _move_axis(axis_get_zero_backoff(home.axis) * direction * -1);
+ home.state = STATE_BACKOFF;
+ }
+ return;
+
+ case STATE_BACKOFF:
+ STATUS_INFO("Homed %c axis", axis_get_char(home.axis));
+
+ // Set axis position & homed
+ mach_set_axis_position(home.axis, min);
+ home.homed[home.axis] = true;
+ home.state = STATE_DONE;
+ break;
+
+ case STATE_DONE:
+ if (home.axis == AXIS_X) {
+ // Done
+ mp_set_cycle(CYCLE_MACHINING);
+ return;
+ }
+
+ // Next axis
+ home.axis--;
+ home.state = STATE_INIT;
+ break;
+ }
+ }
+}
+
+
+uint8_t command_home(int argc, char *argv[]) {
+ if (mp_get_cycle() != CYCLE_MACHINING || mp_get_state() != STATE_READY)
+ return 0;
+
+ // Init
+ memset(&home, 0, sizeof(home));
+ home.axis = AXIS_Z;
+
+ mp_set_cycle(CYCLE_HOMING);
+
+ return 0;
+}
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2016 Buildbotics LLC
+ All rights reserved.
+
+ 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/>.
+
+ The software is distributed in the hope that it will be useful, but
+ WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public
+ License along with the software. If not, see
+ <http://www.gnu.org/licenses/>.
+
+ For information regarding this software email:
+ "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#pragma once
+
+void home_init();
+void home_callback();
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2016 Buildbotics LLC
+ Copyright (c) 2010 - 2015 Alden S. Hart, Jr.
+ All rights reserved.
+
+ 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/>.
+
+ The software is distributed in the hope that it will be useful, but
+ WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public
+ License along with the software. If not, see
+ <http://www.gnu.org/licenses/>.
+
+ For information regarding this software email:
+ "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#include "homing.h"
+
+#include "axis.h"
+#include "machine.h"
+#include "switch.h"
+#include "gcode_parser.h"
+#include "report.h"
+
+#include "plan/planner.h"
+#include "plan/runtime.h"
+#include "plan/state.h"
+
+
+typedef void (*homing_func_t)(int8_t axis);
+
+static void _homing_axis_start(int8_t axis);
+
+
+typedef enum {
+ HOMING_NOT_HOMED, // machine is not homed
+ HOMING_HOMED, // machine is homed
+ HOMING_WAITING, // machine waiting to be homed
+} homing_state_t;
+
+
+/// persistent homing runtime variables
+typedef struct {
+ homing_state_t state; // homing cycle sub-state machine
+ bool homed[AXES]; // individual axis homing flags
+
+ // controls for homing cycle
+ int8_t axis; // axis currently being homed
+
+ /// homing switch for current axis (index into switch flag table)
+ int8_t homing_switch;
+ int8_t limit_switch; // limit switch for current axis or -1 if none
+
+ uint8_t homing_closed; // 0=open, 1=closed
+ uint8_t limit_closed; // 0=open, 1=closed
+ /// G28.4 flag. true = set coords to zero at the end of homing cycle
+ uint8_t set_coordinates;
+ homing_func_t func; // binding for callback function state machine
+
+ // per-axis parameters
+ /// set to 1 for positive (max), -1 for negative (to min);
+ float direction;
+ float search_travel; // signed distance to travel in search
+ float search_velocity; // search speed as positive number
+ float latch_velocity; // latch speed as positive number
+ /// max distance to back off switch during latch phase
+ float latch_backoff;
+ /// distance to back off switch before setting zero
+ float zero_backoff;
+ /// maximum distance of switch clearing backoffs before erring out
+ float max_clear_backoff;
+
+ // state saved from gcode model
+ uint8_t saved_units; // G20,G21 global setting
+ uint8_t saved_coord_system; // G54 - G59 setting
+ uint8_t saved_distance_mode; // G90,G91 global setting
+ uint8_t saved_feed_mode; // G93,G94 global setting
+ float saved_feed_rate; // F setting
+ float saved_jerk; // saved and restored for each axis homed
+} homing_t;
+
+
+static homing_t hm = {0};
+
+
+// G28.2 homing cycle
+
+/* Homing works from a G28.2 according to the following writeup:
+ *
+ * https://github.com/synthetos
+ * /TinyG/wiki/Homing-and-Limits-Description-and-Operation
+ *
+ * How does this work?
+ *
+ * Homing is invoked using a G28.2 command with 1 or more axes specified in the
+ * command: e.g. g28.2 x0 y0 z0 (FYI: the number after each axis is irrelevant)
+ *
+ * Homing is always run in the following order - for each enabled axis:
+ * Z,X,Y,A Note: B and C cannot be homed
+ *
+ * At the start of a homing cycle those switches configured for homing
+ * (or for homing and limits) are treated as homing switches (they are modal).
+ *
+ * After initialization the following sequence is run for each axis to be homed:
+ *
+ * 0. If a homing or limit switch is closed on invocation, clear the switch
+ * 1. Drive towards the homing switch at search velocity until switch is hit
+ * 2. Drive away from the homing switch at latch velocity until switch opens
+ * 3. Back off switch by the zero backoff distance and set zero for that axis
+ *
+ * Homing works as a state machine that is driven by registering a callback
+ * function at hm.func() for the next state to be run. Once the axis is
+ * initialized each callback basically does two things (1) start the move
+ * for the current function, and (2) register the next state with hm.func().
+ * When a move is started it will either be interrupted if the homing switch
+ * changes state, This will cause the move to stop with a feedhold. The other
+ * thing that can happen is the move will run to its full length if no switch
+ * change is detected (hit or open),
+ *
+ * Once all moves for an axis are complete the next axis in the sequence is
+ * homed.
+ *
+ * When a homing cycle is initiated the homing state is set to HOMING_NOT_HOMED
+ * When homing completes successfully this is set to HOMING_HOMED, otherwise it
+ * remains HOMING_NOT_HOMED.
+ *
+ * Notes:
+ *
+ * 1. When coding a cycle (like this one) you get to perform one queued
+ * move per entry into the continuation, then you must exit.
+ *
+ * 2. When coding a cycle (like this one) you must wait until
+ * the last move has actually been queued (or has finished) before
+ * declaring the cycle to be done. Otherwise there is a nasty race
+ * condition that will accept the next command before the position of
+ * the final move has been recorded in the Gcode model.
+ */
+
+
+/*** Return next axis in sequence based on axis in arg
+ *
+ * Accepts "axis" arg as the current axis; or -1 to retrieve the first axis
+ * Returns next axis based on "axis" argument and if that axis is flagged for
+ * homing in the gf struct
+ * Returns -1 when all axes have been processed
+ * Returns -2 if no axes are specified (Gcode calling error)
+ * Homes Z first, then the rest in sequence
+ *
+ * Isolating this function facilitates implementing more complex and
+ * user-specified axis homing orders
+ */
+static int8_t _get_next_axis(int8_t axis) {
+ switch (axis) {
+ case -1: if (parser.gf.target[AXIS_Z]) return AXIS_Z;
+ case AXIS_Z: if (parser.gf.target[AXIS_X]) return AXIS_X;
+ case AXIS_X: if (parser.gf.target[AXIS_Y]) return AXIS_Y;
+ case AXIS_Y: if (parser.gf.target[AXIS_A]) return AXIS_A;
+ case AXIS_A: if (parser.gf.target[AXIS_B]) return AXIS_B;
+ case AXIS_B: if (parser.gf.target[AXIS_C]) return AXIS_C;
+ }
+
+ return axis == -1 ? -2 : -1; // error or done
+}
+
+
+/// Helper to finalize homing, third part of return to home
+static void _homing_finalize_exit() {
+ mp_flush_planner(); // should be stopped, but in case of switch closure
+
+ // Restore saved machine state
+ mach_set_coord_system(hm.saved_coord_system);
+ mach_set_units(hm.saved_units);
+ mach_set_distance_mode(hm.saved_distance_mode);
+ mach_set_feed_mode(hm.saved_feed_mode);
+ mach_set_feed_rate(hm.saved_feed_rate);
+ mach_set_motion_mode(MOTION_MODE_CANCEL_MOTION_MODE);
+
+ mp_set_cycle(CYCLE_MACHINING); // Default cycle
+}
+
+
+static void _homing_error_exit(stat_t status) {
+ _homing_finalize_exit();
+ status_error(status);
+}
+
+
+/// Execute moves
+static void _homing_axis_move(int8_t axis, float target, float velocity) {
+ float vect[AXES] = {0};
+ bool flags[AXES] = {0};
+
+ vect[axis] = target;
+ flags[axis] = true;
+ mach_set_feed_rate(velocity);
+ mp_flush_planner(); // don't use mp_request_flush() here
+
+ stat_t status = mach_feed(vect, flags);
+ if (status) _homing_error_exit(status);
+}
+
+
+/// End homing cycle in progress
+static void _homing_abort(int8_t axis) {
+ axis_set_jerk_max(axis, hm.saved_jerk); // restore the max jerk value
+
+ // homing state remains HOMING_NOT_HOMED
+ _homing_error_exit(STAT_HOMING_CYCLE_FAILED);
+
+ report_request();
+}
+
+
+/// set zero and finish up
+static void _homing_axis_set_zero(int8_t axis) {
+ if (hm.set_coordinates) {
+ mach_set_axis_position(axis, 0);
+ hm.homed[axis] = true;
+
+ } else // do not set axis if in G28.4 cycle
+ mach_set_axis_position(axis, mp_runtime_get_work_position(axis));
+
+ axis_set_jerk_max(axis, hm.saved_jerk); // restore the max jerk value
+
+ hm.func = _homing_axis_start;
+}
+
+
+/// backoff to zero position
+static void _homing_axis_zero_backoff(int8_t axis) {
+ _homing_axis_move(axis, hm.zero_backoff, hm.search_velocity);
+ hm.func = _homing_axis_set_zero;
+}
+
+
+/// Slow reverse until switch opens again
+static void _homing_axis_latch(int8_t axis) {
+ // verify assumption that we arrived here because of homing switch closure
+ // rather than user-initiated feedhold or other disruption
+ if (!switch_is_active(hm.homing_switch)) hm.func = _homing_abort;
+
+ else {
+ _homing_axis_move(axis, hm.latch_backoff, hm.latch_velocity);
+ hm.func = _homing_axis_zero_backoff;
+ }
+}
+
+
+/// Fast search for switch, closes switch
+static void _homing_axis_search(int8_t axis) {
+ // use the homing jerk for search onward
+ _homing_axis_move(axis, hm.search_travel, hm.search_velocity);
+ hm.func = _homing_axis_latch;
+}
+
+
+/// Initiate a clear to move off a switch that is thrown at the start
+static void _homing_axis_clear(int8_t axis) {
+ // Handle an initial switch closure by backing off the closed switch
+ // NOTE: Relies on independent switches per axis (not shared)
+
+ if (switch_is_active(hm.homing_switch))
+ _homing_axis_move(axis, hm.latch_backoff, hm.search_velocity);
+
+ else if (switch_is_active(hm.limit_switch))
+ _homing_axis_move(axis, -hm.latch_backoff, hm.search_velocity);
+
+ hm.func = _homing_axis_search;
+}
+
+
+/// Get next axis, initialize variables, call the clear
+static void _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
+ hm.state = HOMING_HOMED;
+ _homing_finalize_exit();
+ return;
+
+ } else if (axis == -2) // -2 is error
+ return _homing_error_exit(STAT_HOMING_ERROR_BAD_OR_NO_AXIS);
+ }
+
+ // clear the homed flag for axis to move w/o triggering soft limits
+ hm.homed[axis] = false;
+
+ // trap axis mis-configurations
+ if (fp_ZERO(axis_get_search_velocity(axis)))
+ return _homing_error_exit(STAT_HOMING_ERROR_ZERO_SEARCH_VELOCITY);
+ if (fp_ZERO(axis_get_latch_velocity(axis)))
+ return _homing_error_exit(STAT_HOMING_ERROR_ZERO_LATCH_VELOCITY);
+ if (axis_get_latch_backoff(axis) < 0)
+ return _homing_error_exit(STAT_HOMING_ERROR_NEGATIVE_LATCH_BACKOFF);
+
+ // calculate and test travel distance
+ float travel_distance =
+ fabs(axis_get_travel_max(axis) - axis_get_travel_min(axis)) +
+ axis_get_latch_backoff(axis);
+ if (fp_ZERO(travel_distance))
+ return _homing_error_exit(STAT_HOMING_ERROR_TRAVEL_MIN_MAX_IDENTICAL);
+
+ hm.axis = axis; // persist the axis
+ // search velocity is always positive
+ hm.search_velocity = fabs(axis_get_search_velocity(axis));
+ // latch velocity is always positive
+ hm.latch_velocity = fabs(axis_get_latch_velocity(axis));
+
+ // determine which switch to use
+ bool min_enabled = switch_is_enabled(MIN_SWITCH(axis));
+ bool max_enabled = switch_is_enabled(MAX_SWITCH(axis));
+
+ if (min_enabled) {
+ // setup parameters homing to the minimum switch
+ hm.homing_switch = MIN_SWITCH(axis); // min is the homing switch
+ hm.limit_switch = MAX_SWITCH(axis); // max would be limit switch
+ hm.search_travel = -travel_distance; // in negative direction
+ hm.latch_backoff = axis_get_latch_backoff(axis); // in positive direction
+ hm.zero_backoff = axis_get_zero_backoff(axis);
+
+ } else if (max_enabled) {
+ // setup parameters for positive travel (homing to the maximum switch)
+ hm.homing_switch = MAX_SWITCH(axis); // max is homing switch
+ hm.limit_switch = MIN_SWITCH(axis); // min would be limit switch
+ hm.search_travel = travel_distance; // in positive direction
+ hm.latch_backoff = -axis_get_latch_backoff(axis); // in negative direction
+ hm.zero_backoff = -axis_get_zero_backoff(axis);
+
+ } else {
+ // if homing is disabled for the axis then skip to the next axis
+ hm.limit_switch = -1; // disable the limit switch parameter
+ hm.func = _homing_axis_start;
+ return;
+ }
+
+ hm.saved_jerk = axis_get_jerk_max(axis); // save the max jerk value
+ hm.func = _homing_axis_clear; // start the clear
+}
+
+
+bool mach_is_homing() {
+ return mp_get_cycle() == CYCLE_HOMING;
+}
+
+
+void mach_set_not_homed() {
+ for (int axis = 0; axis < AXES; axis++)
+ mach_set_homed(axis, false);
+}
+
+
+bool mach_get_homed(int axis) {
+ return hm.homed[axis];
+}
+
+
+void mach_set_homed(int axis, bool homed) {
+ // TODO save homed to EEPROM
+ hm.homed[axis] = homed;
+}
+
+
+/// G28.2 homing cycle using limit switches
+void mach_homing_cycle_start() {
+ // save relevant non-axis parameters from Gcode model
+ hm.saved_units = mach_get_units();
+ hm.saved_coord_system = mach_get_coord_system();
+ hm.saved_distance_mode = mach_get_distance_mode();
+ hm.saved_feed_mode = mach_get_feed_mode();
+ hm.saved_feed_rate = mach_get_feed_rate();
+
+ // set working values
+ mach_set_units(MILLIMETERS);
+ mach_set_distance_mode(INCREMENTAL_MODE);
+ mach_set_coord_system(ABSOLUTE_COORDS); // in machine coordinates
+ mach_set_feed_mode(UNITS_PER_MINUTE_MODE);
+ hm.set_coordinates = true;
+
+ hm.axis = -1; // set to retrieve initial axis
+ hm.func = _homing_axis_start; // bind initial processing function
+ mp_set_cycle(CYCLE_HOMING);
+ hm.state = HOMING_NOT_HOMED;
+}
+
+
+void mach_homing_cycle_start_no_set() {
+ mach_homing_cycle_start();
+ hm.set_coordinates = false; // don't update position variables at end of cycle
+}
+
+
+/// Main loop callback for running the homing cycle
+void mach_homing_callback() {
+ if (mp_get_cycle() != CYCLE_HOMING || mp_get_state() != STATE_READY) return;
+ hm.func(hm.axis); // execute the current homing move
+}
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2016 Buildbotics LLC
+ All rights reserved.
+
+ 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/>.
+
+ The software is distributed in the hope that it will be useful, but
+ WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public
+ License along with the software. If not, see
+ <http://www.gnu.org/licenses/>.
+
+ For information regarding this software email:
+ "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#pragma once
+
+#include <stdbool.h>
+
+
+bool mach_is_homing();
+void mach_set_not_homed();
+bool mach_get_homed(int axis);
+void mach_set_homed(int axis, bool homed);
+void mach_homing_cycle_start();
+void mach_homing_cycle_start_no_set();
+void mach_homing_callback();
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2016 Buildbotics LLC
+ All rights reserved.
+
+ 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/>.
+
+ The software is distributed in the hope that it will be useful, but
+ WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public
+ License along with the software. If not, see
+ <http://www.gnu.org/licenses/>.
+
+ For information regarding this software email:
+ "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#include "huanyang.h"
+#include "config.h"
+#include "rtc.h"
+#include "report.h"
+
+#include <avr/io.h>
+#include <avr/interrupt.h>
+#include <avr/pgmspace.h>
+#include <util/crc16.h>
+
+#include <stdio.h>
+#include <string.h>
+#include <math.h>
+
+
+enum {
+ HUANYANG_FUNC_READ = 1,
+ HUANYANG_FUNC_WRITE,
+ HUANYANG_CTRL_WRITE,
+ HUANYANG_CTRL_READ,
+ HUANYANG_FREQ_WRITE,
+ HUANYANG_RESERVED_1,
+ HUANYANG_RESERVED_2,
+ HUANYANG_LOOP_TEST
+};
+
+
+enum {
+ HUANYANG_BASE_FREQ = 4,
+ HUANYANG_MAX_FREQ = 5,
+ HUANYANG_MIN_FREQ = 11,
+ HUANYANG_RATED_MOTOR_VOLTAGE = 141,
+ HUANYANG_RATED_MOTOR_CURRENT = 142,
+ HUANYANG_MOTOR_POLE = 143,
+ HUANYANG_RATED_RPM = 144,
+};
+
+
+enum {
+ HUANYANG_TARGET_FREQ,
+ HUANYANG_ACTUAL_FREQ,
+ HUANYANG_ACTUAL_CURRENT,
+ HUANYANG_ACTUAL_RPM,
+ HUANYANG_DC_VOLTAGE,
+ HUANYANG_AC_VOLTAGE,
+ HUANYANG_CONT,
+ HUANYANG_TEMPERATURE,
+};
+
+
+enum {
+ HUANYANG_FORWARD = 1,
+ HUANYANG_STOP = 8,
+ HUANYANG_REVERSE = 17,
+};
+
+
+enum {
+ HUANYANG_RUN = 1 << 0,
+ HUANYANG_JOG = 1 << 1,
+ HUANYANG_COMMAND_RF = 1 << 2,
+ HUANYANG_RUNNING = 1 << 3,
+ HUANYANG_JOGGING = 1 << 4,
+ HUANYANG_RUNNING_RF = 1 << 5,
+ HUANYANG_BRACKING = 1 << 6,
+ HUANYANG_TRACK_START = 1 << 7,
+};
+
+
+typedef bool (*next_command_cb_t)(int index);
+
+
+typedef struct {
+ uint8_t id;
+ bool debug;
+
+ next_command_cb_t next_command_cb;
+ uint8_t command_index;
+ uint8_t current_offset;
+ uint8_t command[8];
+ uint8_t command_length;
+ uint8_t response_length;
+ uint8_t response[10];
+ uint32_t last;
+ uint8_t retry;
+
+ bool connected;
+ bool changed;
+ bool estop;
+ spindle_mode_t mode;
+ float speed;
+
+ float actual_freq;
+ float actual_current;
+ uint16_t actual_rpm;
+ uint16_t dc_voltage;
+ uint16_t ac_voltage;
+ uint16_t temperature;
+
+ float max_freq;
+ float min_freq;
+ uint16_t rated_rpm;
+
+ uint8_t status;
+} huanyang_t;
+
+
+static huanyang_t ha = {0};
+
+
+#define CTRL_STATUS_RESPONSE(R) ((uint16_t)R[4] << 8 | R[5])
+#define FUNC_RESPONSE(R) (R[2] == 2 ? R[4] : ((uint16_t)R[4] << 8 | R[5]))
+
+
+static uint16_t _crc16(const uint8_t *buffer, unsigned length) {
+ uint16_t crc = 0xffff;
+
+ for (unsigned i = 0; i < length; i++)
+ crc = _crc16_update(crc, buffer[i]);
+
+ return crc;
+}
+
+
+static void _set_baud(uint16_t bsel, uint8_t bscale) {
+ HUANYANG_PORT.BAUDCTRLB = (uint8_t)((bscale << 4) | (bsel >> 8));
+ HUANYANG_PORT.BAUDCTRLA = bsel;
+}
+
+
+static void _set_write(bool x) {
+ if (x) OUTSET_PIN(RS485_RW_PIN); // High
+ else OUTCLR_PIN(RS485_RW_PIN); // Low
+}
+
+
+static void _set_dre_interrupt(bool enable) {
+ if (enable) HUANYANG_PORT.CTRLA |= USART_DREINTLVL_MED_gc;
+ else HUANYANG_PORT.CTRLA &= ~USART_DREINTLVL_MED_gc;
+}
+
+
+static void _set_txc_interrupt(bool enable) {
+ if (enable) HUANYANG_PORT.CTRLA |= USART_TXCINTLVL_MED_gc;
+ else HUANYANG_PORT.CTRLA &= ~USART_TXCINTLVL_MED_gc;
+}
+
+
+static void _set_rxc_interrupt(bool enable) {
+ if (enable) HUANYANG_PORT.CTRLA |= USART_RXCINTLVL_MED_gc;
+ else HUANYANG_PORT.CTRLA &= ~USART_RXCINTLVL_MED_gc;
+}
+
+
+static void _set_command1(int code, uint8_t arg0) {
+ ha.command_length = 4;
+ ha.command[1] = code;
+ ha.command[2] = 1;
+ ha.command[3] = arg0;
+}
+
+
+static void _set_command2(int code, uint8_t arg0, uint8_t arg1) {
+ ha.command_length = 5;
+ ha.command[1] = code;
+ ha.command[2] = 2;
+ ha.command[3] = arg0;
+ ha.command[4] = arg1;
+}
+
+
+static void _set_command3(int code, uint8_t arg0, uint8_t arg1, uint8_t arg2) {
+ ha.command_length = 6;
+ ha.command[1] = code;
+ ha.command[2] = 3;
+ ha.command[3] = arg0;
+ ha.command[4] = arg1;
+ ha.command[5] = arg2;
+}
+
+
+static int _response_length(int code) {
+ switch (code) {
+ case HUANYANG_FUNC_READ: return 8;
+ case HUANYANG_FUNC_WRITE: return 8;
+ case HUANYANG_CTRL_WRITE: return 6;
+ case HUANYANG_CTRL_READ: return 8;
+ case HUANYANG_FREQ_WRITE: return 7;
+ default: return -1;
+ }
+}
+
+
+static bool _update(int index) {
+ // Read response
+ switch (index) {
+ case 1: ha.status = ha.response[5]; break;
+ case 2: ha.max_freq = FUNC_RESPONSE(ha.response) * 0.01; break;
+ case 3: ha.min_freq = FUNC_RESPONSE(ha.response) * 0.01; break;
+ case 4: ha.rated_rpm = FUNC_RESPONSE(ha.response); break;
+ default: break;
+ }
+
+ // Setup next command
+ uint8_t var;
+ switch (index) {
+ case 0: { // Update mode
+ uint8_t state;
+ switch (ha.mode) {
+ case SPINDLE_CW: state = HUANYANG_FORWARD; break;
+ case SPINDLE_CCW: state = HUANYANG_REVERSE; break;
+ default: state = HUANYANG_STOP; break;
+ }
+
+ _set_command1(HUANYANG_CTRL_WRITE, state);
+
+ return true;
+ }
+
+ case 1: var = HUANYANG_MAX_FREQ; break;
+ case 2: var = HUANYANG_MIN_FREQ; break;
+ case 3: var = HUANYANG_RATED_RPM; break;
+
+ case 4: { // Update freqency
+ // Compute frequency in Hz
+ float freq = fabs(ha.speed * 50 / ha.rated_rpm);
+
+ // Clamp frequency
+ if (ha.max_freq < freq) freq = ha.max_freq;
+ if (freq < ha.min_freq) freq = ha.min_freq;
+
+ // Frequency write command
+ uint16_t f = freq * 100;
+ _set_command2(HUANYANG_FREQ_WRITE, f >> 8, f);
+
+ return true;
+ }
+
+ default:
+ report_request();
+ return false;
+ }
+
+ _set_command3(HUANYANG_FUNC_READ, var, 0, 0);
+
+ return true;
+}
+
+
+static bool _query_status(int index) {
+ // Read response
+ switch (index) {
+ case 1: ha.actual_freq = CTRL_STATUS_RESPONSE(ha.response) * 0.01; break;
+ case 2: ha.actual_current = CTRL_STATUS_RESPONSE(ha.response) * 0.1; break;
+ case 3: ha.actual_rpm = CTRL_STATUS_RESPONSE(ha.response); break;
+ case 4: ha.dc_voltage = CTRL_STATUS_RESPONSE(ha.response); break;
+ case 5: ha.ac_voltage = CTRL_STATUS_RESPONSE(ha.response); break;
+ case 6: ha.temperature = CTRL_STATUS_RESPONSE(ha.response); break;
+ default: break;
+ }
+
+ // Setup next command
+ uint8_t var;
+ switch (index) {
+ case 0: var = HUANYANG_ACTUAL_FREQ; break;
+ case 1: var = HUANYANG_ACTUAL_CURRENT; break;
+ case 2: var = HUANYANG_ACTUAL_RPM; break;
+ case 3: var = HUANYANG_DC_VOLTAGE; break;
+ case 4: var = HUANYANG_AC_VOLTAGE; break;
+ case 5: var = HUANYANG_TEMPERATURE; break;
+ default:
+ report_request();
+ return false;
+ }
+
+ _set_command1(HUANYANG_CTRL_READ, var);
+
+ return true;
+}
+
+
+static void _next_command();
+
+
+static void _next_state() {
+ if (ha.changed) {
+ ha.next_command_cb = _update;
+ ha.changed = false;
+
+ } else ha.next_command_cb = _query_status;
+
+ _next_command();
+}
+
+
+static bool _check_response() {
+ // Check CRC
+ uint16_t computed = _crc16(ha.response, ha.response_length - 2);
+ uint16_t expected = (ha.response[ha.response_length - 1] << 8) |
+ ha.response[ha.response_length - 2];
+
+ if (computed != expected) {
+ STATUS_WARNING("huanyang: invalid CRC, expected=0x%04u got=0x%04u",
+ expected, computed);
+ return false;
+ }
+
+ // Check return function code matches sent
+ if (ha.command[1] != ha.response[1]) {
+ STATUS_WARNING("huanyang: invalid function code, expected=%u got=%u",
+ ha.command[2], ha.response[2]);
+ return false;
+ }
+
+ return true;
+}
+
+
+static void _next_command() {
+ if (ha.next_command_cb && ha.next_command_cb(ha.command_index++)) {
+ ha.response_length = _response_length(ha.command[1]);
+
+ ha.command[0] = ha.id;
+
+ uint16_t crc = _crc16(ha.command, ha.command_length);
+ ha.command[ha.command_length++] = crc;
+ ha.command[ha.command_length++] = crc >> 8;
+
+ _set_dre_interrupt(true);
+
+ return;
+ }
+
+ ha.command_index = 0;
+ _next_state();
+}
+
+
+static void _retry_command() {
+ ha.last = 0;
+ ha.current_offset = 0;
+ ha.retry++;
+
+ _set_write(true); // RS485 write mode
+
+ _set_txc_interrupt(false);
+ _set_rxc_interrupt(false);
+ _set_dre_interrupt(true);
+
+ if (ha.debug) STATUS_DEBUG("huanyang: retry %d", ha.retry);
+}
+
+
+// Data register empty interrupt
+ISR(HUANYANG_DRE_vect) {
+ HUANYANG_PORT.DATA = ha.command[ha.current_offset++];
+
+ if (ha.current_offset == ha.command_length) {
+ _set_dre_interrupt(false);
+ _set_txc_interrupt(true);
+ ha.current_offset = 0;
+ }
+}
+
+
+/// Transmit complete interrupt
+ISR(HUANYANG_TXC_vect) {
+ _set_txc_interrupt(false);
+ _set_rxc_interrupt(true);
+ _set_write(false); // RS485 read mode
+ ha.last = rtc_get_time(); // Set timeout timer
+}
+
+
+// Data received interrupt
+ISR(HUANYANG_RXC_vect) {
+ ha.response[ha.current_offset++] = USARTD1.DATA;
+
+ if (ha.current_offset == ha.response_length) {
+ _set_rxc_interrupt(false);
+ ha.current_offset = 0;
+ _set_write(true); // RS485 write mode
+
+ if (_check_response()) {
+ ha.last = 0; // Clear timeout timer
+ ha.retry = 0; // Reset retry counter
+ ha.connected = true;
+
+ _next_command();
+ }
+ }
+}
+
+
+void huanyang_init() {
+ PR.PRPD &= ~PR_USART1_bm; // Disable power reduction
+
+ DIRCLR_PIN(RS485_RO_PIN); // Input
+ OUTSET_PIN(RS485_DI_PIN); // High
+ DIRSET_PIN(RS485_DI_PIN); // Output
+ OUTSET_PIN(RS485_RW_PIN); // High
+ DIRSET_PIN(RS485_RW_PIN); // Output
+
+ _set_baud(3325, 0b1101); // 9600 @ 32MHz with 2x USART
+
+ // No parity, 8 data bits, 1 stop bit
+ USARTD1.CTRLC = USART_CMODE_ASYNCHRONOUS_gc | USART_PMODE_DISABLED_gc |
+ USART_CHSIZE_8BIT_gc;
+
+ // Configure receiver and transmitter
+ USARTD1.CTRLB = USART_RXEN_bm | USART_TXEN_bm | USART_CLK2X_bm;
+
+ ha.id = HUANYANG_ID;
+ huanyang_reset();
+}
+
+
+void huanyang_set(spindle_mode_t mode, float speed) {
+ if ((ha.mode != mode || ha.speed != speed) && !ha.estop) {
+ if (ha.debug) STATUS_DEBUG("huanyang: mode=%d, speed=%0.2f", mode, speed);
+
+ ha.mode = mode;
+ ha.speed = speed;
+ ha.changed = true;
+ }
+}
+
+
+void huanyang_reset() {
+ _set_dre_interrupt(false);
+ _set_txc_interrupt(false);
+ _set_rxc_interrupt(false);
+ _set_write(true); // RS485 write mode
+
+ // Flush USART
+ uint8_t x = USARTD1.DATA;
+ x = USARTD1.STATUS;
+ x = x;
+
+ // Save settings
+ uint8_t id = ha.id;
+ spindle_mode_t mode = ha.mode;
+ float speed = ha.speed;
+ bool debug = ha.debug;
+
+ // Clear state
+ memset(&ha, 0, sizeof(ha));
+
+ // Restore settings
+ ha.id = id;
+ ha.mode = mode;
+ ha.speed = speed;
+ ha.debug = debug;
+ ha.changed = true;
+
+ _next_state();
+}
+
+
+void huanyang_rtc_callback() {
+ if (ha.last && rtc_expired(ha.last + HUANYANG_TIMEOUT)) {
+ if (ha.retry < HUANYANG_RETRIES) _retry_command();
+ else {
+ if (ha.debug) STATUS_DEBUG("huanyang: timedout");
+
+ if (ha.debug && ha.current_offset) {
+ const uint8_t buf_len = 8 * 2 + 1;
+ char sent[buf_len];
+ char received[buf_len];
+
+ uint8_t i;
+ for (i = 0; i < ha.command_length; i++)
+ sprintf(sent + i * 2, "%02x", ha.command[i]);
+ sent[i * 2] = 0;
+
+ for (i = 0; i < ha.current_offset; i++)
+ sprintf(received + i * 2, "%02x", ha.response[i]);
+ received[i * 2] = 0;
+
+ STATUS_DEBUG("huanyang: sent 0x%s received 0x%s expected %u bytes",
+ sent, received, ha.response_length);
+ }
+
+ huanyang_reset();
+ }
+ }
+}
+
+
+void huanyang_estop() {
+ huanyang_set(SPINDLE_OFF, 0);
+ huanyang_reset();
+ ha.estop = true;
+}
+
+
+bool huanyang_stopping() {
+ return ha.estop && (ha.changed || ha.next_command_cb == _update);
+}
+
+
+uint8_t get_huanyang_id() {return ha.id;}
+void set_huanyang_id(uint8_t value) {ha.id = value;}
+bool get_huanyang_debug() {return ha.debug;}
+void set_huanyang_debug(bool value) {ha.debug = value;}
+bool get_huanyang_connected() {return ha.connected;}
+float get_huanyang_freq() {return ha.actual_freq;}
+float get_huanyang_current() {return ha.actual_current;}
+uint16_t get_huanyang_rpm() {return ha.actual_rpm;}
+uint16_t get_huanyang_dcv() {return ha.dc_voltage;}
+uint16_t get_huanyang_acv() {return ha.ac_voltage;}
+uint16_t get_huanyang_temp() {return ha.temperature;}
+float get_huanyang_max_freq() {return ha.max_freq;}
+float get_huanyang_min_freq() {return ha.min_freq;}
+uint16_t get_huanyang_rated_rpm() {return ha.rated_rpm;}
+float get_huanyang_status() {return ha.status;}
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2016 Buildbotics LLC
+ All rights reserved.
+
+ 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/>.
+
+ The software is distributed in the hope that it will be useful, but
+ WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public
+ License along with the software. If not, see
+ <http://www.gnu.org/licenses/>.
+
+ For information regarding this software email:
+ "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#pragma once
+
+#include "machine.h"
+
+
+void huanyang_init();
+void huanyang_set(spindle_mode_t mode, float speed);
+void huanyang_reset();
+void huanyang_rtc_callback();
+void huanyang_estop();
+bool huanyang_stopping();
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2016 Buildbotics LLC
+ Copyright (c) 2010 - 2013 Alden S. Hart Jr.
+ All rights reserved.
+
+ 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/>.
+
+ The software is distributed in the hope that it will be useful, but
+ WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public
+ License along with the software. If not, see
+ <http://www.gnu.org/licenses/>.
+
+ For information regarding this software email:
+ "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#include "i2c.h"
+
+#include <avr/interrupt.h>
+
+#include <stdbool.h>
+
+
+typedef struct {
+ i2c_read_cb_t read_cb;
+ i2c_write_cb_t write_cb;
+ uint8_t data[I2C_MAX_DATA];
+ uint8_t length;
+ bool done;
+ bool write;
+} i2c_t;
+
+static i2c_t i2c = {0};
+
+
+static void _i2c_reset_command() {
+ i2c.length = 0;
+ i2c.done = true;
+ i2c.write = false;
+}
+
+
+static void _i2c_end_command() {
+ if (i2c.length && !i2c.write && i2c.read_cb)
+ i2c.read_cb(*i2c.data, i2c.data + 1, i2c.length - 1);
+
+ _i2c_reset_command();
+}
+
+
+static void _i2c_command_byte(uint8_t byte) {
+ i2c.data[i2c.length++] = byte;
+}
+
+
+ISR(I2C_ISR) {
+ uint8_t status = I2C_DEV.SLAVE.STATUS;
+
+ // Error or collision
+ if (status & (TWI_SLAVE_BUSERR_bm | TWI_SLAVE_COLL_bm)) {
+ _i2c_reset_command();
+ return; // Ignore
+
+ } else if ((status & TWI_SLAVE_APIF_bm) && (status & TWI_SLAVE_AP_bm)) {
+ // START + address match
+ I2C_DEV.SLAVE.CTRLB = TWI_SLAVE_CMD_RESPONSE_gc; // ACK address byte
+ _i2c_end_command(); // Handle repeated START
+
+ } else if (status & TWI_SLAVE_APIF_bm) {
+ // STOP
+ I2C_DEV.SLAVE.STATUS = TWI_SLAVE_APIF_bm; // Clear interrupt flag
+ _i2c_end_command();
+
+ } else if (status & TWI_SLAVE_DIF_bm) {
+ i2c.write = status & TWI_SLAVE_DIR_bm;
+
+ // DATA
+ if (i2c.write) { // Write
+ // Check if master ACKed last byte sent
+ if (i2c.length && (status & TWI_SLAVE_RXACK_bm || i2c.done))
+ I2C_DEV.SLAVE.CTRLB = TWI_SLAVE_CMD_COMPTRANS_gc; // End transaction
+
+ else {
+ // Send some data
+ i2c.done = false;
+ I2C_DEV.SLAVE.DATA = i2c.write_cb(i2c.length++, &i2c.done);
+ I2C_DEV.SLAVE.CTRLB = TWI_SLAVE_CMD_RESPONSE_gc; // Continue transaction
+ }
+
+ } else { // Read
+ _i2c_command_byte(I2C_DEV.SLAVE.DATA);
+
+ // ACK and continue transaction
+ I2C_DEV.SLAVE.CTRLB = TWI_SLAVE_CMD_RESPONSE_gc;
+ }
+ }
+}
+
+
+static uint8_t _i2c_default_write_cb(uint8_t offset, bool *done) {
+ *done = true;
+ return 0;
+}
+
+
+void i2c_init() {
+ i2c_set_write_callback(_i2c_default_write_cb);
+
+ I2C_DEV.SLAVE.CTRLA = TWI_SLAVE_INTLVL_HI_gc | TWI_SLAVE_DIEN_bm |
+ TWI_SLAVE_ENABLE_bm | TWI_SLAVE_APIEN_bm | TWI_SLAVE_PIEN_bm;
+ I2C_DEV.SLAVE.ADDR = I2C_ADDR << 1;
+}
+
+
+void i2c_set_read_callback(i2c_read_cb_t cb) {i2c.read_cb = cb;}
+void i2c_set_write_callback(i2c_write_cb_t cb) {i2c.write_cb = cb;}
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2016 Buildbotics LLC
+ Copyright (c) 2010 - 2013 Alden S. Hart Jr.
+ All rights reserved.
+
+ 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/>.
+
+ The software is distributed in the hope that it will be useful, but
+ WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public
+ License along with the software. If not, see
+ <http://www.gnu.org/licenses/>.
+
+ For information regarding this software email:
+ "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#pragma once
+
+#include "config.h"
+
+#include <stdbool.h>
+
+
+typedef enum {
+ I2C_NULL,
+ I2C_ESTOP,
+ I2C_CLEAR,
+ I2C_PAUSE,
+ I2C_OPTIONAL_PAUSE,
+ I2C_RUN,
+ I2C_STEP,
+ I2C_FLUSH,
+ I2C_REPORT,
+ I2C_HOME,
+ I2C_REBOOT,
+ I2C_ZERO,
+} i2c_cmd_t;
+
+
+typedef void (*i2c_read_cb_t)(i2c_cmd_t cmd, uint8_t *data, uint8_t length);
+typedef uint8_t (*i2c_write_cb_t)(uint8_t offset, bool *done);
+
+
+void i2c_init();
+void i2c_set_read_callback(i2c_read_cb_t cb);
+void i2c_set_write_callback(i2c_write_cb_t cb);
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2016 Buildbotics LLC
+ Copyright (c) 2010 - 2015 Alden S. Hart, Jr.
+ Copyright (c) 2012 - 2015 Rob Giseburt
+ All rights reserved.
+
+ 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/>.
+
+ The software is distributed in the hope that it will be useful, but
+ WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public
+ License along with the software. If not, see
+ <http://www.gnu.org/licenses/>.
+
+ For information regarding this software email:
+ "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+/* This code is a loose implementation of Kramer, Proctor and Messina's
+ * machining functions as described in the NIST RS274/NGC v3
+ *
+ * The machine is the layer between the Gcode parser and the motion control code
+ * for a specific robot. It keeps state and executes commands - passing the
+ * stateless commands to the motion planning layer.
+ *
+ * Synchronizing command execution:
+ *
+ * "Synchronous commands" are commands that affect the runtime and need to be
+ * synchronized with movement. Examples include G4 dwells, program stops and
+ * ends, and most M commands. These are queued into the planner queue and
+ * execute from the queue. Synchronous commands work like this:
+ *
+ * - Call the mach_xxx_xxx() function which will do any input validation and
+ * return an error if it detects one.
+ *
+ * - The mach_ function calls mp_queue_push(). Arguments are a callback to
+ * the _exec_...() function, which is the runtime execution routine, and any
+ * arguments that are needed by the runtime.
+ *
+ * - mp_queue_push() stores the callback and the args in a planner buffer.
+ *
+ * - When planner execution reaches the buffer it executes the callback w/ the
+ * args. Take careful note that the callback executes under an interrupt.
+ *
+ * Note: The synchronous command execution mechanism uses 2 vectors in the bf
+ * buffer to store and return values for the callback. It's obvious, but
+ * impractical to pass the entire bf buffer to the callback as some of these
+ * commands are actually executed locally and have no buffer.
+ */
+
+#include "machine.h"
+
+#include "config.h"
+#include "axis.h"
+#include "gcode_parser.h"
+#include "spindle.h"
+#include "coolant.h"
+#include "homing.h"
+
+#include "plan/planner.h"
+#include "plan/runtime.h"
+#include "plan/dwell.h"
+#include "plan/arc.h"
+#include "plan/line.h"
+#include "plan/state.h"
+
+
+typedef struct { // struct to manage mach globals and cycles
+ float offset[COORDS + 1][AXES]; // coordinate systems & offsets G53-G59
+ float origin_offset[AXES]; // G92 offsets
+ bool origin_offset_enable; // G92 offsets enabled / disabled
+
+ float position[AXES]; // model position
+ float g28_position[AXES]; // stored machine position for G28
+ float g30_position[AXES]; // stored machine position for G30
+
+ gcode_state_t gm; // core gcode model state
+} machine_t;
+
+
+static machine_t mach = {
+ // Offsets
+ .offset = {
+ {}, // ABSOLUTE_COORDS
+
+ {0, 0, 0, 0, 0, 0}, // G54
+ {X_TRAVEL_MAX / 2, Y_TRAVEL_MAX / 2, 0, 0, 0, 0}, // G55
+ {0, 0, 0, 0, 0, 0}, // G56
+ {0, 0, 0, 0, 0, 0}, // G57
+ {0, 0, 0, 0, 0, 0}, // G58
+ {0, 0, 0, 0, 0, 0}, // G59
+ },
+
+ // State
+ .gm = {.motion_mode = MOTION_MODE_CANCEL_MOTION_MODE},
+};
+
+
+// Machine State functions
+uint32_t mach_get_line() {return mach.gm.line;}
+float mach_get_feed_rate() {return mach.gm.feed_rate;}
+feed_mode_t mach_get_feed_mode() {return mach.gm.feed_mode;}
+
+
+bool mach_is_inverse_time_mode() {
+ return mach.gm.feed_mode == INVERSE_TIME_MODE;
+}
+
+
+float mach_get_feed_override() {
+ return mach.gm.feed_override_enable ? mach.gm.feed_override : 1;
+}
+
+
+float mach_get_spindle_override() {
+ return mach.gm.spindle_override_enable ? mach.gm.spindle_override : 1;
+}
+
+
+motion_mode_t mach_get_motion_mode() {return mach.gm.motion_mode;}
+bool mach_is_rapid() {return mach.gm.motion_mode == MOTION_MODE_RAPID;}
+plane_t mach_get_plane() {return mach.gm.plane;}
+units_t mach_get_units() {return mach.gm.units;}
+coord_system_t mach_get_coord_system() {return mach.gm.coord_system;}
+bool mach_get_absolute_mode() {return mach.gm.absolute_mode;}
+path_mode_t mach_get_path_mode() {return mach.gm.path_mode;}
+bool mach_is_exact_stop() {return mach.gm.path_mode == PATH_EXACT_STOP;}
+distance_mode_t mach_get_distance_mode() {return mach.gm.distance_mode;}
+distance_mode_t mach_get_arc_distance_mode() {return mach.gm.arc_distance_mode;}
+
+
+void mach_set_motion_mode(motion_mode_t motion_mode) {
+ mach.gm.motion_mode = motion_mode;
+}
+
+
+/// Spindle speed callback from planner queue
+static stat_t _exec_spindle_speed(mp_buffer_t *bf) {
+ spindle_set_speed(bf->value);
+ return STAT_NOOP; // No move queued
+}
+
+
+/// Queue the S parameter to the planner buffer
+void mach_set_spindle_speed(float speed) {
+ mp_buffer_t *bf = mp_queue_get_tail();
+ bf->value = speed * mach_get_spindle_override();
+ mp_queue_push_nonstop(_exec_spindle_speed, mach_get_line());
+}
+
+
+/// execute the spindle command (called from planner)
+static stat_t _exec_spindle_mode(mp_buffer_t *bf) {
+ spindle_set_mode(bf->value);
+ return STAT_NOOP; // No move queued
+}
+
+
+/// Queue the spindle command to the planner buffer
+void mach_set_spindle_mode(spindle_mode_t mode) {
+ mp_buffer_t *bf = mp_queue_get_tail();
+ bf->value = mode;
+ mp_queue_push_nonstop(_exec_spindle_mode, mach_get_line());
+}
+
+
+void mach_set_absolute_mode(bool absolute_mode) {
+ mach.gm.absolute_mode = absolute_mode;
+}
+
+
+void mach_set_line(uint32_t line) {mach.gm.line = line;}
+
+
+/* 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
+ *
+ * Notes on Coordinate System and Offset functions
+ *
+ * All positional information in the 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.
+ *
+ * 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
+ * - G28 and G30 moves; these are run in absolute coordinates
+ *
+ * The offsets themselves are considered static, are kept in mach, and are
+ * supposed to be persistent.
+ *
+ * To reduce complexity and data load the following is done:
+ * - Full data for coordinates/offsets is only accessible by the
+ * machine, not the downstream
+ * - 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
+ */
+
+/* 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.
+ */
+float mach_get_active_coord_offset(uint8_t axis) {
+ // no offset in absolute override mode
+ if (mach.gm.absolute_mode) return 0;
+ float offset = mach.offset[mach.gm.coord_system][axis];
+
+ if (mach.origin_offset_enable)
+ offset += mach.origin_offset[axis]; // includes G5x and G92 components
+
+ return offset;
+}
+
+
+static stat_t _exec_update_work_offsets(mp_buffer_t *bf) {
+ mp_runtime_set_work_offsets(bf->target);
+ return STAT_NOOP; // No move queued
+}
+
+
+// Capture coord offsets from the model into absolute values
+void mach_update_work_offsets() {
+ static float work_offset[AXES] = {0};
+ bool same = true;
+
+ for (int axis = 0; axis < AXES; axis++) {
+ float offset = mach_get_active_coord_offset(axis);
+
+ if (offset != work_offset[axis]) {
+ work_offset[axis] = offset;
+ same = false;
+ }
+ }
+
+ if (!same) {
+ mp_buffer_t *bf = mp_queue_get_tail();
+ copy_vector(bf->target, work_offset);
+ mp_queue_push_nonstop(_exec_update_work_offsets, mach_get_line());
+ }
+}
+
+
+const float *mach_get_position() {return mach.position;}
+
+
+void mach_set_position(const float position[]) {
+ copy_vector(mach.position, position);
+}
+
+
+/*** Get position of axis in absolute coordinates
+ *
+ * NOTE: Machine position is always returned in mm mode. No units conversion
+ * is performed.
+ */
+float mach_get_axis_position(uint8_t axis) {return mach.position[axis];}
+
+
+/* Critical helpers
+ *
+ * Core functions supporting the machining functions
+ * These functions are not part of the NIST defined functions
+ */
+
+
+
+/*** Calculate target vector
+ *
+ * This is a core routine. It handles:
+ * - conversion of linear units to internal canonical form (mm)
+ * - conversion of relative mode to absolute (internal canonical form)
+ * - translation of work coordinates to machine coordinates (internal
+ * canonical form)
+ * - application of axis radius mode
+ *
+ * Target coordinates are provided in @param values.
+ * Axes that need processing are signaled in @param flags.
+ */
+void mach_calc_target(float target[], const float values[],
+ const bool flags[]) {
+ for (int axis = 0; axis < AXES; axis++) {
+ target[axis] = mach.position[axis];
+ if (!flags[axis] || !axis_is_enabled(axis)) continue;
+
+ target[axis] = mach.gm.distance_mode == ABSOLUTE_MODE ?
+ mach_get_active_coord_offset(axis) : mach.position[axis];
+
+ float radius = axis_get_radius(axis);
+ if (radius) // Handle radius mode if radius is non-zero
+ target[axis] += TO_MM(values[axis]) * 360 / (2 * M_PI * radius);
+ // For ABC axes no mm conversion - it's already in degrees
+ else if (AXIS_Z < axis) target[axis] += values[axis];
+ else target[axis] += TO_MM(values[axis]);
+ }
+}
+
+
+/*** Return error code if soft limit is exceeded
+ *
+ * Must be called with target properly set in GM struct. Best done
+ * after mach_calc_target().
+ *
+ * Tests for soft limit for any homed axis if min and max are
+ * different values. You can set min and max to 0,0 to disable soft
+ * limits for an axis. Also will not test a min or a max if the value
+ * is < -1000000 (negative one million). This allows a single end to
+ * be tested w/the other disabled, should that requirement ever arise.
+ */
+stat_t mach_test_soft_limits(float target[]) {
+ for (int axis = 0; axis < AXES; axis++) {
+ if (!axis_get_homed(axis)) continue; // don't test axes that arent homed
+
+ float min = axis_get_travel_min(axis);
+ float max = axis_get_travel_max(axis);
+
+ // min == max means no soft limits
+ if (fp_EQ(min, max)) continue;
+
+ if ((min > DISABLE_SOFT_LIMIT && target[axis] < min) ||
+ (max > DISABLE_SOFT_LIMIT && target[axis] > max))
+ return STAT_SOFT_LIMIT_EXCEEDED;
+ }
+
+ return STAT_OK;
+}
+
+
+/* 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
+ */
+
+// Initialization and Termination (4.3.2)
+
+void machine_init() {
+ // Set gcode defaults
+ mach_set_units(GCODE_DEFAULT_UNITS);
+ mach_set_coord_system(GCODE_DEFAULT_COORD_SYSTEM);
+ mach_set_plane(GCODE_DEFAULT_PLANE);
+ mach_set_path_mode(GCODE_DEFAULT_PATH_CONTROL);
+ mach_set_distance_mode(GCODE_DEFAULT_DISTANCE_MODE);
+ mach_set_arc_distance_mode(GCODE_DEFAULT_ARC_DISTANCE_MODE);
+ mach_set_feed_mode(UNITS_PER_MINUTE_MODE); // always the default
+
+ // Sub-system inits
+ spindle_init();
+ coolant_init();
+}
+
+
+// Representation (4.3.3)
+//
+// Affect the Gcode model only (asynchronous)
+// These functions assume input validation occurred upstream.
+
+/// G17, G18, G19 select axis plane
+void mach_set_plane(plane_t plane) {mach.gm.plane = plane;}
+
+
+/// G20, G21
+void mach_set_units(units_t mode) {mach.gm.units = mode;}
+
+
+/// G90, G91
+void mach_set_distance_mode(distance_mode_t mode) {
+ mach.gm.distance_mode = mode;
+}
+
+
+/// G90.1, G91.1
+void mach_set_arc_distance_mode(distance_mode_t mode) {
+ mach.gm.arc_distance_mode = mode;
+}
+
+
+/* G10 L2 Pn, delayed persistence
+ *
+ * This function applies the offset to the GM model.
+ */
+void mach_set_coord_offsets(coord_system_t coord_system, float offset[],
+ bool flags[]) {
+ if (coord_system < G54 || G59 < coord_system) return;
+
+ for (int axis = 0; axis < AXES; axis++)
+ if (flags[axis])
+ mach.offset[coord_system][axis] = TO_MM(offset[axis]);
+}
+
+
+/// G54-G59
+void mach_set_coord_system(coord_system_t coord_system) {
+ mach.gm.coord_system = coord_system;
+}
+
+
+/* Set the position of a single axis in the model, planner and runtime
+ *
+ * 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 !!!!!
+ *
+ * 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.
+ */
+void mach_set_axis_position(unsigned axis, float position) {
+ //if (!mp_is_quiescent()) ALARM(STAT_MACH_NOT_QUIESCENT);
+ if (AXES <= axis) return;
+
+ mach.position[axis] = position;
+ mp_set_axis_position(axis, position);
+ mp_runtime_set_axis_position(axis, position);
+ mp_runtime_set_steps_from_position();
+}
+
+
+stat_t mach_zero_all() {
+ for (unsigned axis = 0; axis < AXES; axis++) {
+ stat_t status = mach_zero_axis(axis);
+ if (status != STAT_OK) return status;
+ }
+
+ return STAT_OK;
+}
+
+
+stat_t mach_zero_axis(unsigned axis) {
+ if (!mp_is_quiescent()) return STAT_MACH_NOT_QUIESCENT;
+ if (AXES <= axis) return STAT_INVALID_AXIS;
+
+ mach_set_axis_position(axis, 0);
+
+ return STAT_OK;
+}
+
+
+// G28.3 functions and support
+static stat_t _exec_absolute_origin(mp_buffer_t *bf) {
+ const float *origin = bf->target;
+ const float *flags = bf->unit;
+
+ for (int axis = 0; axis < AXES; axis++)
+ if (flags[axis]) {
+ mp_runtime_set_axis_position(axis, origin[axis]);
+ mach_set_homed(axis, true); // G28.3 is not considered homed until here
+ }
+
+ mp_runtime_set_steps_from_position();
+
+ return STAT_NOOP; // No move queued
+}
+
+
+/* G28.3 - model, planner and queue to runtime
+ *
+ * Takes a vector of origins (presumably 0's, but not necessarily) and
+ * applies them to all axes where the corresponding position in the
+ * flags vector is true (1).
+ *
+ * This is a 2 step process. The model and planner contexts are set
+ * immediately, the runtime command is queued and synchronized with
+ * the planner queue. This includes the runtime position and the step
+ * recording done by the encoders. At that point any axis that is set
+ * is also marked as homed.
+ */
+void mach_set_absolute_origin(float origin[], bool flags[]) {
+ float value[AXES];
+
+ for (int axis = 0; axis < AXES; axis++)
+ if (flags[axis]) {
+ value[axis] = TO_MM(origin[axis]);
+ mach.position[axis] = value[axis]; // set model position
+ mp_set_axis_position(axis, value[axis]); // set mm position
+ }
+
+ mp_buffer_t *bf = mp_queue_get_tail();
+ copy_vector(bf->target, origin);
+ copy_vector(bf->unit, flags);
+ mp_queue_push_nonstop(_exec_absolute_origin, mach_get_line());
+}
+
+
+/* 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
+ */
+
+/// G92
+void mach_set_origin_offsets(float offset[], bool flags[]) {
+ // set offsets in the Gcode model extended context
+ mach.origin_offset_enable = true;
+ for (int axis = 0; axis < AXES; axis++)
+ if (flags[axis])
+ mach.origin_offset[axis] = mach.position[axis] -
+ mach.offset[mach.gm.coord_system][axis] - TO_MM(offset[axis]);
+}
+
+
+/// G92.1
+void mach_reset_origin_offsets() {
+ mach.origin_offset_enable = false;
+ for (int axis = 0; axis < AXES; axis++)
+ mach.origin_offset[axis] = 0;
+}
+
+
+/// G92.2
+void mach_suspend_origin_offsets() {
+ mach.origin_offset_enable = false;
+}
+
+
+/// G92.3
+void mach_resume_origin_offsets() {
+ mach.origin_offset_enable = true;
+}
+
+
+stat_t mach_plan_line(float target[]) {
+ return mp_aline(target, mach_is_rapid(), mach_is_inverse_time_mode(),
+ mach_is_exact_stop(), mach.gm.feed_rate,
+ mach_get_feed_override(), mach_get_line());
+}
+
+
+// Free Space Motion (4.3.4)
+static stat_t _feed(float values[], bool flags[]) {
+ float target[AXES];
+ mach_calc_target(target, values, flags);
+
+ // test soft limits
+ stat_t status = mach_test_soft_limits(target);
+ if (status != STAT_OK) return ALARM(status);
+
+ // prep and plan the move
+ mach_update_work_offsets(); // update resolved offsets
+ mach_plan_line(target);
+ copy_vector(mach.position, target); // update model position
+
+ return status;
+}
+
+
+/// G0 linear rapid
+stat_t mach_rapid(float values[], bool flags[]) {
+ mach.gm.motion_mode = MOTION_MODE_RAPID;
+ return _feed(values, flags);
+}
+
+
+/// G28.1
+void mach_set_g28_position() {copy_vector(mach.g28_position, mach.position);}
+
+
+/// G28
+stat_t mach_goto_g28_position(float target[], bool flags[]) {
+ mach_set_absolute_mode(true);
+
+ // move through intermediate point, or skip
+ mach_rapid(target, flags);
+
+ // execute actual stored move
+ bool f[] = {true, true, true, true, true, true};
+ return mach_rapid(mach.g28_position, f);
+}
+
+
+/// G30.1
+void mach_set_g30_position() {copy_vector(mach.g30_position, mach.position);}
+
+
+/// G30
+stat_t mach_goto_g30_position(float target[], bool flags[]) {
+ mach_set_absolute_mode(true);
+
+ // move through intermediate point, or skip
+ mach_rapid(target, flags);
+
+ // execute actual stored move
+ bool f[] = {true, true, true, true, true, true};
+ return mach_rapid(mach.g30_position, f);
+}
+
+
+// Machining Attributes (4.3.5)
+
+/// F parameter
+/// Normalize feed rate to mm/min or to minutes if in inverse time mode
+void mach_set_feed_rate(float feed_rate) {
+ if (mach.gm.feed_mode == INVERSE_TIME_MODE)
+ // normalize to minutes (active for this gcode block only)
+ mach.gm.feed_rate = feed_rate ? 1 / feed_rate : 0; // Avoid div by zero
+
+ else mach.gm.feed_rate = TO_MM(feed_rate);
+}
+
+
+/// G93, G94
+void mach_set_feed_mode(feed_mode_t mode) {
+ if (mach.gm.feed_mode == mode) return;
+ mach.gm.feed_rate = 0; // Force setting feed rate after changing modes
+ mach.gm.feed_mode = mode;
+}
+
+
+/// G61, G61.1, G64
+void mach_set_path_mode(path_mode_t mode) {
+ mach.gm.path_mode = mode;
+}
+
+
+// Machining Functions (4.3.6) See arc.c
+
+/// G4, P parameter (seconds)
+stat_t mach_dwell(float seconds) {
+ return mp_dwell(seconds, mach_get_line());
+}
+
+
+/// G1
+stat_t mach_feed(float values[], bool flags[]) {
+ // trap zero feed rate condition
+ if (fp_ZERO(mach.gm.feed_rate) ||
+ (mach.gm.feed_mode == INVERSE_TIME_MODE && !parser.gf.feed_rate))
+ return STAT_GCODE_FEEDRATE_NOT_SPECIFIED;
+
+ mach.gm.motion_mode = MOTION_MODE_FEED;
+ return _feed(values, flags);
+}
+
+
+// Spindle Functions (4.3.7) see spindle.c, spindle.h
+
+// Tool Functions (4.3.8)
+
+/// T parameter
+void mach_select_tool(uint8_t tool) {mach.gm.tool = tool;}
+
+
+static stat_t _exec_change_tool(mp_buffer_t *bf) {
+ mp_runtime_set_tool(bf->value);
+ mp_set_hold_reason(HOLD_REASON_TOOL_CHANGE);
+ mp_state_holding();
+ return STAT_NOOP; // No move queued
+}
+
+
+/// M6
+void mach_change_tool(bool x) {
+ mp_buffer_t *bf = mp_queue_get_tail();
+ bf->value = mach.gm.tool;
+ mp_queue_push(_exec_change_tool, mach_get_line());
+}
+
+
+// Miscellaneous Functions (4.3.9)
+static stat_t _exec_mist_coolant(mp_buffer_t *bf) {
+ coolant_set_mist(bf->value);
+ return STAT_NOOP; // No move queued
+}
+
+
+/// M7
+void mach_mist_coolant_control(bool mist_coolant) {
+ mp_buffer_t *bf = mp_queue_get_tail();
+ bf->value = mist_coolant;
+ mp_queue_push_nonstop(_exec_mist_coolant, mach_get_line());
+}
+
+
+static stat_t _exec_flood_coolant(mp_buffer_t *bf) {
+ coolant_set_flood(bf->value);
+ if (!bf->value) coolant_set_mist(false); // M9 special function
+ return STAT_NOOP; // No move queued
+}
+
+
+/// M8, M9
+void mach_flood_coolant_control(bool flood_coolant) {
+ mp_buffer_t *bf = mp_queue_get_tail();
+ bf->value = flood_coolant;
+ mp_queue_push_nonstop(_exec_flood_coolant, mach_get_line());
+}
+
+
+/* Override enables are kind of a mess in Gcode. This is an attempt to sort
+ * them out. See
+ * http://www.linuxcnc.org/docs/2.4/html/gcode_main.html#sec:M50:-Feed-Override
+ */
+
+/// M48, M49
+void mach_override_enables(bool flag) {
+ mach.gm.feed_override_enable = flag;
+ mach.gm.spindle_override_enable = flag;
+}
+
+
+/// M50
+void mach_feed_override_enable(bool flag) {
+ if (parser.gf.parameter && fp_ZERO(parser.gn.parameter))
+ mach.gm.feed_override_enable = false;
+ else {
+ if (parser.gf.parameter) mach.gm.feed_override = parser.gf.parameter;
+ mach.gm.feed_override_enable = true;
+ }
+}
+
+
+/// M51
+void mach_spindle_override_enable(bool flag) {
+ if (parser.gf.parameter && fp_ZERO(parser.gn.parameter))
+ mach.gm.spindle_override_enable = false;
+ else {
+ if (parser.gf.parameter) mach.gm.spindle_override = parser.gf.parameter;
+ mach.gm.spindle_override_enable = true;
+ }
+}
+
+
+void mach_message(const char *message) {
+ status_message_P(0, STAT_LEVEL_INFO, STAT_OK, PSTR("%s"), message);
+}
+
+
+/* Program Functions (4.3.10)
+ *
+ * This group implements stop, start, end, and hold.
+ * It is extended beyond the NIST spec to handle various situations.
+ *
+ * mach_program_stop and mach_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).
+ *
+ * mach_program_end is a stop that also resets the machine to initial state
+ */
+
+
+static stat_t _exec_program_stop(mp_buffer_t *bf) {
+ // Machine should be stopped at this point. Go into hold so that a start is
+ // needed before executing further instructions.
+ mp_set_hold_reason(HOLD_REASON_PROGRAM_PAUSE);
+ mp_state_holding();
+ return STAT_NOOP; // No move queued
+}
+
+
+/// M0 Queue a program stop
+void mach_program_stop() {
+ mp_queue_push(_exec_program_stop, mach_get_line());
+}
+
+
+static stat_t _exec_optional_program_stop(mp_buffer_t *bf) {
+ mp_state_optional_pause(); // Pause here if it was requested by the user
+ return STAT_NOOP; // No move queued
+}
+
+
+/// M1
+void mach_optional_program_stop() {
+ mp_queue_push(_exec_optional_program_stop, mach_get_line());
+}
+
+
+static stat_t _exec_pallet_change_stop(mp_buffer_t *bf) {
+ // Emit pallet change signal
+ mp_set_hold_reason(HOLD_REASON_PALLET_CHANGE);
+ mp_state_holding();
+ return STAT_NOOP; // No move queued
+}
+
+
+/// M60
+void mach_pallet_change_stop() {
+ mp_queue_push(_exec_pallet_change_stop, mach_get_line());
+}
+
+
+/*** mach_program_end() implements M2 and M30. End behaviors are defined by
+ * NIST 3.6.1 are:
+ *
+ * 1. Axis offsets are set to zero (like G92.2) and origin offsets are set
+ * to the default (like G54)
+ * 2. Selected plane is set to 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)
+ * 5. Feed and speed overrides are set to ON (like M48)
+ * 6. Cutter compensation is turned off (like G40)
+ * 7. The spindle is stopped (like M5)
+ * 8. The current motion mode is set to G_1 (like G1)
+ * 9. Coolant is turned off (like M9)
+ *
+ * mach_program_end() implements things slightly differently:
+ *
+ * 1. Axis offsets are set to G92.1 CANCEL offsets
+ * (instead of using G92.2 SUSPEND Offsets)
+ * Set default coordinate system
+ * 2. Selected plane is set to default plane
+ * 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
+ * 7. The spindle is stopped (like M5)
+ * 8. Motion mode is canceled like G80 (not set to G1)
+ * 9. Coolant is turned off (like M9)
+ * + Default INCHES or MM units mode is restored
+ */
+
+
+/// M2, M30
+void mach_program_end() {
+ mach_reset_origin_offsets(); // G92.1 - we do G91.1 instead of G92.2
+ mach_set_coord_system(GCODE_DEFAULT_COORD_SYSTEM);
+ mach_set_plane(GCODE_DEFAULT_PLANE);
+ mach_set_distance_mode(GCODE_DEFAULT_DISTANCE_MODE);
+ mach_set_arc_distance_mode(GCODE_DEFAULT_ARC_DISTANCE_MODE);
+ mach_set_spindle_mode(SPINDLE_OFF); // M5
+ mach_flood_coolant_control(false); // M9
+ mach_set_feed_mode(UNITS_PER_MINUTE_MODE); // G94
+ mach_set_motion_mode(MOTION_MODE_CANCEL_MOTION_MODE);
+}
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2016 Buildbotics LLC
+ Copyright (c) 2010 - 2015 Alden S. Hart, Jr.
+ All rights reserved.
+
+ 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/>.
+
+ The software is distributed in the hope that it will be useful, but
+ WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public
+ License along with the software. If not, see
+ <http://www.gnu.org/licenses/>.
+
+ For information regarding this software email:
+ "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#pragma once
+
+
+#include "config.h"
+#include "status.h"
+#include "gcode_state.h"
+
+
+#define TO_MM(a) (mach_get_units() == INCHES ? (a) * MM_PER_INCH : a)
+
+// Model state getters and setters
+uint32_t mach_get_line();
+float mach_get_feed_rate();
+bool mach_is_inverse_time_mode();
+feed_mode_t mach_get_feed_mode();
+float mach_get_feed_override();
+float mach_get_spindle_override();
+motion_mode_t mach_get_motion_mode();
+bool mach_is_rapid();
+plane_t mach_get_plane();
+units_t mach_get_units();
+coord_system_t mach_get_coord_system();
+bool mach_get_absolute_mode();
+path_mode_t mach_get_path_mode();
+bool mach_is_exact_stop();
+distance_mode_t mach_get_distance_mode();
+distance_mode_t mach_get_arc_distance_mode();
+
+void mach_set_motion_mode(motion_mode_t motion_mode);
+void mach_set_spindle_mode(spindle_mode_t spindle_mode);
+void mach_set_spindle_speed(float speed);
+void mach_set_absolute_mode(bool absolute_mode);
+void mach_set_line(uint32_t line);
+
+// Coordinate systems and offsets
+float mach_get_active_coord_offset(uint8_t axis);
+void mach_update_work_offsets();
+const float *mach_get_position();
+void mach_set_position(const float position[]);
+float mach_get_axis_position(uint8_t axis);
+
+// Critical helpers
+void mach_calc_target(float target[], const float values[], const bool flags[]);
+stat_t mach_test_soft_limits(float target[]);
+
+// machining functions defined by NIST [organized by NIST Gcode doc]
+
+// Initialization and termination (4.3.2)
+void machine_init();
+
+// Representation (4.3.3)
+void mach_set_plane(plane_t plane);
+void mach_set_units(units_t mode);
+void mach_set_distance_mode(distance_mode_t mode);
+void mach_set_arc_distance_mode(distance_mode_t mode);
+void mach_set_coord_offsets(coord_system_t coord_system, float offset[],
+ bool flags[]);
+
+void mach_set_axis_position(unsigned axis, float position);
+void mach_set_absolute_origin(float origin[], bool flags[]);
+
+stat_t mach_zero_all();
+stat_t mach_zero_axis(unsigned axis);
+
+void mach_set_coord_system(coord_system_t coord_system);
+void mach_set_origin_offsets(float offset[], bool flags[]);
+void mach_reset_origin_offsets();
+void mach_suspend_origin_offsets();
+void mach_resume_origin_offsets();
+
+// Free Space Motion (4.3.4)
+stat_t mach_plan_line(float target[]);
+stat_t mach_rapid(float target[], bool flags[]);
+void mach_set_g28_position();
+stat_t mach_goto_g28_position(float target[], bool flags[]);
+void mach_set_g30_position();
+stat_t mach_goto_g30_position(float target[], bool flags[]);
+
+// Machining Attributes (4.3.5)
+void mach_set_feed_rate(float feed_rate);
+void mach_set_feed_mode(feed_mode_t mode);
+void mach_set_path_mode(path_mode_t mode);
+
+// Machining Functions (4.3.6) see arc.h
+stat_t mach_feed(float target[], bool flags[]);
+stat_t mach_dwell(float seconds);
+
+// Spindle Functions (4.3.7) see spindle.h
+
+// Tool Functions (4.3.8)
+void mach_select_tool(uint8_t tool);
+void mach_change_tool(bool x);
+
+// Miscellaneous Functions (4.3.9)
+void mach_mist_coolant_control(bool mist_coolant);
+void mach_flood_coolant_control(bool flood_coolant);
+
+void mach_override_enables(bool flag);
+void mach_feed_override_enable(bool flag);
+void mach_spindle_override_enable(bool flag);
+
+void mach_message(const char *message);
+
+// Program Functions (4.3.10)
+void mach_program_stop();
+void mach_optional_program_stop();
+void mach_pallet_change_stop();
+void mach_program_end();
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2016 Buildbotics LLC
+ Copyright (c) 2010 - 2015 Alden S. Hart, Jr.
+ Copyright (c) 2013 - 2015 Robert Giseburt
+ All rights reserved.
+
+ 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/>.
+
+ The software is distributed in the hope that it will be useful, but
+ WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public
+ License along with the software. If not, see
+ <http://www.gnu.org/licenses/>.
+
+ For information regarding this software email:
+ "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#include "hardware.h"
+#include "machine.h"
+#include "stepper.h"
+#include "motor.h"
+#include "switch.h"
+#include "usart.h"
+#include "drv8711.h"
+#include "vars.h"
+#include "rtc.h"
+#include "report.h"
+#include "command.h"
+#include "estop.h"
+#include "probing.h"
+#include "homing.h"
+#include "home.h"
+#include "i2c.h"
+
+#include "plan/planner.h"
+#include "plan/arc.h"
+#include "plan/state.h"
+
+#include <avr/pgmspace.h>
+#include <avr/wdt.h>
+
+#include <stdio.h>
+#include <stdbool.h>
+
+
+int main() {
+ //wdt_enable(WDTO_250MS); TODO
+
+ // Init
+ cli(); // disable interrupts
+
+ hardware_init(); // hardware setup - must be first
+ usart_init(); // serial port
+ i2c_init(); // i2c port
+ drv8711_init(); // motor drivers
+ stepper_init(); // steppers
+ motor_init(); // motors
+ switch_init(); // switches
+ mp_init(); // motion planning
+ machine_init(); // gcode machine
+ vars_init(); // configuration variables
+ estop_init(); // emergency stop handler
+ command_init();
+
+ sei(); // enable interrupts
+
+ // Splash
+ fprintf_P(stdout, PSTR("\n{\"firmware\": \"Buildbotics AVR\", "
+ "\"version\": \"" VERSION "\"}\n"));
+
+ // Nominal segment time cannot be longer than maximum
+ if (MAX_SEGMENT_TIME < NOM_SEGMENT_TIME) ALARM(STAT_INTERNAL_ERROR);
+
+ // Main loop
+ while (true) {
+ hw_reset_handler(); // handle hard reset requests
+ if (!estop_triggered()) {
+ mp_state_callback();
+ mach_arc_callback(); // arc generation runs
+ home_callback();
+ //mach_homing_callback(); // G28.2 continuation
+ mach_probe_callback(); // G38.2 continuation
+ }
+ command_callback(); // process next command
+ report_callback(); // report changes
+ wdt_reset();
+ }
+
+ return 0;
+}
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2016 Buildbotics LLC
+ All rights reserved.
+
+ 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/>.
+
+ The software is distributed in the hope that it will be useful, but
+ WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public
+ License along with the software. If not, see
+ <http://www.gnu.org/licenses/>.
+
+ For information regarding this software email:
+ "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+// OS, communications and low-level status
+STAT_MSG(OK, "OK")
+STAT_MSG(EAGAIN, "Run command again")
+STAT_MSG(NOOP, "No op")
+STAT_MSG(COMPLETE, "Complete")
+STAT_MSG(BUSY, "Busy")
+STAT_MSG(NO_SUCH_DEVICE, "No such device")
+STAT_MSG(BUFFER_FULL, "Buffer full")
+STAT_MSG(BUFFER_FULL_FATAL, "Buffer full - fatal")
+STAT_MSG(EEPROM_DATA_INVALID, "EEPROM data invalid")
+STAT_MSG(STEP_CHECK_FAILED, "Step check failed")
+STAT_MSG(MACH_NOT_QUIESCENT, "Cannot perform action while machine active")
+STAT_MSG(INTERNAL_ERROR, "Internal error")
+
+STAT_MSG(MOTOR_STALLED, "Motor stalled")
+STAT_MSG(MOTOR_OVER_TEMP, "Motor over temperature")
+STAT_MSG(MOTOR_OVER_CURRENT, "Motor over temperature")
+STAT_MSG(MOTOR_DRIVER_FAULT, "Motor driver fault")
+STAT_MSG(MOTOR_UNDER_VOLTAGE, "Motor under voltage")
+
+STAT_MSG(PREP_LINE_MOVE_TIME_INFINITE, "Move time is infinite")
+STAT_MSG(PREP_LINE_MOVE_TIME_NAN, "Move time is NAN")
+STAT_MSG(MOVE_TARGET_INFINITE, "Move target is infinite")
+STAT_MSG(MOVE_TARGET_NAN, "Move target is NAN")
+STAT_MSG(EXCESSIVE_MOVE_ERROR, "Excessive move error")
+
+STAT_MSG(ESTOP_USER, "User triggered EStop")
+STAT_MSG(ESTOP_SWITCH, "Switch triggered EStop")
+
+// Generic data input errors
+STAT_MSG(UNRECOGNIZED_NAME, "Unrecognized command or variable name")
+STAT_MSG(INVALID_OR_MALFORMED_COMMAND, "Invalid or malformed command")
+STAT_MSG(TOO_MANY_ARGUMENTS, "Too many arguments to command")
+STAT_MSG(TOO_FEW_ARGUMENTS, "Too few arguments to command")
+STAT_MSG(BAD_NUMBER_FORMAT, "Bad number format")
+STAT_MSG(PARAMETER_IS_READ_ONLY, "Parameter is read-only")
+STAT_MSG(PARAMETER_CANNOT_BE_READ, "Parameter cannot be read")
+STAT_MSG(COMMAND_NOT_ACCEPTED, "Command not accepted at this time")
+STAT_MSG(INPUT_EXCEEDS_MAX_LENGTH, "Input exceeds max length")
+STAT_MSG(INPUT_LESS_THAN_MIN_VALUE, "Input less than minimum value")
+STAT_MSG(INPUT_EXCEEDS_MAX_VALUE, "Input exceeds maximum value")
+STAT_MSG(INPUT_VALUE_RANGE_ERROR, "Input value range error")
+
+// Gcode errors & warnings (Most originate from NIST)
+STAT_MSG(MODAL_GROUP_VIOLATION, "Modal group violation")
+STAT_MSG(GCODE_COMMAND_UNSUPPORTED, "Invalid or unsupported G-Code command")
+STAT_MSG(MCODE_COMMAND_UNSUPPORTED, "M code unsupported")
+STAT_MSG(GCODE_AXIS_IS_MISSING, "Axis word missing")
+STAT_MSG(GCODE_FEEDRATE_NOT_SPECIFIED, "Feedrate not specified")
+STAT_MSG(ARC_SPECIFICATION_ERROR, "Arc specification error")
+STAT_MSG(ARC_AXIS_MISSING_FOR_SELECTED_PLANE, "Arc missing axis")
+STAT_MSG(ARC_RADIUS_OUT_OF_TOLERANCE, "Arc radius arc out of tolerance")
+STAT_MSG(ARC_ENDPOINT_IS_STARTING_POINT, "Arc end point is starting point")
+STAT_MSG(ARC_OFFSETS_MISSING_FOR_PLANE, "arc offsets missing for plane")
+STAT_MSG(P_WORD_IS_NOT_A_POSITIVE_INTEGER, "P word is not a positive integer")
+
+// Errors and warnings
+STAT_MSG(MINIMUM_LENGTH_MOVE, "Move less than minimum length")
+STAT_MSG(MINIMUM_TIME_MOVE, "Move less than minimum time")
+STAT_MSG(MAXIMUM_TIME_MOVE, "Move greater than maximum time")
+STAT_MSG(MACHINE_ALARMED, "Machine alarmed - Command not processed")
+STAT_MSG(LIMIT_SWITCH_HIT, "Limit switch hit - Shutdown occurred")
+STAT_MSG(SOFT_LIMIT_EXCEEDED, "Soft limit exceeded")
+STAT_MSG(INVALID_AXIS, "Invalid axis")
+STAT_MSG(EXPECTED_MOVE, "A move was expected but none was queued")
+
+// Homing
+STAT_MSG(HOMING_CYCLE_FAILED, "Homing cycle failed")
+STAT_MSG(HOMING_ERROR_BAD_OR_NO_AXIS, "Homing Error - Bad or no axis specified")
+STAT_MSG(HOMING_ERROR_AXIS_MISCONFIGURED, "Homing Error - Axis misconfigured")
+STAT_MSG(HOMING_ERROR_ZERO_SEARCH_VELOCITY,
+ "Homing Error - Search velocity is zero")
+STAT_MSG(HOMING_ERROR_ZERO_LATCH_VELOCITY,
+ "Homing Error - Latch velocity is zero")
+STAT_MSG(HOMING_ERROR_TRAVEL_MIN_MAX_IDENTICAL,
+ "Homing Error - Travel min & max are the same")
+STAT_MSG(HOMING_ERROR_NEGATIVE_LATCH_BACKOFF,
+ "Homing Error - Negative latch backoff")
+STAT_MSG(HOMING_ERROR_SWITCH_MISCONFIGURATION,
+ "Homing Error - Homing switches misconfigured")
+
+// Probing
+STAT_MSG(PROBE_INVALID_DEST, "Probing error - invalid destination")
+STAT_MSG(MOVE_DURING_PROBE, "Probing error - move during probe")
+
+// End of stats marker
+STAT_MSG(MAX, "")
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2016 Buildbotics LLC
+ All rights reserved.
+
+ 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/>.
+
+ The software is distributed in the hope that it will be useful, but
+ WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public
+ License along with the software. If not, see
+ <http://www.gnu.org/licenses/>.
+
+ For information regarding this software email:
+ "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#include "motor.h"
+#include "config.h"
+#include "hardware.h"
+#include "cpp_magic.h"
+#include "rtc.h"
+#include "report.h"
+#include "stepper.h"
+#include "drv8711.h"
+#include "estop.h"
+#include "gcode_state.h"
+#include "axis.h"
+#include "util.h"
+
+#include "plan/runtime.h"
+#include "plan/calibrate.h"
+
+#include <avr/interrupt.h>
+#include <avr/pgmspace.h>
+
+#include <string.h>
+#include <math.h>
+#include <stdio.h>
+#include <stdlib.h>
+
+
+typedef enum {
+ MOTOR_IDLE, // motor stopped and may be partially energized
+ MOTOR_ENERGIZING,
+ MOTOR_ACTIVE
+} motor_power_state_t;
+
+
+typedef struct {
+ // Config
+ uint8_t axis; // map motor to axis
+ uint16_t microsteps; // microsteps per full step
+ bool reverse;
+ motor_power_mode_t power_mode;
+ float step_angle; // degrees per whole step
+ float travel_rev; // mm or deg of travel per motor revolution
+ uint8_t step_pin;
+ uint8_t dir_pin;
+ TC0_t *timer;
+ DMA_CH_t *dma;
+ uint8_t dma_trigger;
+
+ // Runtime state
+ motor_power_state_t power_state; // state machine for managing motor power
+ uint32_t timeout;
+ motor_flags_t flags;
+ int32_t encoder;
+ int32_t commanded;
+ int32_t steps; // Currently used by the x-axis only
+ uint8_t last_clock;
+ bool wasEnabled;
+ int32_t error;
+ bool last_negative; // Last step sign
+ bool reading;
+
+ // Move prep
+ uint8_t timer_clock; // clock divisor setting or zero for off
+ uint16_t timer_period; // clock period counter
+ bool negative; // step sign
+ direction_t direction; // travel direction corrected for polarity
+ int32_t position;
+ bool round_up; // toggle rounding direction
+ float power;
+} motor_t;
+
+
+static motor_t motors[MOTORS] = {
+ {
+ .axis = M1_AXIS,
+ .step_angle = M1_STEP_ANGLE,
+ .travel_rev = M1_TRAVEL_PER_REV,
+ .microsteps = M1_MICROSTEPS,
+ .reverse = M1_REVERSE,
+ .power_mode = M1_POWER_MODE,
+ .step_pin = STEP_X_PIN,
+ .dir_pin = DIR_X_PIN,
+ .timer = &M1_TIMER,
+ .dma = &M1_DMA_CH,
+ .dma_trigger = M1_DMA_TRIGGER,
+ }, {
+ .axis = M2_AXIS,
+ .step_angle = M2_STEP_ANGLE,
+ .travel_rev = M2_TRAVEL_PER_REV,
+ .microsteps = M2_MICROSTEPS,
+ .reverse = M2_REVERSE,
+ .power_mode = M2_POWER_MODE,
+ .step_pin = STEP_Y_PIN,
+ .dir_pin = DIR_Y_PIN,
+ .timer = &M2_TIMER,
+ .dma = &M2_DMA_CH,
+ .dma_trigger = M2_DMA_TRIGGER,
+ }, {
+ .axis = M3_AXIS,
+ .step_angle = M3_STEP_ANGLE,
+ .travel_rev = M3_TRAVEL_PER_REV,
+ .microsteps = M3_MICROSTEPS,
+ .reverse = M3_REVERSE,
+ .power_mode = M3_POWER_MODE,
+ .step_pin = STEP_Z_PIN,
+ .dir_pin = DIR_Z_PIN,
+ .timer = &M3_TIMER,
+ .dma = &M3_DMA_CH,
+ .dma_trigger = M3_DMA_TRIGGER,
+ }, {
+ .axis = M4_AXIS,
+ .step_angle = M4_STEP_ANGLE,
+ .travel_rev = M4_TRAVEL_PER_REV,
+ .microsteps = M4_MICROSTEPS,
+ .reverse = M4_REVERSE,
+ .power_mode = M4_POWER_MODE,
+ .step_pin = STEP_A_PIN,
+ .dir_pin = DIR_A_PIN,
+ .timer = (TC0_t *)&M4_TIMER,
+ .dma = &M4_DMA_CH,
+ .dma_trigger = M4_DMA_TRIGGER,
+ }
+};
+
+
+static uint8_t _dummy;
+
+
+void motor_init() {
+ // Enable DMA
+ DMA.CTRL = DMA_RESET_bm;
+ DMA.CTRL = DMA_ENABLE_bm;
+ DMA.INTFLAGS = 0xff; // clear all interrupts
+
+ // Motor enable
+ OUTSET_PIN(MOTOR_ENABLE_PIN); // Low (disabled)
+ DIRSET_PIN(MOTOR_ENABLE_PIN); // Output
+
+ for (int motor = 0; motor < MOTORS; motor++) {
+ motor_t *m = &motors[motor];
+
+ axis_set_motor(m->axis, motor);
+
+ // IO pins
+ DIRSET_PIN(m->step_pin); // Output
+ DIRSET_PIN(m->dir_pin); // Output
+
+ // Setup motor timer
+ m->timer->CTRLB = TC_WGMODE_FRQ_gc | TC1_CCAEN_bm;
+
+ // Setup DMA channel as timer event counter
+ m->dma->ADDRCTRL = DMA_CH_SRCDIR_FIXED_gc | DMA_CH_DESTDIR_FIXED_gc;
+ m->dma->TRIGSRC = m->dma_trigger;
+ m->dma->REPCNT = 0;
+
+ // Note, the DMA transfer must read CCA to clear the trigger
+ m->dma->SRCADDR0 = (((uintptr_t)&m->timer->CCA) >> 0) & 0xff;
+ m->dma->SRCADDR1 = (((uintptr_t)&m->timer->CCA) >> 8) & 0xff;
+ m->dma->SRCADDR2 = 0;
+
+ m->dma->DESTADDR0 = (((uintptr_t)&_dummy) >> 0) & 0xff;
+ m->dma->DESTADDR1 = (((uintptr_t)&_dummy) >> 8) & 0xff;
+ m->dma->DESTADDR2 = 0;
+
+ m->dma->CTRLB = 0;
+ m->dma->CTRLA =
+ DMA_CH_REPEAT_bm | DMA_CH_SINGLE_bm | DMA_CH_BURSTLEN_1BYTE_gc;
+ m->dma->CTRLA |= DMA_CH_ENABLE_bm;
+
+ drv8711_set_microsteps(motor, m->microsteps);
+ }
+}
+
+
+void motor_enable(int motor, bool enable) {
+ if (enable) OUTSET_PIN(MOTOR_ENABLE_PIN); // Active high
+ else {
+ OUTCLR_PIN(MOTOR_ENABLE_PIN);
+ motors[motor].power_state = MOTOR_IDLE;
+ }
+}
+
+
+bool motor_is_enabled(int motor) {
+ return motors[motor].power_mode != MOTOR_DISABLED;
+}
+
+
+int motor_get_axis(int motor) {return motors[motor].axis;}
+
+
+void motor_set_stall_callback(int motor, stall_callback_t cb) {
+ drv8711_set_stall_callback(motor, cb);
+}
+
+
+/// @return computed homing velocity
+float motor_get_stall_homing_velocity(int motor) {
+ // Compute velocity:
+ // velocity = travel_rev * step_angle * 60 / (SMPLTH * mstep * 360 * 2)
+ // SMPLTH = 50us = 0.00005s
+ // mstep = 8
+ return motors[motor].travel_rev * motors[motor].step_angle * 1667 /
+ motors[motor].microsteps;
+}
+
+
+float motor_get_steps_per_unit(int motor) {
+ return 360 * motors[motor].microsteps / motors[motor].travel_rev /
+ motors[motor].step_angle;
+}
+
+
+float motor_get_units_per_step(int motor) {
+ return motors[motor].travel_rev * motors[motor].step_angle /
+ motors[motor].microsteps / 360;
+}
+
+
+float _get_max_velocity(int motor) {
+ return
+ axis_get_velocity_max(motors[motor].axis) * motor_get_steps_per_unit(motor);
+}
+
+
+uint16_t motor_get_microsteps(int motor) {return motors[motor].microsteps;}
+
+
+void motor_set_microsteps(int motor, uint16_t microsteps) {
+ switch (microsteps) {
+ case 1: case 2: case 4: case 8: case 16: case 32: case 64: case 128: case 256:
+ break;
+ default: return;
+ }
+
+ motors[motor].microsteps = microsteps;
+ drv8711_set_microsteps(motor, microsteps);
+}
+
+
+int32_t motor_get_encoder(int motor) {return motors[motor].encoder;}
+
+
+void motor_set_encoder(int motor, float encoder) {
+ //if (st_is_busy()) ALARM(STAT_INTERNAL_ERROR); TODO
+
+ motor_t *m = &motors[motor];
+ m->encoder = m->position = m->commanded = round(encoder);
+ m->error = 0;
+}
+
+
+int32_t motor_get_error(int motor) {
+ motor_t *m = &motors[motor];
+
+ m->reading = true;
+ int32_t error = motors[motor].error;
+ m->reading = false;
+
+ return error;
+}
+
+
+int32_t motor_get_position(int motor) {return motors[motor].position;}
+
+
+/// returns true if motor is in an error state
+bool motor_error(int m) {
+ uint8_t flags = motors[m].flags;
+ if (calibrate_busy()) flags &= ~MOTOR_FLAG_STALLED_bm;
+ return flags & MOTOR_FLAG_ERROR_bm;
+}
+
+
+bool motor_stalled(int m) {
+ return motors[m].flags & MOTOR_FLAG_STALLED_bm;
+}
+
+
+void motor_reset(int m) {
+ motors[m].flags = 0;
+}
+
+
+/// Remove power from a motor
+static void _deenergize(int m) {
+ if (motors[m].power_state == MOTOR_ACTIVE) {
+ motors[m].power_state = MOTOR_IDLE;
+ drv8711_disable(m);
+ }
+}
+
+
+/// Apply power to a motor
+static void _energize(int m) {
+ if (motors[m].power_state == MOTOR_IDLE && !motor_error(m)) {
+ motors[m].power_state = MOTOR_ENERGIZING;
+ drv8711_enable(m);
+
+ motor_driver_callback(m); // TODO Shouldn't call this directly
+ }
+
+ // Reset timeout, regardless
+ motors[m].timeout = rtc_get_time() + MOTOR_IDLE_TIMEOUT * 1000;
+}
+
+
+bool motor_energizing() {
+ for (int m = 0; m < MOTORS; m++)
+ if (motors[m].power_state == MOTOR_ENERGIZING)
+ return true;
+
+ return false;
+}
+
+
+void motor_driver_callback(int motor) {
+ motor_t *m = &motors[motor];
+
+ if (m->power_state == MOTOR_IDLE) m->flags &= ~MOTOR_FLAG_ENABLED_bm;
+ else if (!estop_triggered()) {
+ m->power_state = MOTOR_ACTIVE;
+ m->flags |= MOTOR_FLAG_ENABLED_bm;
+ motor_enable(motor, true);
+ }
+
+ report_request();
+}
+
+
+/// Callback to manage motor power sequencing and power-down timing.
+stat_t motor_rtc_callback() { // called by controller
+ for (int motor = 0; motor < MOTORS; motor++)
+ // Deenergize motor if disabled or timedout
+ if (motors[motor].power_mode == MOTOR_DISABLED ||
+ rtc_expired(motors[motor].timeout)) _deenergize(motor);
+
+ return STAT_OK;
+}
+
+
+void motor_error_callback(int motor, motor_flags_t errors) {
+ motor_t *m = &motors[motor];
+
+ if (m->power_state != MOTOR_ACTIVE) return;
+
+ m->flags |= errors & MOTOR_FLAG_ERROR_bm;
+ report_request();
+
+ if (motor_error(motor)) {
+ if (m->flags & MOTOR_FLAG_STALLED_bm) ALARM(STAT_MOTOR_STALLED);
+ if (m->flags & MOTOR_FLAG_OVER_TEMP_bm) ALARM(STAT_MOTOR_OVER_TEMP);
+ if (m->flags & MOTOR_FLAG_OVER_CURRENT_bm) ALARM(STAT_MOTOR_OVER_CURRENT);
+ if (m->flags & MOTOR_FLAG_DRIVER_FAULT_bm) ALARM(STAT_MOTOR_DRIVER_FAULT);
+ if (m->flags & MOTOR_FLAG_UNDER_VOLTAGE_bm) ALARM(STAT_MOTOR_UNDER_VOLTAGE);
+ }
+}
+
+
+void motor_load_move(int motor) {
+ motor_t *m = &motors[motor];
+
+ // Get actual step count from DMA channel
+ uint16_t steps = 0xffff - m->dma->TRFCNT;
+ m->dma->TRFCNT = 0xffff; // Reset DMA channel counter
+ m->dma->CTRLB = DMA_CH_CHBUSY_bm | DMA_CH_CHPEND_bm;
+ m->dma->CTRLA |= DMA_CH_ENABLE_bm;
+
+ // Adjust clock count
+ if (m->last_clock) {
+ uint32_t count = m->timer->CNT;
+ int8_t freq_change = m->last_clock - m->timer_clock;
+
+ count <<= freq_change; // Adjust count
+
+ if (m->timer_period < count) count -= m->timer_period;
+ if (m->timer_period < count) count -= m->timer_period;
+ if (m->timer_period < count) count = m->timer_period / 2;
+
+ m->timer->CNT = count;
+
+ } else m->timer->CNT = m->timer_period / 2;
+
+ // Set or zero runtime clock and period
+ m->timer->CCA = m->timer_period; // Set frequency
+ m->timer->CTRLA = m->timer_clock; // Start or stop
+ m->last_clock = m->timer_clock;
+ m->timer_clock = 0; // Clear clock
+
+ // Set direction
+ if (m->direction == DIRECTION_CW) OUTCLR_PIN(m->dir_pin);
+ else OUTSET_PIN(m->dir_pin);
+
+ // Accumulate encoder
+ if (!m->wasEnabled) steps = 0;
+
+ m->encoder += m->last_negative ? -(int32_t)steps : steps;
+ m->last_negative = m->negative;
+
+ // Compute error
+ if (!m->reading) m->error = m->commanded - m->encoder;
+ m->commanded = m->position;
+
+ // Set power
+ drv8711_set_power(motor, m->power);
+}
+
+
+void motor_end_move(int motor) {
+ motor_t *m = &motors[motor];
+ m->wasEnabled = m->dma->CTRLA & DMA_CH_ENABLE_bm;
+ m->dma->CTRLA &= ~DMA_CH_ENABLE_bm;
+ m->timer->CTRLA = 0; // Stop clock
+}
+
+
+stat_t motor_prep_move(int motor, int32_t clocks, float target, int32_t error,
+ float time) {
+ motor_t *m = &motors[motor];
+
+ // Validate input
+ if (motor < 0 || MOTORS < motor) return ALARM(STAT_INTERNAL_ERROR);
+ if (clocks < 0) return ALARM(STAT_INTERNAL_ERROR);
+ if (isinf(target)) return ALARM(STAT_MOVE_TARGET_INFINITE);
+ if (isnan(target)) return ALARM(STAT_MOVE_TARGET_NAN);
+
+ // Compute motor timer clock and period. Rounding is performed to eliminate
+ // a negative bias in the uint32_t conversion that results in long-term
+ // negative drift.
+ int32_t travel = round(target) - m->position + error;
+ uint32_t ticks_per_step = travel ? labs(clocks / 2 / travel) : 0;
+ m->position = round(target);
+
+ // Setup the direction, compensating for polarity.
+ m->negative = travel < 0;
+ if (m->negative) m->direction = DIRECTION_CCW ^ m->reverse;
+ else m->direction = DIRECTION_CW ^ m->reverse;
+
+ // Find the clock rate that will fit the required number of steps
+ if (ticks_per_step <= 0xffff) m->timer_clock = TC_CLKSEL_DIV1_gc;
+ else if (ticks_per_step <= 0x1ffff) m->timer_clock = TC_CLKSEL_DIV2_gc;
+ else if (ticks_per_step <= 0x3ffff) m->timer_clock = TC_CLKSEL_DIV4_gc;
+ else if (ticks_per_step <= 0x7ffff) m->timer_clock = TC_CLKSEL_DIV8_gc;
+ else m->timer_clock = 0; // Clock off, too slow
+
+ // Note, we rely on the fact that TC_CLKSEL_DIV1_gc through TC_CLKSEL_DIV8_gc
+ // equal 1, 2, 3 & 4 respectively.
+ m->timer_period = ticks_per_step >> (m->timer_clock - 1);
+
+ // Round up if DIV4 or DIV8 and the error is high enough
+ if (0xffff < ticks_per_step && m->timer_period < 0xffff) {
+ int8_t step_error = ticks_per_step & ((1 << (m->timer_clock - 1)) - 1);
+ int8_t half_error = 1 << (m->timer_clock - 2);
+
+ if (step_error == half_error) {
+ if (m->round_up) m->timer_period++;
+ m->round_up = !m->round_up;
+
+ } else if (half_error < step_error) m->timer_period++;
+ }
+
+ if (!m->timer_period) m->timer_clock = 0;
+ if (!m->timer_clock) m->timer_period = 0;
+
+ // Power motor
+ switch (m->power_mode) {
+ case MOTOR_DISABLED: break;
+
+ case MOTOR_POWERED_ONLY_WHEN_MOVING:
+ if (!m->timer_clock) break; // Not moving
+ // Fall through
+
+ case MOTOR_ALWAYS_POWERED: case MOTOR_POWERED_IN_CYCLE:
+ _energize(motor); // TODO is ~5ms enough time to enable the motor?
+ break;
+
+ default: break; // Shouldn't get here
+ }
+
+ // Compute power from axis velocity
+ m->power = travel / (_get_max_velocity(motor) * time);
+
+ return STAT_OK;
+}
+
+
+// Var callbacks
+char get_axis_name(int motor) {return axis_get_char(motors[motor].axis);}
+
+
+void set_axis_name(int motor, char axis) {
+ int id = axis_get_id(axis);
+ if (id != -1) motors[motor].axis = id;
+}
+
+
+float get_step_angle(int motor) {return motors[motor].step_angle;}
+void set_step_angle(int motor, float value) {motors[motor].step_angle = value;}
+float get_travel(int motor) {return motors[motor].travel_rev;}
+void set_travel(int motor, float value) {motors[motor].travel_rev = value;}
+uint16_t get_microstep(int motor) {return motors[motor].microsteps;}
+
+
+void set_microstep(int motor, uint16_t value) {
+ if (motor < 0 || MOTORS <= motor) return;
+ motor_set_microsteps(motor, value);
+}
+
+
+bool get_reverse(int motor) {
+ if (motor < 0 || MOTORS <= motor) return 0;
+ return motors[motor].reverse;
+}
+
+
+void set_reverse(int motor, bool value) {motors[motor].reverse = value;}
+uint8_t get_motor_axis(int motor) {return motors[motor].axis;}
+
+
+void set_motor_axis(int motor, uint8_t value) {
+ if (MOTORS <= motor || AXES <= value || value == motors[motor].axis) return;
+ axis_set_motor(motors[motor].axis, -1);
+ motors[motor].axis = value;
+ axis_set_motor(value, motor);
+}
+
+
+uint8_t get_power_mode(int motor) {return motors[motor].power_mode;}
+
+
+void set_power_mode(int motor, uint8_t value) {
+ if (value < MOTOR_POWER_MODE_MAX_VALUE)
+ motors[motor].power_mode = value;
+}
+
+
+uint8_t get_status_flags(int motor) {return motors[motor].flags;}
+
+
+void print_status_flags(uint8_t flags) {
+ bool first = true;
+
+ putchar('"');
+
+ if (MOTOR_FLAG_ENABLED_bm & flags) {
+ printf_P(PSTR("enable"));
+ first = false;
+ }
+
+ if (MOTOR_FLAG_STALLED_bm & flags) {
+ if (!first) printf_P(PSTR(", "));
+ printf_P(PSTR("stall"));
+ first = false;
+ }
+
+ if (MOTOR_FLAG_OVER_TEMP_bm & flags) {
+ if (!first) printf_P(PSTR(", "));
+ printf_P(PSTR("over temp"));
+ first = false;
+ }
+
+ if (MOTOR_FLAG_OVER_CURRENT_bm & flags) {
+ if (!first) printf_P(PSTR(", "));
+ printf_P(PSTR("over current"));
+ first = false;
+ }
+
+ if (MOTOR_FLAG_DRIVER_FAULT_bm & flags) {
+ if (!first) printf_P(PSTR(", "));
+ printf_P(PSTR("fault"));
+ first = false;
+ }
+
+ if (MOTOR_FLAG_UNDER_VOLTAGE_bm & flags) {
+ if (!first) printf_P(PSTR(", "));
+ printf_P(PSTR("uvlo"));
+ first = false;
+ }
+
+ putchar('"');
+}
+
+
+uint8_t get_status_strings(int m) {return get_status_flags(m);}
+
+
+// Command callback
+void command_mreset(int argc, char *argv[]) {
+ if (argc == 1)
+ for (int m = 0; m < MOTORS; m++)
+ motor_reset(m);
+
+ else {
+ int m = atoi(argv[1]);
+ if (m < MOTORS) motor_reset(m);
+ }
+
+ report_request();
+}
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2016 Buildbotics LLC
+ All rights reserved.
+
+ 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/>.
+
+ The software is distributed in the hope that it will be useful, but
+ WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public
+ License along with the software. If not, see
+ <http://www.gnu.org/licenses/>.
+
+ For information regarding this software email:
+ "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#pragma once
+
+#include "status.h"
+
+#include <stdint.h>
+#include <stdbool.h>
+
+
+typedef enum {
+ MOTOR_FLAG_ENABLED_bm = 1 << 0,
+ MOTOR_FLAG_STALLED_bm = 1 << 1,
+ MOTOR_FLAG_OVER_TEMP_bm = 1 << 2,
+ MOTOR_FLAG_OVER_CURRENT_bm = 1 << 3,
+ MOTOR_FLAG_DRIVER_FAULT_bm = 1 << 4,
+ MOTOR_FLAG_UNDER_VOLTAGE_bm = 1 << 5,
+ MOTOR_FLAG_ERROR_bm = (//MOTOR_FLAG_STALLED_bm |
+ MOTOR_FLAG_OVER_TEMP_bm |
+ MOTOR_FLAG_OVER_CURRENT_bm |
+ MOTOR_FLAG_DRIVER_FAULT_bm |
+ MOTOR_FLAG_UNDER_VOLTAGE_bm)
+} motor_flags_t;
+
+
+typedef enum {
+ MOTOR_DISABLED, // 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, // idles shortly after stopped, even in cycle
+ MOTOR_POWER_MODE_MAX_VALUE // for input range checking
+} motor_power_mode_t;
+
+
+typedef void (*stall_callback_t)(int motor);
+
+
+void motor_init();
+void motor_enable(int motor, bool enable);
+
+bool motor_is_enabled(int motor);
+int motor_get_axis(int motor);
+void motor_set_stall_callback(int motor, stall_callback_t cb);
+float motor_get_stall_homing_velocity(int motor);
+float motor_get_steps_per_unit(int motor);
+float motor_get_units_per_step(int motor);
+uint16_t motor_get_microsteps(int motor);
+void motor_set_microsteps(int motor, uint16_t microsteps);
+int32_t motor_get_encoder(int motor);
+void motor_set_encoder(int motor, float encoder);
+int32_t motor_get_error(int motor);
+int32_t motor_get_position(int motor);
+
+bool motor_error(int motor);
+bool motor_stalled(int motor);
+void motor_reset(int motor);
+
+bool motor_energizing();
+
+void motor_driver_callback(int motor);
+stat_t motor_rtc_callback();
+void motor_error_callback(int motor, motor_flags_t errors);
+
+void motor_load_move(int motor);
+void motor_end_move(int motor);
+stat_t motor_prep_move(int motor, int32_t clocks, float target, int32_t error,
+ float time);
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2016 Buildbotics LLC
+ All rights reserved.
+
+ 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/>.
+
+ The software is distributed in the hope that it will be useful, but
+ WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public
+ License along with the software. If not, see
+ <http://www.gnu.org/licenses/>.
+
+ For information regarding this software email:
+ "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#include "pins.h"
+
+
+PORT_t *pin_ports[] = {&PORTA, &PORTB, &PORTC, &PORTD, &PORTE, &PORTF};
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2016 Buildbotics LLC
+ All rights reserved.
+
+ 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/>.
+
+ The software is distributed in the hope that it will be useful, but
+ WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public
+ License along with the software. If not, see
+ <http://www.gnu.org/licenses/>.
+
+ For information regarding this software email:
+ "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#pragma once
+
+#include <avr/io.h>
+
+
+enum {PORT_A = 1, PORT_B, PORT_C, PORT_D, PORT_E, PORT_F};
+
+extern PORT_t *pin_ports[];
+
+#define PORT(PIN) pin_ports[(PIN >> 3) - 1]
+#define BM(PIN) (1 << (PIN & 7))
+
+#define DIRSET_PIN(PIN) PORT(PIN)->DIRSET = BM(PIN)
+#define DIRCLR_PIN(PIN) PORT(PIN)->DIRCLR = BM(PIN)
+#define OUTCLR_PIN(PIN) PORT(PIN)->OUTCLR = BM(PIN)
+#define OUTSET_PIN(PIN) PORT(PIN)->OUTSET = BM(PIN)
+#define OUTTGL_PIN(PIN) PORT(PIN)->OUTTGL = BM(PIN)
+#define OUT_PIN(PIN) (!!(PORT(PIN)->OUT & BM(PIN)))
+#define IN_PIN(PIN) (!!(PORT(PIN)->IN & BM(PIN)))
+#define PINCTRL_PIN(PIN) ((&PORT(PIN)->PIN0CTRL)[PIN & 7])
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2016 Buildbotics LLC
+ Copyright (c) 2010 - 2015 Alden S. Hart, Jr.
+ All rights reserved.
+
+ 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/>.
+
+ The software is distributed in the hope that it will be useful, but
+ WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public
+ License along with the software. If not, see
+ <http://www.gnu.org/licenses/>.
+
+ For information regarding this software email:
+ "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+/* This module actually contains some parts that belong in the machine, and
+ * other parts that belong at the motion planner level, but the whole thing is
+ * treated as if it were part of the motion planner.
+ */
+
+#include "arc.h"
+
+#include "axis.h"
+#include "buffer.h"
+#include "line.h"
+#include "gcode_parser.h"
+#include "config.h"
+#include "util.h"
+
+#include <math.h>
+#include <stdbool.h>
+#include <string.h>
+
+
+typedef struct {
+ bool running;
+
+ float target[AXES]; // XYZABC where the move should go
+ float position[AXES]; // end point of the current segment
+
+ float theta; // total angle specified by arc
+ float radius; // Raw R value, or computed via offsets
+
+ uint8_t plane_axis_0; // arc plane axis 0 - e.g. X for G17
+ uint8_t plane_axis_1; // arc plane axis 1 - e.g. Y for G17
+ uint8_t linear_axis; // linear axis (normal to plane)
+
+ uint32_t segments; // count of running segments
+ float segment_theta; // angular motion per segment
+ float segment_linear_travel; // linear motion per segment
+ float center_0; // center at axis 0 (e.g. X for G17)
+ float center_1; // center at axis 1 (e.g. Y for G17)
+} arc_t;
+
+arc_t arc = {0};
+
+
+/*** Returns a naive estimate of arc execution time to inform segment
+ * calculation. The arc time is computed not to exceed the time taken
+ * in the slowest dimension in the arc plane or in linear
+ * travel. Maximum feed rates are compared in each dimension, but the
+ * comparison assumes that the arc will have at least one segment
+ * where the unit vector is 1 in that dimension. This is not true for
+ * any arbitrary arc, with the result that the time returned may be
+ * less than optimal.
+ */
+static float _estimate_arc_time(float length, float linear_travel,
+ float planar_travel) {
+ // Determine move time at requested feed rate
+ // Inverse feed rate is normalized to minutes
+ float time = mach_is_inverse_time_mode() ?
+ mach_get_feed_rate() : length / mach_get_feed_rate();
+
+
+ // Apply feed override
+ time /= mach_get_feed_override();
+
+ // Downgrade the time if there is a rate-limiting axis
+ return max4(time, planar_travel / axis_get_feedrate_max(arc.plane_axis_0),
+ planar_travel / axis_get_feedrate_max(arc.plane_axis_1),
+ fabs(linear_travel) / axis_get_feedrate_max(arc.linear_axis));
+}
+
+
+/*** Compute arc center (offset) from radius.
+ *
+ * Needs to calculate the center of the circle that has the designated radius
+ * and passes through both the current position and the target position
+ *
+ * This method calculates the following set of equations where:
+ *
+ * [x,y] is the vector from current to target position,
+ * d == magnitude of that vector,
+ * h == hypotenuse of the triangle formed by the radius of the circle,
+ * the distance to the center of the travel vector.
+ *
+ * A vector perpendicular to the travel vector [-y,x] is scaled to the length
+ * of h [-y/d*h, x/d*h] and added to the center of the travel vector [x/2,y/2]
+ * to form the new point [i,j] at [x/2-y/d*h, y/2+x/d*h] which will be the
+ * center of the arc.
+ *
+ * d^2 == x^2 + y^2
+ * h^2 == r^2 - (d/2)^2
+ * i == x/2 - y/d*h
+ * j == y/2 + x/d*h
+ * O <- [i,j]
+ * - |
+ * r - |
+ * - |
+ * - | h
+ * - |
+ * [0,0] -> C -----------------+--------------- T <- [x,y]
+ * | <------ d/2 ---->|
+ *
+ * C - Current position
+ * T - Target position
+ * O - center of circle that pass through both C and T
+ * d - distance from C to T
+ * r - designated radius
+ * h - distance from center of CT to O
+ *
+ * Expanding the equations:
+ *
+ * d -> sqrt(x^2 + y^2)
+ * h -> sqrt(4 * r^2 - x^2 - y^2)/2
+ * i -> (x - (y * sqrt(4 * r^2 - x^2 - y^2)) / sqrt(x^2 + y^2)) / 2
+ * j -> (y + (x * sqrt(4 * r^2 - x^2 - y^2)) / sqrt(x^2 + y^2)) / 2
+ *
+ * Which can be written:
+ *
+ * i -> (x - (y * sqrt(4 * r^2 - x^2 - y^2))/sqrt(x^2 + y^2))/2
+ * j -> (y + (x * sqrt(4 * r^2 - x^2 - y^2))/sqrt(x^2 + y^2))/2
+ *
+ * Which we for size and speed reasons optimize to:
+ *
+ * h_x2_div_d = sqrt(4 * r^2 - x^2 - y^2)/sqrt(x^2 + y^2)
+ * i = (x - (y * h_x2_div_d))/2
+ * j = (y + (x * h_x2_div_d))/2
+ *
+ * Computing clockwise vs counter-clockwise motion
+ *
+ * The counter clockwise circle lies to the left of the target direction.
+ * When offset is positive the left hand circle will be generated -
+ * when it is negative the right hand circle is generated.
+ *
+ * T <-- Target position
+ * ^
+ * Clockwise circles with | Clockwise circles with
+ * this center will have | this center will have
+ * > 180 deg of angular travel | < 180 deg of angular travel,
+ * \ | which is a good thing!
+ * \ | /
+ * center of arc when -> x <----- | -----> x <- center of arc when
+ * h_x2_div_d is positive | h_x2_div_d is negative
+ * |
+ * C <-- Current position
+ *
+ *
+ * Assumes arc singleton has been pre-loaded with target and position.
+ * Parts of this routine were originally sourced from the grbl project.
+ */
+static stat_t _compute_arc_offsets_from_radius(float offset[], bool clockwise) {
+ // Calculate the change in position along each selected axis
+ float x =
+ arc.target[arc.plane_axis_0] - mach_get_axis_position(arc.plane_axis_0);
+ float y =
+ arc.target[arc.plane_axis_1] - mach_get_axis_position(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 negative indicating that the arc is offset into the complex plane
+ // beyond the reach of any real CNC. However, numerical errors can flip the
+ // sign of disc as it approaches zero (which happens as the arc angle
+ // approaches 180 degrees). To avoid mishandling these arcs we use the
+ // closest real solution (which will be 0 when disc <= 0). This 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 h_x2_div_d = disc > 0 ? -sqrt(disc) / hypotf(x, y) : 0;
+
+ // Invert sign of h_x2_div_d if circle is counter clockwise (see header notes)
+ if (!clockwise) h_x2_div_d = -h_x2_div_d;
+
+ // Negative R is g-code-alese for "I want a circle with more than 180 degrees
+ // of travel" (go figure!), even though it is advised against ever generating
+ // such circles in a single line of g-code. By inverting the sign of
+ // h_x2_div_d the center of the circles is placed on the opposite side of
+ // the line of travel and thus we get the unadvisably long arcs as prescribed.
+ if (arc.radius < 0) h_x2_div_d = -h_x2_div_d;
+
+ // Complete the operation by calculating the actual center of the arc
+ offset[arc.plane_axis_0] = (x - y * h_x2_div_d) / 2;
+ offset[arc.plane_axis_1] = (y + x * h_x2_div_d) / 2;
+ offset[arc.linear_axis] = 0;
+
+ return STAT_OK;
+}
+
+
+/* Compute arc from I and J (arc center point)
+ *
+ * The theta calculation sets up an clockwise or counterclockwise arc
+ * from the current position to the target position around the center
+ * designated by the offset vector. All theta-values measured in
+ * radians of deviance from the positive y-axis.
+ *
+ * | <- theta == 0
+ * * * *
+ * * *
+ * * *
+ * * O ----T <- theta_end (e.g. 90 degrees: theta_end == PI/2)
+ * * /
+ * C <- theta_start (e.g. -145 degrees: theta_start == -PI*(3/4))
+ *
+ * Parts of this routine were originally sourced from the grbl project.
+ */
+static stat_t _compute_arc(bool radius_f, const float position[],
+ float offset[], float rotations, bool clockwise,
+ bool full_circle) {
+ // Compute radius. A non-zero radius value indicates a radius arc
+ if (radius_f) _compute_arc_offsets_from_radius(offset, clockwise);
+ else // compute start radius
+ arc.radius = hypotf(-offset[arc.plane_axis_0], -offset[arc.plane_axis_1]);
+
+ // Test arc specification for correctness according to:
+ // http://linuxcnc.org/docs/html/gcode/gcode.html#sec:G2-G3-Arc
+ // "It is an error if: when the arc is projected on the selected
+ // plane, the distance from the current point to the center differs
+ // from the distance from the end point to the 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.target[arc.plane_axis_0] -
+ position[arc.plane_axis_0] - offset[arc.plane_axis_0];
+
+ float end_1 = arc.target[arc.plane_axis_1] -
+ position[arc.plane_axis_1] - offset[arc.plane_axis_1];
+
+ // end radius - start radius
+ float err = fabs(hypotf(end_0, end_1) - arc.radius);
+
+ if (err > ARC_RADIUS_ERROR_MAX ||
+ (err < ARC_RADIUS_ERROR_MIN && err > arc.radius * ARC_RADIUS_TOLERANCE))
+ return STAT_ARC_SPECIFICATION_ERROR;
+
+ // Calculate the theta (angle) of the current point (position)
+ // arc.theta is angular starting point for the arc (also needed later for
+ // calculating center point)
+ arc.theta = atan2(-offset[arc.plane_axis_0], -offset[arc.plane_axis_1]);
+
+ // g18_correction is used to invert G18 XZ plane arcs for proper CW
+ // orientation
+ float g18_correction = mach_get_plane() == PLANE_XZ ? -1 : 1;
+ float angular_travel = 0;
+
+ if (full_circle) {
+ // angular travel always starts as zero for full circles
+ angular_travel = 0;
+
+ // handle the valid case of a full circle arc w/ P=0
+ if (fp_ZERO(rotations)) rotations = 1.0;
+
+ } else {
+ float theta_end = atan2(end_0, end_1);
+
+ // Compute the angular travel
+ if (fp_EQ(theta_end, arc.theta))
+ // very large radii arcs can have zero angular travel (thanks PartKam)
+ angular_travel = 0;
+
+ else {
+ // make the difference positive so we have clockwise travel
+ if (theta_end < arc.theta) theta_end += 2 * M_PI * g18_correction;
+
+ // compute positive angular travel
+ angular_travel = theta_end - arc.theta;
+
+ // reverse travel direction if it's CCW arc
+ if (!clockwise) angular_travel -= 2 * M_PI * g18_correction;
+ }
+ }
+
+ // Add in travel for rotations
+ if (clockwise) angular_travel += 2 * M_PI * rotations * g18_correction;
+ else angular_travel -= 2 * M_PI * rotations * g18_correction;
+
+ // Calculate travel in the depth axis of the helix and compute the time it
+ // should take to perform the move length is the total mm of travel of
+ // the helix (or just a planar arc)
+ float linear_travel = arc.target[arc.linear_axis] - position[arc.linear_axis];
+ float planar_travel = angular_travel * arc.radius;
+ // hypot is insensitive to +/- signs
+ float length = hypotf(planar_travel, linear_travel);
+
+ // trap zero length arcs that _compute_arc can throw
+ if (fp_ZERO(length)) return STAT_MINIMUM_LENGTH_MOVE;
+
+ // get an estimate of execution time to inform segment calculation
+ float arc_time = _estimate_arc_time(length, linear_travel, planar_travel);
+
+ // Find the minimum number of segments that meets these constraints...
+ float segments_for_chordal_accuracy =
+ length / sqrt(4 * CHORDAL_TOLERANCE * (2 * arc.radius - CHORDAL_TOLERANCE));
+ float segments_for_minimum_distance = length / ARC_SEGMENT_LENGTH;
+ float segments_for_minimum_time =
+ arc_time * MICROSECONDS_PER_MINUTE / MIN_ARC_SEGMENT_USEC;
+
+ float segments = floor(min3(segments_for_chordal_accuracy,
+ segments_for_minimum_distance,
+ segments_for_minimum_time));
+ if (segments < 1) segments = 1; // at least 1 segment
+
+ arc.segments = (uint32_t)segments;
+ arc.segment_theta = angular_travel / segments;
+ arc.segment_linear_travel = linear_travel / segments;
+ arc.center_0 = position[arc.plane_axis_0] - sin(arc.theta) * arc.radius;
+ arc.center_1 = position[arc.plane_axis_1] - cos(arc.theta) * arc.radius;
+
+ // initialize the linear position
+ arc.position[arc.linear_axis] = position[arc.linear_axis];
+
+ return STAT_OK;
+}
+
+
+/*** Machine entry point for arc
+ *
+ * Generates an arc by queuing line segments to the move buffer. The arc is
+ * approximated by generating a large number of tiny, linear segments.
+ */
+stat_t mach_arc_feed(float values[], bool values_f[], // arc endpoints
+ float offsets[], bool offsets_f[], // arc offsets
+ float radius, bool radius_f, // radius
+ float P, bool P_f, // parameter
+ bool modal_g1_f,
+ motion_mode_t motion_mode) { // defined motion mode
+
+ // 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 simple as "I25" if CW or CCW motion mode was
+ // already set by a previous block. Here are 2 cases to handle if CW or CCW
+ // motion mode was set by a previous block:
+ //
+ // Case 1: F, P or other non modal is specified but no movement is specified
+ // (no offsets or radius). This is OK: return STAT_OK
+ //
+ // Case 2: Movement is specified w/o a new G2 or G3 word in the (new) block.
+ // This is OK: continue the move
+ //
+ if (!modal_g1_f && !offsets_f[0] && !offsets_f[1] && !offsets_f[2] &&
+ !radius_f) return STAT_OK;
+
+ // trap missing feed rate
+ if (fp_ZERO(mach_get_feed_rate()) ||
+ (mach_is_inverse_time_mode() && !parser.gf.feed_rate))
+ return STAT_GCODE_FEEDRATE_NOT_SPECIFIED;
+
+ // radius must be positive and > minimum
+ if (radius_f && radius < MIN_ARC_RADIUS)
+ return STAT_ARC_RADIUS_OUT_OF_TOLERANCE;
+
+ // 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.
+ switch (mach_get_plane()) {
+ case PLANE_XY: // G17
+ arc.plane_axis_0 = AXIS_X;
+ arc.plane_axis_1 = AXIS_Y;
+ arc.linear_axis = AXIS_Z;
+ break;
+
+ case PLANE_XZ: // G18
+ arc.plane_axis_0 = AXIS_X;
+ arc.plane_axis_1 = AXIS_Z;
+ arc.linear_axis = AXIS_Y;
+ break;
+
+ case PLANE_YZ: // G19
+ arc.plane_axis_0 = AXIS_Y;
+ arc.plane_axis_1 = AXIS_Z;
+ arc.linear_axis = AXIS_X;
+ break;
+ }
+
+ bool clockwise = motion_mode == MOTION_MODE_CW_ARC;
+
+ // Test if endpoints are specified in the selected plane
+ bool full_circle = false;
+ if (!values_f[arc.plane_axis_0] && !values_f[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 full_circle = true; // in center format arc specifies full circle
+ }
+
+ // Test radius arcs for radius tolerance
+ if (radius_f) {
+ arc.radius = TO_MM(radius); // set to internal format (mm)
+ if (fabs(arc.radius) < MIN_ARC_RADIUS) // radius value must be > minimum
+ return STAT_ARC_RADIUS_OUT_OF_TOLERANCE;
+
+ // Test that center format absolute distance mode arcs have both offsets
+ } else if (mach_get_arc_distance_mode() == ABSOLUTE_MODE &&
+ !(offsets_f[arc.plane_axis_0] && offsets_f[arc.plane_axis_1]))
+ return STAT_ARC_OFFSETS_MISSING_FOR_PLANE;
+
+ // Set arc rotations
+ float rotations = 0;
+
+ if (P_f) {
+ // If P is present it must be a positive integer
+ if (P < 0 || 0 < floor(P) - P) return STAT_P_WORD_IS_NOT_A_POSITIVE_INTEGER;
+
+ rotations = P;
+
+ } else if (full_circle) rotations = 1; // default to 1 for full circles
+
+ // Set model target
+ const float *position = mach_get_position();
+ mach_calc_target(arc.target, values, values_f);
+
+ // in radius mode it's an error for start == end
+ if (radius_f && fp_EQ(position[AXIS_X], arc.target[AXIS_X]) &&
+ fp_EQ(position[AXIS_Y], arc.target[AXIS_Y]) &&
+ fp_EQ(position[AXIS_Z], arc.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
+ mach_set_motion_mode(motion_mode);
+ mach_update_work_offsets(); // Update resolved offsets
+ arc.radius = TO_MM(radius); // set arc radius or zero
+
+ float offset[3];
+ for (int i = 0; i < 3; i++) offset[i] = TO_MM(offsets[i]);
+
+ if (mach_get_arc_distance_mode() == ABSOLUTE_MODE) {
+ if (offsets_f[0]) offset[0] -= position[0];
+ if (offsets_f[1]) offset[1] -= position[1];
+ if (offsets_f[2]) offset[2] -= position[2];
+ }
+
+ // compute arc runtime values
+ RITORNO(_compute_arc
+ (radius_f, position, offset, rotations, clockwise, full_circle));
+
+ // Note, arc soft limits are not tested here
+
+ arc.running = true; // Enable arc run in callback
+ mach_arc_callback(); // Queue initial arc moves
+ mach_set_position(arc.target); // update model position
+
+ return STAT_OK;
+}
+
+
+/*** Generate an arc
+ *
+ * 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.
+ */
+void mach_arc_callback() {
+ while (arc.running && mp_queue_get_room()) {
+ if (arc.segments == 1) { // Final segment
+ arc.position[arc.plane_axis_0] = arc.target[arc.plane_axis_0];
+ arc.position[arc.plane_axis_1] = arc.target[arc.plane_axis_1];
+ arc.position[arc.linear_axis] = arc.target[arc.linear_axis];
+
+ } else {
+ arc.theta += arc.segment_theta;
+
+ arc.position[arc.plane_axis_0] =
+ arc.center_0 + sin(arc.theta) * arc.radius;
+ arc.position[arc.plane_axis_1] =
+ arc.center_1 + cos(arc.theta) * arc.radius;
+ arc.position[arc.linear_axis] += arc.segment_linear_travel;
+ }
+
+ // run the line
+ mach_plan_line(arc.position);
+
+ if (!--arc.segments) arc.running = false;
+ }
+}
+
+
+bool mach_arc_active() {return arc.running;}
+
+
+/// Stop arc movement without maintaining position
+/// OK to call if no arc is running
+void mach_abort_arc() {arc.running = false;}
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2016 Buildbotics LLC
+ Copyright (c) 2010 - 2015 Alden S. Hart, Jr.
+ All rights reserved.
+
+ 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/>.
+
+ The software is distributed in the hope that it will be useful, but
+ WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public
+ License along with the software. If not, see
+ <http://www.gnu.org/licenses/>.
+
+ For information regarding this software email:
+ "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#pragma once
+
+#include "gcode_state.h"
+#include "status.h"
+
+
+#define ARC_SEGMENT_LENGTH 0.1 // mm
+#define MIN_ARC_RADIUS 0.1
+
+#define MIN_ARC_SEGMENT_USEC 10000.0 // minimum arc segment time
+#define MIN_ARC_SEGMENT_TIME (MIN_ARC_SEGMENT_USEC / MICROSECONDS_PER_MINUTE)
+
+
+stat_t mach_arc_feed(float target[], bool flags[], float offsets[],
+ bool offset_f[], float radius, bool radius_f,
+ float P, bool P_f, bool modal_g1_f,
+ motion_mode_t motion_mode);
+void mach_arc_callback();
+bool mach_arc_active();
+void mach_abort_arc();
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2016 Buildbotics LLC
+ Copyright (c) 2010 - 2015 Alden S. Hart, Jr.
+ Copyright (c) 2012 - 2015 Rob Giseburt
+ All rights reserved.
+
+ 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/>.
+
+ The software is distributed in the hope that it will be useful, but
+ WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public
+ License along with the software. If not, see
+ <http://www.gnu.org/licenses/>.
+
+ For information regarding this software email:
+ "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+/* Planner buffers are used to queue and operate on Gcode blocks. Each
+ * buffer contains one Gcode block which may be a move, M code or
+ * other command that must be executed synchronously with movement.
+ */
+
+#include "buffer.h"
+#include "state.h"
+#include "rtc.h"
+#include "util.h"
+
+#include <string.h>
+#include <math.h>
+#include <stdio.h>
+
+
+typedef struct {
+ uint8_t space;
+ mp_buffer_t *tail;
+ mp_buffer_t *head;
+ mp_buffer_t bf[PLANNER_BUFFER_POOL_SIZE];
+} buffer_pool_t;
+
+
+buffer_pool_t mb;
+
+
+/// Zeroes the contents of a buffer
+static void _clear_buffer(mp_buffer_t *bf) {
+ mp_buffer_t *next = bf->next; // save pointers
+ mp_buffer_t *prev = bf->prev;
+ memset(bf, 0, sizeof(mp_buffer_t));
+ bf->next = next; // restore pointers
+ bf->prev = prev;
+}
+
+
+static void _push() {
+ if (!mb.space) {
+ ALARM(STAT_INTERNAL_ERROR);
+ return;
+ }
+
+ mb.tail = mb.tail->next;
+ mb.space--;
+}
+
+
+static void _pop() {
+ if (mb.space == PLANNER_BUFFER_POOL_SIZE) {
+ ALARM(STAT_INTERNAL_ERROR);
+ return;
+ }
+
+ mb.head = mb.head->next;
+ mb.space++;
+}
+
+
+/// Initializes or resets buffers
+void mp_queue_init() {
+ memset(&mb, 0, sizeof(mb)); // clear all values
+
+ mb.tail = mb.head = &mb.bf[0]; // init head and tail
+ mb.space = PLANNER_BUFFER_POOL_SIZE;
+
+ // Setup ring pointers
+ for (int i = 0; i < mb.space; i++) {
+ mb.bf[i].next = &mb.bf[i + 1];
+ mb.bf[i].prev = &mb.bf[i - 1];
+ }
+
+ mb.bf[0].prev = &mb.bf[mb.space -1]; // Fix first->prev
+ mb.bf[mb.space - 1].next = &mb.bf[0]; // Fix last->next
+
+ mp_state_idle();
+}
+
+
+uint8_t mp_queue_get_room() {
+ return mb.space < PLANNER_BUFFER_HEADROOM ?
+ 0 : mb.space - PLANNER_BUFFER_HEADROOM;
+}
+
+
+uint8_t mp_queue_get_fill() {
+ return PLANNER_BUFFER_POOL_SIZE - mb.space;
+}
+
+
+bool mp_queue_is_empty() {return mb.tail == mb.head;}
+
+
+/// Get pointer to next buffer, waiting until one is available.
+mp_buffer_t *mp_queue_get_tail() {
+ while (!mb.space) continue; // Wait for a buffer
+ return mb.tail;
+}
+
+
+/*** Commit the next buffer to the queue.
+ *
+ * WARNING: The routine calling mp_queue_push() must not use the write
+ * buffer once it has been queued. Action may start on the buffer immediately,
+ * invalidating its contents
+ */
+void mp_queue_push(buffer_cb_t cb, uint32_t line) {
+ mp_state_running();
+
+ mb.tail->ts = rtc_get_time();
+ mb.tail->cb = cb;
+ mb.tail->line = line;
+ mb.tail->state = BUFFER_NEW;
+
+ _push();
+}
+
+
+mp_buffer_t *mp_queue_get_head() {
+ return mp_queue_is_empty() ? 0 : mb.head;
+}
+
+
+/// Clear and release buffer to pool
+void mp_queue_pop() {
+ _clear_buffer(mb.head);
+ _pop();
+}
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2016 Buildbotics LLC
+ All rights reserved.
+
+ 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/>.
+
+ The software is distributed in the hope that it will be useful, but
+ WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public
+ License along with the software. If not, see
+ <http://www.gnu.org/licenses/>.
+
+ For information regarding this software email:
+ "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#pragma once
+
+#include "machine.h"
+#include "config.h"
+
+#include <stdbool.h>
+
+
+typedef enum {
+ BUFFER_OFF, // move inactive
+ BUFFER_NEW, // initial value
+ BUFFER_INIT, // first run
+ BUFFER_ACTIVE, // subsequent runs
+ BUFFER_RESTART, // restart buffer when done
+} buffer_state_t;
+
+
+// Callbacks
+struct mp_buffer_t;
+typedef stat_t (*buffer_cb_t)(struct mp_buffer_t *bf);
+
+
+typedef struct mp_buffer_t { // See Planning Velocity Notes
+ struct mp_buffer_t *prev; // pointer to previous buffer
+ struct mp_buffer_t *next; // pointer to next buffer
+
+ uint32_t ts; // Time stamp
+ int32_t line; // gcode block line number
+ buffer_cb_t cb; // callback to buffer exec function
+
+ buffer_state_t state; // buffer state
+ bool replannable; // true if move can be re-planned
+ bool hold; // hold at the start of this block
+
+ float value; // used in dwell and other callbacks
+
+ float target[AXES]; // XYZABC where the move should go
+ float unit[AXES]; // unit vector for axis scaling & planning
+
+ float length; // total length of line or helix in mm
+ float head_length;
+ float body_length;
+ float tail_length;
+
+ // See notes on these variables, in mp_aline()
+ float entry_velocity; // entry velocity requested for the move
+ float cruise_velocity; // cruise velocity requested & achieved
+ float exit_velocity; // exit velocity requested for the move
+ float braking_velocity; // current value for braking velocity
+
+ float entry_vmax; // max junction velocity at entry of this move
+ float cruise_vmax; // max cruise velocity requested for move
+ float exit_vmax; // max exit velocity possible (redundant)
+ float delta_vmax; // max velocity difference for this move
+
+ float jerk; // maximum linear jerk term for this move
+ float cbrt_jerk; // cube root of Jm (computed & cached)
+} mp_buffer_t;
+
+
+void mp_queue_init();
+
+uint8_t mp_queue_get_room();
+uint8_t mp_queue_get_fill();
+
+bool mp_queue_is_empty();
+
+mp_buffer_t *mp_queue_get_tail();
+void mp_queue_push(buffer_cb_t func, uint32_t line);
+
+mp_buffer_t *mp_queue_get_head();
+void mp_queue_pop();
+
+static inline mp_buffer_t *mp_buffer_prev(mp_buffer_t *bp) {return bp->prev;}
+static inline mp_buffer_t *mp_buffer_next(mp_buffer_t *bp) {return bp->next;}
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2016 Buildbotics LLC
+ All rights reserved.
+
+ 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/>.
+
+ The software is distributed in the hope that it will be useful, but
+ WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public
+ License along with the software. If not, see
+ <http://www.gnu.org/licenses/>.
+
+ For information regarding this software email:
+ "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+
+#include "calibrate.h"
+
+#include "buffer.h"
+#include "motor.h"
+#include "machine.h"
+#include "planner.h"
+#include "stepper.h"
+#include "rtc.h"
+#include "state.h"
+#include "config.h"
+
+#include <stdint.h>
+#include <stdio.h>
+#include <string.h>
+#include <stdlib.h>
+
+
+#define CAL_VELOCITIES 256
+#define CAL_MIN_VELOCITY 1000 // mm/sec
+#define CAL_TARGET_SG 100
+#define CAL_MAX_DELTA_SG 75
+#define CAL_WAIT_TIME 3 // ms
+
+
+enum {
+ CAL_START,
+ CAL_ACCEL,
+ CAL_MEASURE,
+ CAL_DECEL,
+};
+
+
+typedef struct {
+ bool stall_valid;
+ bool stalled;
+ bool reverse;
+
+ uint32_t wait;
+ int state;
+ int motor;
+ int axis;
+
+ float velocity;
+ uint16_t stallguard;
+} calibrate_t;
+
+static calibrate_t cal = {0};
+
+
+static stat_t _exec_calibrate(mp_buffer_t *bf) {
+ const float time = MIN_SEGMENT_TIME; // In minutes
+ const float max_delta_v = CAL_ACCELERATION * time;
+
+ do {
+ if (rtc_expired(cal.wait))
+ switch (cal.state) {
+ case CAL_START: {
+ cal.axis = motor_get_axis(cal.motor);
+ cal.state = CAL_ACCEL;
+ cal.velocity = 0;
+ cal.stall_valid = false;
+ cal.stalled = false;
+ cal.reverse = false;
+
+ //tmc2660_set_stallguard_threshold(cal.motor, 8);
+ cal.wait = rtc_get_time() + CAL_WAIT_TIME;
+
+ break;
+ }
+
+ case CAL_ACCEL:
+ if (CAL_MIN_VELOCITY < cal.velocity) cal.stall_valid = true;
+
+ if (cal.velocity < CAL_MIN_VELOCITY || CAL_TARGET_SG < cal.stallguard)
+ cal.velocity += max_delta_v;
+
+ if (cal.stalled) {
+ if (cal.reverse) {
+ int32_t steps = -motor_get_encoder(cal.motor);
+ float mm = (float)steps / motor_get_steps_per_unit(cal.motor);
+ STATUS_DEBUG("%"PRIi32" steps %0.2f mm", steps, mm);
+
+ //tmc2660_set_stallguard_threshold(cal.motor, 63);
+
+ mp_set_cycle(CYCLE_MACHINING); // Default cycle
+
+ return STAT_NOOP; // Done, no move queued
+
+ } else {
+ motor_set_encoder(cal.motor, 0);
+
+ cal.reverse = true;
+ cal.velocity = 0;
+ cal.stall_valid = false;
+ cal.stalled = false;
+ }
+ }
+ break;
+ }
+ } while (fp_ZERO(cal.velocity)); // Repeat if computed velocity was zero
+
+ // Compute travel
+ float travel[AXES] = {0}; // In mm
+ travel[cal.axis] = time * cal.velocity * (cal.reverse ? -1 : 1);
+
+ // Convert to steps
+ float steps[MOTORS] = {0};
+ mp_kinematics(travel, steps);
+
+ // Queue segment
+ st_prep_line(time, steps, 0);
+
+ return STAT_EAGAIN;
+}
+
+
+bool calibrate_busy() {return mp_get_cycle() == CYCLE_CALIBRATING;}
+
+
+void calibrate_set_stallguard(int motor, uint16_t sg) {
+ if (cal.motor != motor) return;
+
+ if (cal.stall_valid) {
+ int16_t delta = sg - cal.stallguard;
+ if (!sg || CAL_MAX_DELTA_SG < abs(delta)) {
+ cal.stalled = true;
+ motor_end_move(cal.motor);
+ }
+ }
+
+ cal.stallguard = sg;
+}
+
+
+uint8_t command_calibrate(int argc, char *argv[]) {
+ if (mp_get_cycle() != CYCLE_MACHINING || mp_get_state() != STATE_READY)
+ return 0;
+
+ // Start
+ memset(&cal, 0, sizeof(cal));
+ mp_set_cycle(CYCLE_CALIBRATING);
+ cal.motor = 1;
+
+ mp_queue_push_nonstop(_exec_calibrate, -1);
+
+ return 0;
+}
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2016 Buildbotics LLC
+ All rights reserved.
+
+ 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/>.
+
+ The software is distributed in the hope that it will be useful, but
+ WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public
+ License along with the software. If not, see
+ <http://www.gnu.org/licenses/>.
+
+ For information regarding this software email:
+ "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#pragma once
+
+#include <stdbool.h>
+#include <stdint.h>
+
+
+bool calibrate_busy();
+void calibrate_set_stallguard(int motor, uint16_t sg);
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2016 Buildbotics LLC
+ Copyright (c) 2010 - 2015 Alden S. Hart, Jr.
+ Copyright (c) 2012 - 2015 Rob Giseburt
+ All rights reserved.
+
+ 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/>.
+
+ The software is distributed in the hope that it will be useful, but
+ WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public
+ License along with the software. If not, see
+ <http://www.gnu.org/licenses/>.
+
+ For information regarding this software email:
+ "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#include "dwell.h"
+
+#include "buffer.h"
+#include "machine.h"
+#include "stepper.h"
+
+
+// Dwells are performed by passing a dwell move to the stepper drivers.
+
+
+/// Dwell execution
+static stat_t _exec_dwell(mp_buffer_t *bf) {
+ st_prep_dwell(bf->value); // in seconds
+ return STAT_OK; // Done
+}
+
+
+/// Queue a dwell
+stat_t mp_dwell(float seconds, int32_t line) {
+ mp_buffer_t *bf = mp_queue_get_tail();
+ bf->value = seconds; // in seconds, not minutes
+ mp_queue_push(_exec_dwell, line);
+
+ return STAT_OK;
+}
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2016 Buildbotics LLC
+ All rights reserved.
+
+ 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/>.
+
+ The software is distributed in the hope that it will be useful, but
+ WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public
+ License along with the software. If not, see
+ <http://www.gnu.org/licenses/>.
+
+ For information regarding this software email:
+ "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#pragma once
+
+#include "status.h"
+
+stat_t mp_dwell(float seconds, int32_t line);
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2016 Buildbotics LLC
+ Copyright (c) 2010 - 2015 Alden S. Hart, Jr.
+ Copyright (c) 2012 - 2015 Rob Giseburt
+ All rights reserved.
+
+ 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/>.
+
+ The software is distributed in the hope that it will be useful, but
+ WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public
+ License along with the software. If not, see
+ <http://www.gnu.org/licenses/>.
+
+ For information regarding this software email:
+ "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#include "planner.h"
+#include "buffer.h"
+#include "axis.h"
+#include "runtime.h"
+#include "state.h"
+#include "forward_dif.h"
+#include "stepper.h"
+#include "motor.h"
+#include "rtc.h"
+#include "usart.h"
+#include "config.h"
+
+#include <string.h>
+#include <stdbool.h>
+#include <math.h>
+#include <stdio.h>
+
+
+typedef struct {
+ float unit[AXES]; // unit vector for axis scaling & planning
+ float final_target[AXES]; // final target, used to correct rounding errors
+ float waypoint[3][AXES]; // head/body/tail endpoints for correction
+
+ // copies of bf variables of same name
+ float head_length;
+ float body_length;
+ float tail_length;
+ float entry_velocity;
+ float cruise_velocity;
+ float exit_velocity;
+
+ uint32_t segment_count; // count of running segments
+ float segment_velocity; // computed velocity for segment
+ float segment_time; // actual time increment per segment
+ forward_dif_t fdif; // forward difference levels
+ bool hold_planned; // true when a feedhold has been planned
+ move_section_t section; // current move section
+ bool section_new; // true if it's a new section
+ bool abort;
+} mp_exec_t;
+
+
+static mp_exec_t ex = {{0}};
+
+
+static stat_t _exec_aline_segment() {
+ float target[AXES];
+
+ // Set target position for the segment. If the segment ends on a section
+ // waypoint, synchronize to the head, body or tail end. 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.
+ if (!--ex.segment_count && !ex.section_new && !ex.hold_planned)
+ copy_vector(target, ex.waypoint[ex.section]);
+
+ else {
+ float segment_length = ex.segment_velocity * ex.segment_time;
+
+ for (int axis = 0; axis < AXES; axis++)
+ target[axis] =
+ mp_runtime_get_axis_position(axis) + ex.unit[axis] * segment_length;
+ }
+
+ mp_runtime_set_velocity(ex.segment_velocity);
+ RITORNO(mp_runtime_move_to_target(target, ex.segment_time));
+
+ // Return EAGAIN to continue or OK if this segment is done
+ return ex.segment_count ? STAT_EAGAIN : STAT_OK;
+}
+
+
+/// Common code for head and tail sections
+static stat_t _exec_aline_section(float length, float vin, float vout) {
+ if (ex.section_new) {
+ if (fp_ZERO(length)) return STAT_NOOP; // end the section
+
+ // len / avg. velocity
+ float move_time = 2 * length / (vin + vout);
+ float segments = ceil(move_time / NOM_SEGMENT_TIME);
+ ex.segment_time = move_time / segments;
+ ex.segment_count = round(segments);
+
+ if (vin == vout) ex.segment_velocity = vin;
+ else ex.segment_velocity =
+ mp_init_forward_dif(ex.fdif, vin, vout, segments);
+
+ if (ex.segment_time < MIN_SEGMENT_TIME)
+ return STAT_MINIMUM_TIME_MOVE; // exit /wo advancing position
+
+ // First segment
+ if (_exec_aline_segment() == STAT_OK) {
+ ex.section_new = false;
+ return STAT_OK;
+ }
+
+ ex.section_new = false;
+
+ } else {
+ if (vin != vout) ex.segment_velocity += mp_next_forward_dif(ex.fdif);
+
+ // Subsequent segments
+ if (_exec_aline_segment() == STAT_OK) return STAT_OK;
+ }
+
+ return STAT_EAGAIN;
+}
+
+
+/// Callback for tail section
+static stat_t _exec_aline_tail() {
+ ex.section = SECTION_TAIL;
+ return
+ _exec_aline_section(ex.tail_length, ex.cruise_velocity, ex.exit_velocity);
+}
+
+
+/// Callback for body section
+static stat_t _exec_aline_body() {
+ ex.section = SECTION_BODY;
+
+ stat_t status =
+ _exec_aline_section(ex.body_length, ex.cruise_velocity, ex.cruise_velocity);
+
+ switch (status) {
+ case STAT_NOOP: return _exec_aline_tail();
+ case STAT_OK:
+ ex.section = SECTION_TAIL;
+ ex.section_new = true;
+
+ return STAT_EAGAIN;
+
+ default: return status;
+ }
+}
+
+
+/// Callback for head section
+static stat_t _exec_aline_head() {
+ ex.section = SECTION_HEAD;
+ stat_t status =
+ _exec_aline_section(ex.head_length, ex.entry_velocity, ex.cruise_velocity);
+
+ switch (status) {
+ case STAT_NOOP: return _exec_aline_body();
+ case STAT_OK:
+ ex.section = SECTION_BODY;
+ ex.section_new = true;
+
+ return STAT_EAGAIN;
+
+ default: return status;
+ }
+}
+
+
+static float _compute_next_segment_velocity() {
+ if (ex.section_new) {
+ if (ex.section == SECTION_HEAD) return mp_runtime_get_velocity();
+ else return ex.cruise_velocity;
+ }
+
+ return ex.segment_velocity +
+ (ex.section == SECTION_BODY ? 0 : mp_next_forward_dif(ex.fdif));
+}
+
+
+/// Replan current move to execute hold
+///
+/// Holds are initiated by the planner entering STATE_STOPPING. In which case
+/// _plan_hold() is called to replan the current move towards zero. If it is
+/// unable to plan to zero in the remaining length of the current move it will
+/// decelerate as much as possible and then wait for the next move. Once it is
+/// possible to plan to zero velocity in the current move the remaining length
+/// is put into the run buffer, which is still allocated, and the run buffer
+/// becomes the hold point. The hold is left by a start request in state.c. At
+/// this point the remaining buffers, if any, are replanned from zero up to
+/// speed.
+static void _plan_hold() {
+ mp_buffer_t *bf = mp_queue_get_head(); // working buffer pointer
+ if (!bf) return; // Oops! nothing's running
+
+ // Examine and process current buffer and compute length left for decel
+ float available_length =
+ axis_get_vector_length(ex.final_target, mp_runtime_get_position());
+ // Compute next_segment velocity, velocity left to shed to brake to zero
+ float braking_velocity = _compute_next_segment_velocity();
+ // Distance to brake to zero from braking_velocity, bf is OK to use here
+ float braking_length = mp_get_target_length(braking_velocity, 0, bf->jerk);
+
+ // Hack to prevent Case 2 moves for perfect-fit decels. Happens when homing.
+ if (available_length < braking_length && fp_ZERO(bf->exit_velocity))
+ braking_length = available_length;
+
+ // Replan to decelerate
+ ex.section = SECTION_TAIL;
+ ex.section_new = true;
+ ex.cruise_velocity = braking_velocity;
+ ex.hold_planned = true;
+
+ // Avoid creating segments before or after the hold which are too small.
+ if (fabs(available_length - braking_length) < HOLD_DECELERATION_TOLERANCE) {
+ // Case 0: deceleration fits almost exactly
+ ex.exit_velocity = 0;
+ ex.tail_length = available_length;
+
+ } else if (braking_length <= available_length) {
+ // Case 1: deceleration fits entirely into the remaining length
+ // Setup tail to perform the deceleration
+ ex.exit_velocity = 0;
+ ex.tail_length = braking_length;
+
+ // Re-use bf to run the remaining block length
+ bf->length = available_length - braking_length;
+ bf->delta_vmax = mp_get_target_velocity(0, bf->length, bf);
+ bf->entry_vmax = 0;
+ bf->state = BUFFER_RESTART; // Restart the buffer when done
+
+ } else {
+ // Case 2: deceleration exceeds length remaining in buffer
+ // Replan to minimum (but non-zero) exit velocity
+ ex.tail_length = available_length;
+ ex.exit_velocity =
+ braking_velocity - mp_get_target_velocity(0, available_length, bf);
+ }
+}
+
+
+/// Initializes move runtime with a new planner buffer
+static stat_t _exec_aline_init(mp_buffer_t *bf) {
+ // Remove zero length lines. Short lines have already been removed.
+ if (fp_ZERO(bf->length)) return STAT_NOOP;
+
+ // Initialize move
+ copy_vector(ex.unit, bf->unit);
+ copy_vector(ex.final_target, bf->target);
+
+ ex.head_length = bf->head_length;
+ ex.body_length = bf->body_length;
+ ex.tail_length = bf->tail_length;
+ ex.entry_velocity = bf->entry_velocity;
+ ex.cruise_velocity = bf->cruise_velocity;
+ ex.exit_velocity = bf->exit_velocity;
+
+ ex.section = SECTION_HEAD;
+ ex.section_new = true;
+ ex.hold_planned = false;
+
+ // Generate waypoints for position correction at section ends. This helps
+ // negate floating point errors in the forward differencing code.
+ for (int axis = 0; axis < AXES; axis++) {
+ float position = mp_runtime_get_axis_position(axis);
+
+ ex.waypoint[SECTION_HEAD][axis] = position + ex.unit[axis] * ex.head_length;
+ ex.waypoint[SECTION_BODY][axis] = position +
+ ex.unit[axis] * (ex.head_length + ex.body_length);
+ ex.waypoint[SECTION_TAIL][axis] = ex.final_target[axis];
+ }
+
+ return STAT_OK;
+}
+
+
+void mp_exec_abort() {ex.abort = true;}
+
+
+/// Aline execution routines
+///
+/// Everything here fires from interrupts and must be interrupt safe
+///
+/// Returns:
+///
+/// STAT_OK move is done
+/// STAT_EAGAIN move is not finished - has more segments to run
+/// STAT_NOOP cause no stepper operation - 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 correct behavior of these routines.
+/// Each call to _exec_aline() must execute and prep *one and only one*
+/// segment. If the segment is not the last segment in the bf buffer the
+/// _aline() returns STAT_EAGAIN. If it's the last segment it returns
+/// STAT_OK. If it encounters a fatal error that would terminate the move it
+/// returns a valid error code.
+///
+/// Notes:
+///
+/// [1] Returning STAT_OK ends the move and frees the bf buffer.
+/// Returning STAT_OK at does NOT advance position meaning
+/// any position error will be compensated by the next move.
+///
+/// Operation:
+///
+/// Aline generates jerk-controlled S-curves as per Ed Red's course notes:
+///
+/// http://www.et.byu.edu/~ered/ME537/Notes/Ch5.pdf
+/// http://www.scribd.com/doc/63521608/Ed-Red-Ch5-537-Jerk-Equations
+///
+/// A full trapezoid is divided into 5 periods. Periods 1 and 2 are the
+/// first and second halves of the acceleration ramp (the concave and convex
+/// parts of the S curve in the "head"). Periods 3 and 4 are the first
+/// and second parts of the deceleration ramp (the tail). There is also
+/// a period for the constant-velocity plateau of the trapezoid (the body).
+/// There are many possible degenerate trapezoids where any of the 5 periods
+/// may be zero length but note that either none or both of a ramping pair can
+/// be zero.
+///
+/// The equations that govern the acceleration and deceleration ramps are:
+///
+/// Period 1 V = Vi + Jm * (T^2) / 2
+/// Period 2 V = Vh + As * T - Jm * (T^2) / 2
+/// Period 3 V = Vi - Jm * (T^2) / 2
+/// Period 4 V = Vh + As * T + Jm * (T^2) / 2
+///
+/// move_time is the actual time of the move, accel_time is the time value
+/// needed to compute the velocity taking the initial velocity into account.
+/// move_time does not need to.
+stat_t mp_exec_aline(mp_buffer_t *bf) {
+ stat_t status = STAT_OK;
+
+ if (ex.abort) {
+ ex.abort = false;
+ mp_runtime_set_velocity(0); // Assume a hard stop
+ return STAT_NOOP;
+ }
+
+ // Start a new move
+ if (bf->state == BUFFER_INIT) {
+ bf->state = BUFFER_ACTIVE;
+ status = _exec_aline_init(bf);
+ if (status != STAT_OK) return status;
+ }
+
+ // Plan holds
+ if (mp_get_state() == STATE_STOPPING && !ex.hold_planned) _plan_hold();
+
+ // Main segment dispatch
+ switch (ex.section) {
+ case SECTION_HEAD: status = _exec_aline_head(); break;
+ case SECTION_BODY: status = _exec_aline_body(); break;
+ case SECTION_TAIL: status = _exec_aline_tail(); break;
+ }
+
+ // Set runtime velocity on exit
+ if (status != STAT_EAGAIN) mp_runtime_set_velocity(ex.exit_velocity);
+
+ return status;
+}
+
+
+/// Dequeues buffers, initializes them, executes their callbacks and cleans up.
+///
+/// This is the guts of the planner runtime execution. Because this routine is
+/// run in an interrupt the state changes must be carefully ordered.
+stat_t mp_exec_move() {
+ // Check if we can run a buffer
+ mp_buffer_t *bf = mp_queue_get_head();
+ if (mp_get_state() == STATE_ESTOPPED || mp_get_state() == STATE_HOLDING ||
+ !bf) {
+ mp_runtime_set_velocity(0);
+ mp_runtime_set_busy(false);
+
+ return STAT_NOOP; // Nothing running
+ }
+
+ // Process new buffers
+ if (bf->state == BUFFER_NEW) {
+ // On restart wait a bit to give planner queue a chance to fill
+ if (!mp_runtime_is_busy() && mp_queue_get_fill() < PLANNER_EXEC_MIN_FILL &&
+ !rtc_expired(bf->ts + PLANNER_EXEC_DELAY)) return STAT_NOOP;
+
+ // Take control of buffer
+ bf->state = BUFFER_INIT;
+ bf->replannable = false;
+
+ // Update runtime
+ mp_runtime_set_line(bf->line);
+ }
+
+ // Execute the buffer
+ stat_t status = bf->cb(bf);
+
+ // Signal that we are busy only if a move was queued. This means that
+ // nonstop buffers, i.e. non-plan-to-zero commands, will not cause the
+ // runtime to enter the busy state. This causes mp_exec_move() to continue
+ // to wait above for the planner buffer to fill when a new stream starts
+ // with some nonstop buffers. If this weren't so, the code below
+ // which marks the next buffer not replannable would lock the first move
+ // buffer and cause it to be unnecessarily planned to zero.
+ if (status == STAT_EAGAIN || status == STAT_OK) mp_runtime_set_busy(true);
+
+ // Process finished buffers
+ if (status != STAT_EAGAIN) {
+ // Signal that we've encountered a stopping point
+ if (fp_ZERO(mp_runtime_get_velocity()) &&
+ (mp_get_state() == STATE_STOPPING || bf->hold)) mp_state_holding();
+
+ // Handle buffer restarts and deallocation
+ if (bf->state == BUFFER_RESTART) bf->state = BUFFER_NEW;
+ else {
+ // Solves a potential race condition where the current buffer ends but
+ // the new buffer has not started because the current one is still
+ // being run by the steppers. Planning can overwrite the new buffer.
+ // See notes above.
+ mp_buffer_next(bf)->replannable = false;
+
+ mp_queue_pop(); // Release buffer
+
+ // Enter READY state
+ if (mp_queue_is_empty()) mp_state_idle();
+ }
+ }
+
+ // Convert return status for stepper.c
+ switch (status) {
+ case STAT_NOOP:
+ // Tell caller to call again if there is more in the queue
+ return mp_queue_is_empty() ? STAT_NOOP : STAT_EAGAIN;
+ case STAT_EAGAIN: return STAT_OK; // A move was queued, call again later
+ default: return status;
+ }
+}
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2016 Buildbotics LLC
+ All rights reserved.
+
+ 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/>.
+
+ The software is distributed in the hope that it will be useful, but
+ WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public
+ License along with the software. If not, see
+ <http://www.gnu.org/licenses/>.
+
+ For information regarding this software email:
+ "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#pragma once
+
+#include "buffer.h"
+
+
+stat_t mp_exec_move();
+void mp_exec_abort();
+stat_t mp_exec_aline(mp_buffer_t *bf);
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2016 Buildbotics LLC
+ Copyright (c) 2010 - 2015 Alden S. Hart, Jr.
+ Copyright (c) 2012 - 2015 Rob Giseburt
+ All rights reserved.
+
+ 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/>.
+
+ The software is distributed in the hope that it will be useful, but
+ WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public
+ License along with the software. If not, see
+ <http://www.gnu.org/licenses/>.
+
+ For information regarding this software email:
+ "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#include "forward_dif.h"
+
+#include <math.h>
+
+
+/// Forward differencing math
+///
+/// We are using a quintic (fifth-degree) Bezier polynomial for the velocity
+/// curve. This gives us a "linear pop" velocity curve; with pop being the
+/// sixth derivative of position: velocity - 1st, acceleration - 2nd, jerk -
+/// 3rd, snap - 4th, crackle - 5th, pop - 6th
+///
+/// The Bezier curve takes the form:
+///
+/// V(t) = P_0 * B_0(t) + P_1 * B_1(t) + P_2 * B_2(t) + P_3 * B_3(t) +
+/// P_4 * B_4(t) + P_5 * B_5(t)
+///
+/// Where 0 <= t <= 1, and V(t) is the velocity. P_0 through P_5 are
+/// the control points, and B_0(t) through B_5(t) are the Bernstein
+/// basis as follows:
+///
+/// B_0(t) = (1 - t)^5 = -t^5 + 5t^4 - 10t^3 + 10t^2 - 5t + 1
+/// B_1(t) = 5(1 - t)^4 * t = 5t^5 - 20t^4 + 30t^3 - 20t^2 + 5t
+/// B_2(t) = 10(1 - t)^3 * t^2 = -10t^5 + 30t^4 - 30t^3 + 10t^2
+/// B_3(t) = 10(1 - t)^2 * t^3 = 10t^5 - 20t^4 + 10t^3
+/// B_4(t) = 5(1 - t) * t^4 = -5t^5 + 5t^4
+/// B_5(t) = t^5 = t^5
+///
+/// ^ ^ ^ ^ ^ ^
+/// A B C D E F
+///
+/// We use forward-differencing to calculate each position through the curve.
+/// This requires a formula of the form:
+///
+/// V_f(t) = A * t^5 + B * t^4 + C * t^3 + D * t^2 + E * t + F
+///
+/// Looking at the above B_0(t) through B_5(t) expanded forms, if we take the
+/// coefficients of t^5 through t of the Bezier form of V(t), we can determine
+/// that:
+///
+/// A = -P_0 + 5 * P_1 - 10 * P_2 + 10 * P_3 - 5 * P_4 + P_5
+/// B = 5 * P_0 - 20 * P_1 + 30 * P_2 - 20 * P_3 + 5 * P_4
+/// C = -10 * P_0 + 30 * P_1 - 30 * P_2 + 10 * P_3
+/// D = 10 * P_0 - 20 * P_1 + 10 * P_2
+/// E = - 5 * P_0 + 5 * P_1
+/// F = P_0
+///
+/// Now, since we will (currently) *always* want the initial acceleration and
+/// jerk values to be 0, We set P_i = P_0 = P_1 = P_2 (initial velocity), and
+/// P_t = P_3 = P_4 = P_5 (target velocity), which, after simplification,
+/// resolves to:
+///
+/// A = - 6 * P_i + 6 * P_t
+/// B = 15 * P_i - 15 * P_t
+/// C = -10 * P_i + 10 * P_t
+/// D = 0
+/// E = 0
+/// F = P_i
+///
+/// Given an interval count of I to get from P_i to P_t, we get the parametric
+/// "step" size of h = 1/I. We need to calculate the initial value of forward
+/// differences (F_0 - F_5) such that the inital velocity V = P_i, then we
+/// iterate over the following I times:
+///
+/// V += F_5
+/// F_5 += F_4
+/// F_4 += F_3
+/// F_3 += F_2
+/// F_2 += F_1
+///
+/// See
+/// http://www.drdobbs.com/forward-difference-calculation-of-bezier/184403417
+/// for an example of how to calculate F_0 - F_5 for a cubic bezier curve. Since
+/// this is a quintic bezier curve, we need to extend the formulas somewhat.
+/// I'll not go into the long-winded step-by-step here, but it gives the
+/// resulting formulas:
+///
+/// a = A, b = B, c = C, d = D, e = E, f = F
+///
+/// F_5(t + h) - F_5(t) = (5ah)t^4 + (10ah^2 + 4bh)t^3 +
+/// (10ah^3 + 6bh^2 + 3ch)t^2 + (5ah^4 + 4bh^3 + 3ch^2 + 2dh)t + ah^5 +
+/// bh^4 + ch^3 + dh^2 + eh
+///
+/// a = 5ah
+/// b = 10ah^2 + 4bh
+/// c = 10ah^3 + 6bh^2 + 3ch
+/// d = 5ah^4 + 4bh^3 + 3ch^2 + 2dh
+///
+/// After substitution, simplification, and rearranging:
+///
+/// F_4(t + h) - F_4(t) = (20ah^2)t^3 + (60ah^3 + 12bh^2)t^2 +
+/// (70ah^4 + 24bh^3 + 6ch^2)t + 30ah^5 + 14bh^4 + 6ch^3 + 2dh^2
+///
+/// a = 20ah^2
+/// b = 60ah^3 + 12bh^2
+/// c = 70ah^4 + 24bh^3 + 6ch^2
+///
+/// After substitution, simplification, and rearranging:
+///
+/// F_3(t + h) - F_3(t) = (60ah^3)t^2 + (180ah^4 + 24bh^3)t + 150ah^5 +
+/// 36bh^4 + 6ch^3
+///
+/// You get the picture...
+///
+/// F_2(t + h) - F_2(t) = (120ah^4)t + 240ah^5 + 24bh^4
+/// F_1(t + h) - F_1(t) = 120ah^5
+///
+/// Normally, we could then assign t = 0, use the A-F values from above, and get
+/// out initial F_* values. However, for the sake of "averaging" the velocity
+/// of each segment, we actually want to have the initial V be be at t = h/2 and
+/// iterate I-1 times. So, the resulting F_* values are (steps not shown):
+///
+/// F_5 = 121Ah^5 / 16 + 5Bh^4 + 13Ch^3 / 4 + 2Dh^2 + Eh
+/// F_4 = 165Ah^5 / 2 + 29Bh^4 + 9Ch^3 + 2Dh^2
+/// F_3 = 255Ah^5 + 48Bh^4 + 6Ch^3
+/// F_2 = 300Ah^5 + 24Bh^4
+/// F_1 = 120Ah^5
+///
+/// Note that with our current control points, D and E are actually 0.
+///
+/// This can be simplified even further by subsituting Ah, Bh & Ch back in and
+/// reducing to:
+///
+/// F_5 = (32.5 * s^2 - 75 * s + 45.375)(Vt - Vi) * h^5
+/// F_4 = (90.0 * s^2 - 435 * s + 495.0 )(Vt - Vi) * h^5
+/// F_3 = (60.0 * s^2 - 720 * s + 1530.0 )(Vt - Vi) * h^5
+/// F_2 = ( - 360 * s + 1800.0 )(Vt - Vi) * h^5
+/// F_1 = ( 720.0 )(Vt - Vi) * h^5
+///
+float mp_init_forward_dif(forward_dif_t fdif, float Vi, float Vt, float s) {
+ const float h = 1 / s;
+ const float s2 = square(s);
+ const float Vdxh5 = (Vt - Vi) * h * h * h * h * h;
+
+ fdif[4] = (32.5 * s2 - 75.0 * s + 45.375) * Vdxh5;
+ fdif[3] = (90.0 * s2 - 435.0 * s + 495.0 ) * Vdxh5;
+ fdif[2] = (60.0 * s2 - 720.0 * s + 1530.0 ) * Vdxh5;
+ fdif[1] = ( - 360.0 * s + 1800.0 ) * Vdxh5;
+ fdif[0] = ( 720.0 ) * Vdxh5;
+
+ // Calculate the initial velocity by calculating:
+ //
+ // V(h / 2) =
+ //
+ // ( -6Vi + 6Vt) / (2^5 * s^8) +
+ // ( 15Vi - 15Vt) / (2^4 * s^8) +
+ // (-10Vi + 10Vt) / (2^3 * s^8) + Vi =
+ //
+ // (Vt - Vi) * 1/2 * h^8 + Vi
+ return (Vt - Vi) * 0.5 * square(square(square(h))) + Vi;
+}
+
+
+float mp_next_forward_dif(forward_dif_t fdif) {
+ float delta = fdif[4];
+
+ for (int i = 4; i; i--)
+ fdif[i] += fdif[i - 1];
+
+ return delta;
+}
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2016 Buildbotics LLC
+ All rights reserved.
+
+ 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/>.
+
+ The software is distributed in the hope that it will be useful, but
+ WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public
+ License along with the software. If not, see
+ <http://www.gnu.org/licenses/>.
+
+ For information regarding this software email:
+ "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#pragma once
+
+
+typedef float forward_dif_t[5];
+
+float mp_init_forward_dif(forward_dif_t fdif, float Vi, float Vt, float s);
+float mp_next_forward_dif(forward_dif_t fdif);
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2016 Buildbotics LLC
+ All rights reserved.
+
+ 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/>.
+
+ The software is distributed in the hope that it will be useful, but
+ WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public
+ License along with the software. If not, see
+ <http://www.gnu.org/licenses/>.
+
+ For information regarding this software email:
+ "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#include "jog.h"
+
+#include "axis.h"
+#include "planner.h"
+#include "buffer.h"
+#include "line.h"
+#include "forward_dif.h"
+#include "runtime.h"
+#include "machine.h"
+#include "state.h"
+#include "config.h"
+
+#include <stdbool.h>
+#include <math.h>
+#include <string.h>
+#include <stdio.h>
+#include <stdlib.h>
+
+
+typedef struct {
+ bool writing;
+ float Vi;
+ float Vt;
+ forward_dif_t fdifs[AXES];
+ unsigned segments[AXES];
+
+ int sign[AXES];
+ float velocity[AXES];
+ float next_velocity[AXES];
+ float target_velocity[AXES];
+} jog_runtime_t;
+
+
+static jog_runtime_t jr;
+
+
+static stat_t _exec_jog(mp_buffer_t *bf) {
+ // Load next velocity
+ bool changed = false;
+ bool done = true;
+ if (!jr.writing)
+ for (int axis = 0; axis < AXES; axis++) {
+ float Vn = jr.next_velocity[axis] * axis_get_velocity_max(axis);
+ float Vi = jr.velocity[axis];
+ float Vt = jr.target_velocity[axis];
+
+ if (JOG_MIN_VELOCITY < fabs(Vn)) done = false;
+
+ if (!fp_ZERO(Vi) && (Vn < 0) != (Vi < 0))
+ Vn = 0; // Plan to zero on sign change
+
+ if (fabs(Vn) < JOG_MIN_VELOCITY) Vn = 0;
+
+ if (Vt != Vn) {
+ jr.target_velocity[axis] = Vn;
+ if (Vn) jr.sign[axis] = Vn < 0 ? -1 : 1;
+ changed = true;
+ }
+ }
+
+ float velocity_sqr = 0;
+
+ for (int axis = 0; axis < AXES; axis++) {
+ float Vi = fabs(jr.velocity[axis]);
+ float Vt = fabs(jr.target_velocity[axis]);
+
+ if (changed) {
+ if (fp_EQ(Vi, Vt)) {
+ Vi = Vt;
+ jr.segments[axis] = 0;
+
+ } else {
+ // Compute axis max jerk
+ float jerk = axis_get_jerk_max(axis) * JERK_MULTIPLIER;
+
+ // Compute length to velocity given max jerk
+ float length = mp_get_target_length(Vi, Vt, jerk * JOG_JERK_MULT);
+
+ // Compute move time
+ float move_time = 2 * length / (Vi + Vt);
+ if (move_time < MIN_SEGMENT_TIME) {
+ Vi = Vt;
+ jr.segments[axis] = 0;
+
+ } else {
+ jr.segments[axis] = ceil(move_time / NOM_SEGMENT_TIME);
+
+ Vi = mp_init_forward_dif(jr.fdifs[axis], Vi, Vt, jr.segments[axis]);
+ jr.segments[axis]--;
+ }
+ }
+
+ } else if (jr.segments[axis]) {
+ Vi += mp_next_forward_dif(jr.fdifs[axis]);
+ jr.segments[axis]--;
+
+ } else Vi = Vt;
+
+ if (JOG_MIN_VELOCITY < Vi || JOG_MIN_VELOCITY < Vt) done = false;
+
+ velocity_sqr += square(Vi);
+ jr.velocity[axis] = Vi * jr.sign[axis];
+ }
+
+ // Check if we are done
+ if (done) {
+ // Update machine position
+ for (int axis = 0; axis < AXES; axis++)
+ mach_set_axis_position(axis, mp_runtime_get_work_position(axis));
+
+ mp_set_cycle(CYCLE_MACHINING); // Default cycle
+
+ return STAT_NOOP; // Done, no move executed
+ }
+
+ // Compute target from velocity
+ float target[AXES];
+ for (int axis = 0; axis < AXES; axis++)
+ target[axis] = mp_runtime_get_axis_position(axis) +
+ jr.velocity[axis] * NOM_SEGMENT_TIME;
+
+ // Set velocity and target
+ mp_runtime_set_velocity(sqrt(velocity_sqr));
+ stat_t status = mp_runtime_move_to_target(target, NOM_SEGMENT_TIME);
+ if (status != STAT_OK) return status;
+
+ return STAT_EAGAIN;
+}
+
+
+bool mp_jog_busy() {return mp_get_cycle() == CYCLE_JOGGING;}
+
+
+uint8_t command_jog(int argc, char *argv[]) {
+ if (!mp_jog_busy() &&
+ (mp_get_state() != STATE_READY || mp_get_cycle() != CYCLE_MACHINING))
+ return STAT_NOOP;
+
+ float velocity[AXES];
+
+ for (int axis = 0; axis < AXES; axis++)
+ if (axis < argc - 1) velocity[axis] = atof(argv[axis + 1]);
+ else velocity[axis] = 0;
+
+ // Reset
+ if (!mp_jog_busy()) memset(&jr, 0, sizeof(jr));
+
+ jr.writing = true;
+ for (int axis = 0; axis < AXES; axis++)
+ jr.next_velocity[axis] = velocity[axis];
+ jr.writing = false;
+
+ if (!mp_jog_busy()) {
+ mp_set_cycle(CYCLE_JOGGING);
+ mp_queue_push_nonstop(_exec_jog, -1);
+ }
+
+ return STAT_OK;
+}
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2016 Buildbotics LLC
+ All rights reserved.
+
+ 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/>.
+
+ The software is distributed in the hope that it will be useful, but
+ WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public
+ License along with the software. If not, see
+ <http://www.gnu.org/licenses/>.
+
+ For information regarding this software email:
+ "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#pragma once
+
+#include <stdbool.h>
+
+
+bool mp_jog_busy();
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2016 Buildbotics LLC
+ Copyright (c) 2010 - 2015 Alden S. Hart, Jr.
+ Copyright (c) 2012 - 2015 Rob Giseburt
+ All rights reserved.
+
+ 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/>.
+
+ The software is distributed in the hope that it will be useful, but
+ WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public
+ License along with the software. If not, see
+ <http://www.gnu.org/licenses/>.
+
+ For information regarding this software email:
+ "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#include "line.h"
+
+#include "axis.h"
+#include "planner.h"
+#include "exec.h"
+#include "buffer.h"
+
+
+/* Sonny's algorithm - simple
+ *
+ * Computes the maximum allowable junction speed by finding the velocity that
+ * will yield the centripetal acceleration in the corner_acceleration value. The
+ * value of delta sets the effective radius of curvature. Here's Sonny's
+ * (Sungeun K. Jeon's) explanation of what's going on:
+ *
+ * "First let's assume that at a junction we only look a centripetal
+ * acceleration to simplify things. At a junction of two lines, let's place a
+ * circle such that both lines are tangent to the circle. The circular segment
+ * joining the lines represents the path for constant centripetal acceleration.
+ * This creates a deviation from the path (let's call this delta),
+ * which is the distance from the junction to the edge of the circular
+ * segment. Delta needs to be defined, so let's replace the term max_jerk (see
+ * note 1) with max_junction_deviation, or "delta". This indirectly sets the
+ * radius of the circle, and hence limits the velocity by the centripetal
+ * acceleration. Think of the this as widening the race track. If a race car is
+ * driving on a track only as wide as a car, it'll have to slow down a lot to
+ * turn corners. If we widen the track a bit, the car can start to use the
+ * track to go into the turn. The wider it is, the faster through the corner
+ * it can go.
+ *
+ * Note 1: "max_jerk" refers to the old grbl/marlin "max_jerk" approximation
+ * term, not the TinyG jerk terms.
+ *
+ * If you do the geometry in terms of the known variables, you get:
+ *
+ * sin(theta/2) = R / (R + delta)
+ *
+ * Re-arranging in terms of circle radius (R)
+ *
+ * R = delta * sin(theta/2) / (1 - sin(theta/2))
+ *
+ * Theta is the angle between line segments given by:
+ *
+ * cos(theta) = dot(a, b) / (norm(a) * norm(b))
+ *
+ * Most of these calculations are already done in the planner.
+ * To remove the acos() and sin() computations, use the trig half
+ * angle identity:
+ *
+ * sin(theta/2) = +/-sqrt((1 - cos(theta)) / 2)
+ *
+ * For our applications, this should always be positive. Now just plug the
+ * equations into the centripetal acceleration equation:
+ *
+ * v_c = sqrt(a_max * R)
+ *
+ * You'll see that there are only two sqrt computations and no sine/cosines.
+ *
+ * How to compute the radius using brute-force trig:
+ *
+ * float theta = acos(costheta);
+ * float radius = delta * sin(theta/2) / (1 - sin(theta/2));
+ *
+ * This version extends Chamnit's algorithm by computing a value for delta that
+ * takes the contributions of the individual axes in the move into account.
+ * This allows the control radius to vary by axis. This is necessary to
+ * support axes that have different dynamics; such as a Z axis that doesn't
+ * move as fast as X and Y (such as a screw driven Z axis on machine with a belt
+ * driven XY - like a Shapeoko), or rotary axes ABC that have completely
+ * different dynamics than their linear counterparts.
+ *
+ * The function takes the absolute values of the sum of the unit vector
+ * components as a measure of contribution to the move, then scales the delta
+ * values from the non-zero axes into a composite delta to be used for the
+ * move. Shown for an XY vector:
+ *
+ * U[i] Unit sum of i'th axis fabs(unit_a[i]) + fabs(unit_b[i])
+ * Usum Length of sums Ux + Uy
+ * d Delta of sums (Dx * Ux + DY * UY) / Usum
+ */
+static float _get_junction_vmax(const float a_unit[], const float b_unit[]) {
+ float costheta = 0;
+ for (int axis = 0; axis < AXES; axis++)
+ costheta -= a_unit[axis] * b_unit[axis];
+
+ if (costheta < -0.99) return 10000000; // straight line cases
+ if (0.99 < costheta) return 0; // reversal cases
+
+ // Fuse the junction deviations into a vector sum
+ float a_delta = 0;
+ float b_delta = 0;
+
+ for (int axis = 0; axis < AXES; axis++) {
+ a_delta += square(a_unit[axis] * axis_get_junction_dev(axis));
+ b_delta += square(b_unit[axis] * axis_get_junction_dev(axis));
+ }
+
+ if (!a_delta || !b_delta) return 0; // One or both unit vectors are null
+
+ float delta = (sqrt(a_delta) + sqrt(b_delta)) / 2;
+ float sintheta_over2 = sqrt((1 - costheta) / 2);
+ float radius = delta * sintheta_over2 / (1 - sintheta_over2);
+ float velocity = sqrt(radius * JUNCTION_ACCELERATION);
+
+ return velocity;
+}
+
+
+/* Find the axis for which the jerk cannot be exceeded - the 'jerk_axis'.
+ * This is the axis for which the time to decelerate from the target velocity
+ * to zero would be the longest.
+ *
+ * We can determine the "longest" deceleration in terms of time or distance.
+ *
+ * The math for time-to-decelerate T from speed S to speed E with constant
+ * jerk J is:
+ *
+ * T = 2 * sqrt((S - E) / J)
+ *
+ * Since E is always zero in this calculation, we can simplify:
+ *
+ * T = 2 * sqrt(S / J)
+ *
+ * The math for distance-to-decelerate l from speed S to speed E with
+ * constant jerk J is:
+ *
+ * l = (S + E) * sqrt((S - E) / J)
+ *
+ * Since E is always zero in this calculation, we can simplify:
+ *
+ * l = S * sqrt(S / J)
+ *
+ * The final value we want to know is which one is *longest*, compared to the
+ * others, so we don't need the actual value. This means that the scale of
+ * the value doesn't matter, so for T we can remove the "2 *" and for L we can
+ * remove the "S *". This leaves both to be simply "sqrt(S / J)". Since we
+ * don't need the scale, it doesn't matter what speed we're coming from, so S
+ * can be replaced with 1.
+ *
+ * However, we *do* need to compensate for the fact that each axis contributes
+ * differently to the move, so we will scale each contribution C[n] by the
+ * proportion of the axis movement length D[n]. Using that, we construct the
+ * following, with these definitions:
+ *
+ * J[n] = max_jerk for axis n
+ * D[n] = distance traveled for this move for axis n
+ * C[n] = "axis contribution" of axis n
+ *
+ * For each axis n:
+ *
+ * C[n] = sqrt(1 / J[n]) * D[n]
+ *
+ * Keeping in mind that we only need a rank compared to the other axes, we can
+ * further optimize the calculations:
+ *
+ * Square the expression to remove the square root:
+ *
+ * C[n]^2 = 1 / J[n] * D[n]^2
+ *
+ * We don't care that C is squared, so we'll use it that way. Also note that
+ * we already have 1 / J[n] calculated for each axis.
+ */
+int mp_find_jerk_axis(const float axis_square[]) {
+ float C;
+ float maxC = 0;
+ int jerk_axis = 0;
+
+ for (int axis = 0; axis < AXES; axis++)
+ if (axis_square[axis]) { // Do not use fp_ZERO here
+ // Squaring axis_length ensures it's positive
+ C = axis_square[axis] * axis_get_recip_jerk(axis);
+
+ if (maxC < C) {
+ maxC = C;
+ jerk_axis = axis;
+ }
+ }
+
+ return jerk_axis;
+}
+
+
+/// Determine jerk value to use for the block.
+static float _calc_jerk(const float axis_square[], const float unit[]) {
+ int axis = mp_find_jerk_axis(axis_square);
+
+ // Finally, the selected jerk term needs to be scaled by the
+ // reciprocal of the absolute value of the axis's unit
+ // vector term. This way when the move is finally decomposed into
+ // its constituent axes for execution the jerk for that axis will be
+ // at it's maximum value.
+ return axis_get_jerk_max(axis) * JERK_MULTIPLIER / fabs(unit[axis]);
+}
+
+
+/// Compute cached jerk terms used by planning
+static void _calc_and_cache_jerk_values(mp_buffer_t *bf) {
+ static float jerk = 0;
+ static float cbrt_jerk = 0;
+
+ if (JERK_MATCH_PRECISION < fabs(bf->jerk - jerk)) { // Tolerance comparison
+ jerk = bf->jerk;
+ cbrt_jerk = cbrt(bf->jerk);
+ }
+
+ bf->cbrt_jerk = cbrt_jerk;
+}
+
+
+static void _calc_max_velocities(mp_buffer_t *bf, float move_time,
+ bool exact_stop) {
+ float junction_velocity =
+ _get_junction_vmax(mp_buffer_prev(bf)->unit, bf->unit);
+
+ bf->cruise_vmax = bf->length / move_time; // target velocity requested
+ bf->entry_vmax = min(bf->cruise_vmax, junction_velocity);
+ bf->delta_vmax = mp_get_target_velocity(0, bf->length, bf);
+ bf->exit_vmax = min(bf->cruise_vmax, (bf->entry_vmax + bf->delta_vmax));
+ bf->braking_velocity = bf->delta_vmax;
+
+ // Zero out exact stop cases
+ if (exact_stop) bf->entry_vmax = bf->exit_vmax = 0;
+ else bf->replannable = true;
+}
+
+
+/* Compute optimal and minimum move times
+ *
+ * "Minimum time" is the fastest the move can be performed given the velocity
+ * constraints on each participating axis - regardless of the feed rate
+ * requested. The minimum time is the time limited by the rate-limiting
+ * axis. The minimum time is needed to compute the optimal time and is recorded
+ * for possible feed override computation.
+ *
+ * "Optimal time" is either the time resulting from the requested feed rate or
+ * the minimum time if the requested feed rate is not achievable. Optimal times
+ * for rapids are always the minimum time.
+ *
+ * The following times are compared and the longest is returned:
+ * - G93 inverse time (if G93 is active)
+ * - time for coordinated move at requested feed rate
+ * - time that the slowest axis would require for the move
+ *
+ * NIST RS274NGC_v3 Guidance
+ *
+ * The following is verbatim text from NIST RS274NGC_v3. As I interpret A for
+ * moves that combine both linear and rotational movement, the feed rate should
+ * apply to the XYZ movement, with the rotational axis (or axes) timed to start
+ * and end at the same time the linear move is performed. It is possible under
+ * this case for the rotational move to rate-limit the linear move.
+ *
+ * 2.1.2.5 Feed Rate
+ *
+ * The rate at which the controlled point or the axes move is nominally a steady
+ * rate which may be set by the user. In the Interpreter, the interpretation of
+ * the feed rate is as follows unless inverse time feed rate mode is being used
+ * in the RS274/NGC view (see Section 3.5.19). The machining functions view of
+ * feed rate, as described in Section 4.3.5.1, has conditions under which the
+ * set feed rate is applied differently, but none of these is used in the
+ * Interpreter.
+ *
+ * A. For motion involving one or more of the X, Y, and Z axes (with or without
+ * simultaneous rotational axis motion), the feed rate means length units
+ * per minute along the programmed XYZ path, as if the rotational axes were
+ * not moving.
+ *
+ * B. For motion of one rotational axis with X, Y, and Z axes not moving, the
+ * feed rate means degrees per minute rotation of the rotational axis.
+ *
+ * C. For motion of two or three rotational axes with X, Y, and Z axes not
+ * moving, the rate is applied as follows. Let dA, dB, and dC be the angles
+ * in degrees through which the A, B, and C axes, respectively, must move.
+ * Let D = sqrt(dA^2 + dB^2 + dC^2). Conceptually, D is a measure of total
+ * angular motion, using the usual Euclidean metric. Let T be the amount of
+ * time required to move through D degrees at the current feed rate in
+ * degrees per minute. The rotational axes should be moved in coordinated
+ * linear motion so that the elapsed time from the start to the end of the
+ * motion is T plus any time required for acceleration or deceleration.
+ */
+static float _calc_move_time(const float axis_length[],
+ const float axis_square[], bool rapid,
+ bool inverse_time, float feed_rate,
+ float feed_override) {
+ float max_time = 0;
+
+ // Compute times for feed motion
+ if (!rapid) {
+ if (inverse_time) max_time = feed_rate;
+ else {
+ // Compute length of linear move in millimeters. Feed rate in mm/min.
+ max_time = sqrt(axis_square[AXIS_X] + axis_square[AXIS_Y] +
+ axis_square[AXIS_Z]) / feed_rate;
+
+ // If no linear axes, compute length of multi-axis rotary move in degrees.
+ // Feed rate is provided as degrees/min
+ if (fp_ZERO(max_time))
+ max_time = sqrt(axis_square[AXIS_A] + axis_square[AXIS_B] +
+ axis_square[AXIS_C]) / feed_rate;
+ }
+ }
+
+ // Apply feed override
+ max_time /= feed_override;
+
+ // Compute time required for rate-limiting axis
+ for (int axis = 0; axis < AXES; axis++) {
+ float time = fabs(axis_length[axis]) /
+ (rapid ? axis_get_velocity_max(axis) : axis_get_feedrate_max(axis));
+
+ if (max_time < time) max_time = time;
+ }
+
+ return max_time < MIN_SEGMENT_TIME ? MIN_SEGMENT_TIME : max_time;
+}
+
+
+/*** Plan line acceleration / deceleration
+ *
+ * This function uses constant jerk motion equations to plan acceleration and
+ * deceleration. Jerk is the rate of change of acceleration; it's the 1st
+ * derivative of acceleration, and the 3rd derivative of position. Jerk is a
+ * measure of impact to the machine. Controlling jerk smooths transitions
+ * between moves and allows for faster feeds while controlling machine
+ * oscillations and other undesirable side-effects.
+ *
+ * Notes:
+ * [1] All math is done in absolute coordinates using single precision floats.
+ *
+ * [2] 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.
+ *
+ * @param reed_rate is in minutes when @param inverse_time is true.
+ * See mach_set_feed_rate()
+ */
+stat_t mp_aline(const float target[], bool rapid, bool inverse_time,
+ bool exact_stop, float feed_rate, float feed_override,
+ int32_t line) {
+ // Compute axis and move lengths
+ float axis_length[AXES];
+ float axis_square[AXES];
+ float length_square = 0;
+
+ for (int axis = 0; axis < AXES; axis++) {
+ axis_length[axis] = target[axis] - mp_get_axis_position(axis);
+ axis_square[axis] = square(axis_length[axis]);
+ length_square += axis_square[axis];
+ }
+
+ float length = sqrt(length_square);
+ if (fp_ZERO(length)) return STAT_OK;
+
+ // Get a buffer. Note, new buffers are initialized to zero.
+ mp_buffer_t *bf = mp_queue_get_tail(); // current move pointer
+
+ // Set buffer values
+ bf->length = length;
+ copy_vector(bf->target, target);
+
+ // Compute unit vector
+ for (int axis = 0; axis < AXES; axis++)
+ bf->unit[axis] = axis_length[axis] / length;
+
+ // Compute and cache jerk values
+ bf->jerk = _calc_jerk(axis_square, bf->unit);
+ _calc_and_cache_jerk_values(bf);
+
+ // Compute move time and velocities
+ float time = _calc_move_time(axis_length, axis_square, rapid, inverse_time,
+ feed_rate, feed_override);
+ _calc_max_velocities(bf, time, exact_stop);
+
+ // Note, the following lines must remain in order.
+ bf->line = line; // Planner needs then when planning steps
+ mp_plan(bf); // Plan block list
+ mp_set_position(target); // Set planner position before committing buffer
+ mp_queue_push(mp_exec_aline, line); // After position update
+
+ return STAT_OK;
+}
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2016 Buildbotics LLC
+ All rights reserved.
+
+ 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/>.
+
+ The software is distributed in the hope that it will be useful, but
+ WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public
+ License along with the software. If not, see
+ <http://www.gnu.org/licenses/>.
+
+ For information regarding this software email:
+ "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#pragma once
+
+#include "status.h"
+
+#include <stdbool.h>
+
+
+stat_t mp_aline(const float target[], bool rapid, bool inverse_time,
+ bool exact_stop, float feed_rate, float feed_override,
+ int32_t line);
+int mp_find_jerk_axis(const float axis_square[]);
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2016 Buildbotics LLC
+ Copyright (c) 2010 - 2015 Alden S. Hart, Jr.
+ Copyright (c) 2012 - 2015 Rob Giseburt
+ All rights reserved.
+
+ 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/>.
+
+ The software is distributed in the hope that it will be useful, but
+ WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public
+ License along with the software. If not, see
+ <http://www.gnu.org/licenses/>.
+
+ For information regarding this software email:
+ "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+/* Planner Notes
+ *
+ * The planner works below the machine and above the
+ * motor mapping and stepper execution layers. A rudimentary
+ * multitasking capability is implemented for long-running commands
+ * such as lines, arcs, and dwells. These functions are coded as
+ * non-blocking continuations - which are simple state machines
+ * that are re-entered multiple times until a particular operation
+ * is complete. These functions have 2 parts - the initial call,
+ * which sets up the local context, and callbacks (continuations)
+ * that are called from the main loop.
+ *
+ * One important concept is isolation of the three layers of the
+ * data model - the Gcode model (gm), planner model (bf queue & mm),
+ * and runtime model (exec.c).
+ *
+ * The Gcode model is owned by the machine and should only be
+ * accessed by mach_xxxx() functions. Data from the Gcode model is
+ * transferred to the planner by the mp_xxx() functions called by
+ * the machine.
+ *
+ * The planner should only use data in the planner model. When a
+ * move (block) is ready for execution the planner data is
+ * transferred to the runtime model, which should also be isolated.
+ *
+ * Lower-level models should never use data from upper-level models
+ * as the data may have changed and lead to unpredictable results.
+ */
+
+#include "planner.h"
+
+#include "axis.h"
+#include "buffer.h"
+#include "machine.h"
+#include "stepper.h"
+#include "motor.h"
+#include "state.h"
+#include "usart.h"
+
+#include <string.h>
+#include <stdbool.h>
+#include <stdio.h>
+
+
+typedef struct {
+ float position[AXES]; // final move position for planning purposes
+ bool plan_steps; // if true plan one GCode line at a time
+} planner_t;
+
+
+static planner_t mp = {{0}};
+
+
+void mp_init() {mp_queue_init();}
+
+
+/// Set planner position for a single axis
+void mp_set_axis_position(int axis, float p) {mp.position[axis] = p;}
+float mp_get_axis_position(int axis) {return mp.position[axis];}
+void mp_set_position(const float p[]) {copy_vector(mp.position, p);}
+void mp_set_plan_steps(bool plan_steps) {mp.plan_steps = plan_steps;}
+
+
+/*** Flush all moves in the planner
+ *
+ * Does not affect the move currently running. 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 usually
+ * be directly called. Call mp_request_flush() instead.
+ */
+void mp_flush_planner() {mp_queue_init();}
+
+
+/*** Performs axis mapping & conversion of length units to steps.
+ *
+ * The reason steps are returned as floats (as opposed to, say,
+ * uint32_t) is to accommodate fractional steps. stepper.c deals
+ * with fractional step values as fixed-point binary in order to get
+ * the smoothest possible operation. Steps are passed to the move prep
+ * routine as floats and converted to fixed-point binary during queue
+ * loading. See stepper.c for details.
+ */
+void mp_kinematics(const float travel[], float steps[]) {
+ // You could insert inverse kinematics transformations here
+ // or just use travel directly for Cartesian machines.
+
+ // Map motors to axes and convert length units to steps
+ // Most of the conversion math has already been done during config in
+ // steps_per_unit() which takes axis travel, step angle and microsteps into
+ // account.
+ for (int motor = 0; motor < MOTORS; motor++)
+ steps[motor] =
+ travel[motor_get_axis(motor)] * motor_get_steps_per_unit(motor);
+}
+
+
+// The minimum lengths are dynamic and depend on the velocity. These
+// expressions evaluate to the minimum lengths for the current velocity
+// settings. Note: The head and tail lengths are 2 minimum segments, the body
+// is 1 min segment.
+#define MIN_HEAD_LENGTH \
+ (MIN_SEGMENT_TIME_PLUS_MARGIN * (bf->cruise_velocity + bf->entry_velocity))
+#define MIN_TAIL_LENGTH \
+ (MIN_SEGMENT_TIME_PLUS_MARGIN * (bf->cruise_velocity + bf->exit_velocity))
+#define MIN_BODY_LENGTH (MIN_SEGMENT_TIME_PLUS_MARGIN * bf->cruise_velocity)
+
+
+/*** Calculate move acceleration / deceleration
+ *
+ * This rather brute-force and long-ish function sets section lengths and
+ * velocities based on the line length and velocities requested. It modifies
+ * the incoming bf buffer and returns accurate head, body and tail lengths, and
+ * accurate or reasonably approximate velocities. We care about accuracy on
+ * lengths, less so for velocity, as long as velocity errs on the side of too
+ * slow.
+ *
+ * Note: We need the velocities to be set even for zero-length sections (Note:
+ * sections, not moves) so we can compute entry and exits for adjacent sections.
+ *
+ * Inputs used are:
+ *
+ * bf->length - actual block length, length is never changed
+ * bf->entry_velocity - requested Ve, entry velocity is never changed
+ * bf->cruise_velocity - requested Vt, is often changed
+ * bf->exit_velocity - requested Vx, may change for degenerate cases
+ * bf->cruise_vmax - used in some comparisons
+ * bf->delta_vmax - used to degrade velocity of short blocks
+ *
+ * Variables that may be set/updated are:
+ *
+ * bf->entry_velocity - requested Ve
+ * bf->cruise_velocity - requested Vt
+ * bf->exit_velocity - requested Vx
+ * bf->head_length - bf->length allocated to head
+ * bf->body_length - bf->length allocated to body
+ * bf->tail_length - bf->length allocated to tail
+ *
+ * Note: The following conditions must be met on entry:
+ *
+ * bf->length must be non-zero (filter these out upstream)
+ * bf->entry_velocity <= bf->cruise_velocity >= bf->exit_velocity
+ *
+ * Classes of moves:
+ *
+ * Requested-Fit - The move has sufficient length to achieve the target
+ * velocity (cruise velocity). I.e it will accommodate the acceleration /
+ * deceleration profile in the given length.
+ *
+ * Rate-Limited-Fit - The move does not have sufficient length to achieve
+ * target velocity. In this case the cruise velocity will be set lower than
+ * the requested velocity (incoming bf->cruise_velocity). The entry and
+ * exit velocities are satisfied.
+ *
+ * Degraded-Fit - The move does not have sufficient length to transition from
+ * the entry velocity to the exit velocity in the available length. These
+ * velocities are not negotiable, so a degraded solution is found.
+ *
+ * In worst cases, the move cannot be executed as the required execution
+ * time is less than the minimum segment time. The first degradation is to
+ * reduce the move to a body-only segment with an average velocity. If that
+ * still doesn't fit then the move velocity is reduced so it fits into a
+ * minimum segment. This will reduce the velocities in that region of the
+ * planner buffer as the moves are replanned to that worst-case move.
+ *
+ * Various cases handled (H=head, B=body, T=tail)
+ *
+ * Requested-Fit cases:
+ *
+ * HBT Ve<Vt>Vx sufficient length exists for all parts (corner
+ * case: HBT')
+ * HB Ve<Vt=Vx head accelerates to cruise - exits at full speed
+ * (corner case: H')
+ * BT Ve=Vt>Vx enter at full speed & decelerate (corner case: T')
+ * HT Ve & Vx perfect fit HT (very rare). May be symmetric or
+ * asymmetric
+ * H Ve<Vx perfect fit H (common, results from planning)
+ * T Ve>Vx perfect fit T (common, results from planning)
+ * B Ve=Vt=Vx Velocities are close to each other and within
+ * matching tolerance
+ *
+ * Rate-Limited cases - Ve and Vx can be satisfied but Vt cannot:
+ *
+ * HT (Ve=Vx)<Vt symmetric case. Split the length and compute Vt.
+ * HT' (Ve!=Vx)<Vt asymmetric case. Find H and T by successive
+ * approximation.
+ * HBT' body length < min body length - treated as an HT case
+ * H' body length < min body length - subsume body into head length
+ * T' body length < min body length - subsume body into tail length
+ *
+ * Degraded fit cases - line is too short to satisfy both Ve and Vx:
+ *
+ * H" Ve<Vx Ve is degraded (velocity step). Vx is met
+ * T" Ve>Vx Ve is degraded (velocity step). Vx is met
+ * B" <short> line is very short but drawable; is treated as a
+ * body only
+ * F <too short> force fit: This block is slowed down until it can
+ * be executed
+ *
+ * Note: The order of the cases/tests in the code is important. Start with
+ * the shortest cases first and work up. Not only does this simplify the order
+ * of the tests, but it reduces execution time when you need it most - when
+ * tons of pathologically short Gcode blocks are being thrown at you.
+ */
+void mp_calculate_trapezoid(mp_buffer_t *bf) {
+ // RULE #1 of mp_calculate_trapezoid(): Don't change bf->length
+
+ if (!bf->length) return;
+
+ // F case: Block is too short - run time < minimum segment time
+ // Force block into a single segment body with limited velocities
+ // Accept the entry velocity, limit the cruise, and go for the best exit
+ // velocity you can get given the delta_vmax (maximum velocity slew).
+
+ float naive_move_time =
+ 2 * bf->length / (bf->entry_velocity + bf->exit_velocity); // average
+
+ if (naive_move_time < MIN_SEGMENT_TIME_PLUS_MARGIN) {
+ bf->cruise_velocity = bf->length / MIN_SEGMENT_TIME_PLUS_MARGIN;
+ bf->exit_velocity =
+ max(0, min(bf->cruise_velocity, (bf->entry_velocity - bf->delta_vmax)));
+ bf->body_length = bf->length;
+ bf->head_length = 0;
+ bf->tail_length = 0;
+
+ // Violating jerk but since it's a single segment move we don't use it.
+ return;
+ }
+
+ // B" case: Block is short, but fits into a single body segment
+ if (naive_move_time <= NOM_SEGMENT_TIME) {
+ bf->entry_velocity = mp_buffer_prev(bf)->exit_velocity;
+
+ if (fp_NOT_ZERO(bf->entry_velocity)) {
+ bf->cruise_velocity = bf->entry_velocity;
+ bf->exit_velocity = bf->entry_velocity;
+
+ } else {
+ bf->cruise_velocity = bf->delta_vmax / 2;
+ bf->exit_velocity = bf->delta_vmax;
+ }
+
+ bf->body_length = bf->length;
+ bf->head_length = 0;
+ bf->tail_length = 0;
+
+ // Violating jerk but since it's a single segment move we don't use it.
+ return;
+ }
+
+ // B case: Velocities all match (or close enough)
+ // This occurs frequently in normal gcode files with lots of short lines.
+ // This case is not really necessary, but saves lots of processing time.
+ if (((bf->cruise_velocity - bf->entry_velocity) <
+ TRAPEZOID_VELOCITY_TOLERANCE) &&
+ ((bf->cruise_velocity - bf->exit_velocity) <
+ TRAPEZOID_VELOCITY_TOLERANCE)) {
+ bf->body_length = bf->length;
+ bf->head_length = 0;
+ bf->tail_length = 0;
+
+ return;
+ }
+
+ // Head-only and tail-only short-line cases
+ // H" and T" degraded-fit cases
+ // H' and T' requested-fit cases where the body residual is less than
+ // MIN_BODY_LENGTH
+ bf->body_length = 0;
+ float minimum_length =
+ mp_get_target_length(bf->entry_velocity, bf->exit_velocity, bf->jerk);
+
+ // head-only & tail-only cases
+ if (bf->length <= (minimum_length + MIN_BODY_LENGTH)) {
+ // tail-only cases (short decelerations)
+ if (bf->entry_velocity > bf->exit_velocity) {
+ // T" (degraded case)
+ if (bf->length < minimum_length)
+ bf->entry_velocity =
+ mp_get_target_velocity(bf->exit_velocity, bf->length, bf);
+
+ bf->cruise_velocity = bf->entry_velocity;
+ bf->tail_length = bf->length;
+ bf->head_length = 0;
+
+ return;
+ }
+
+ // head-only cases (short accelerations)
+ if (bf->entry_velocity < bf->exit_velocity) {
+ // H" (degraded case)
+ if (bf->length < minimum_length)
+ bf->exit_velocity =
+ mp_get_target_velocity(bf->entry_velocity, bf->length, bf);
+
+ bf->cruise_velocity = bf->exit_velocity;
+ bf->head_length = bf->length;
+ bf->tail_length = 0;
+
+ return;
+ }
+ }
+
+ // Set head and tail lengths for evaluating the next cases
+ bf->head_length =
+ mp_get_target_length(bf->entry_velocity, bf->cruise_velocity, bf->jerk);
+ bf->tail_length =
+ mp_get_target_length(bf->exit_velocity, bf->cruise_velocity, bf->jerk);
+ if (bf->head_length < MIN_HEAD_LENGTH) { bf->head_length = 0;}
+ if (bf->tail_length < MIN_TAIL_LENGTH) { bf->tail_length = 0;}
+
+ // Rate-limited HT and HT' cases
+ if (bf->length < (bf->head_length + bf->tail_length)) { // it's rate limited
+
+ // Symmetric rate-limited case (HT)
+ if (fabs(bf->entry_velocity - bf->exit_velocity) <
+ TRAPEZOID_VELOCITY_TOLERANCE) {
+ bf->head_length = bf->length/2;
+ bf->tail_length = bf->head_length;
+ bf->cruise_velocity =
+ min(bf->cruise_vmax,
+ mp_get_target_velocity(bf->entry_velocity, bf->head_length, bf));
+
+ if (bf->head_length < MIN_HEAD_LENGTH) {
+ // Convert this to a body-only move
+ bf->body_length = bf->length;
+ bf->head_length = 0;
+ bf->tail_length = 0;
+
+ // Average the entry speed and computed best cruise-speed
+ bf->cruise_velocity = (bf->entry_velocity + bf->cruise_velocity)/2;
+ bf->entry_velocity = bf->cruise_velocity;
+ bf->exit_velocity = bf->cruise_velocity;
+ }
+
+ return;
+ }
+
+ // Asymmetric HT' rate-limited case. This is relatively expensive but it's
+ // not called very often
+ float computed_velocity = bf->cruise_vmax;
+ do {
+ // initialize from previous iteration
+ bf->cruise_velocity = computed_velocity;
+ bf->head_length =
+ mp_get_target_length(bf->entry_velocity, bf->cruise_velocity, bf->jerk);
+ bf->tail_length =
+ mp_get_target_length(bf->exit_velocity, bf->cruise_velocity, bf->jerk);
+
+ if (bf->head_length > bf->tail_length) {
+ bf->head_length =
+ (bf->head_length / (bf->head_length + bf->tail_length)) * bf->length;
+ computed_velocity =
+ mp_get_target_velocity(bf->entry_velocity, bf->head_length, bf);
+
+ } else {
+ bf->tail_length =
+ (bf->tail_length / (bf->head_length + bf->tail_length)) * bf->length;
+ computed_velocity =
+ mp_get_target_velocity(bf->exit_velocity, bf->tail_length, bf);
+ }
+
+ } while ((fabs(bf->cruise_velocity - computed_velocity) /
+ computed_velocity) > TRAPEZOID_ITERATION_ERROR_PERCENT);
+
+ // set velocity and clean up any parts that are too short
+ bf->cruise_velocity = computed_velocity;
+ bf->head_length =
+ mp_get_target_length(bf->entry_velocity, bf->cruise_velocity, bf->jerk);
+ bf->tail_length = bf->length - bf->head_length;
+
+ if (bf->head_length < MIN_HEAD_LENGTH) {
+ // adjust the move to be all tail...
+ bf->tail_length = bf->length;
+ bf->head_length = 0;
+ }
+
+ if (bf->tail_length < MIN_TAIL_LENGTH) {
+ bf->head_length = bf->length; //...or all head
+ bf->tail_length = 0;
+ }
+
+ return;
+ }
+
+ // Requested-fit cases: remaining of: HBT, HB, BT, BT, H, T, B, cases
+ bf->body_length = bf->length - bf->head_length - bf->tail_length;
+
+ // If a non-zero body is < minimum length distribute it to the head and/or
+ // tail. This will generate small (acceptable) velocity errors in runtime
+ // execution but preserve correct distance, which is more important.
+ if ((bf->body_length < MIN_BODY_LENGTH) && (fp_NOT_ZERO(bf->body_length))) {
+ if (fp_NOT_ZERO(bf->head_length)) {
+ if (fp_NOT_ZERO(bf->tail_length)) { // HBT reduces to HT
+ bf->head_length += bf->body_length / 2;
+ bf->tail_length += bf->body_length / 2;
+
+ } else bf->head_length += bf->body_length; // HB reduces to H
+ } else bf->tail_length += bf->body_length; // BT reduces to T
+
+ bf->body_length = 0;
+
+ // If the body is a standalone make the cruise velocity match the entry
+ // velocity. This removes a potential velocity discontinuity at the expense
+ // of top speed
+ } else if ((fp_ZERO(bf->head_length)) && (fp_ZERO(bf->tail_length)))
+ bf->cruise_velocity = bf->entry_velocity;
+}
+
+
+#if 0
+/// Prints the entire planning queue as comma separated values embedded in
+/// JSON ``msg`` entries. Used for debugging.
+void mp_print_queue(mp_buffer_t *bf) {
+ printf_P(PSTR("{\"msg\":\"id,replannable,callback,"
+ "length,head_length,body_length,tail_length,"
+ "entry_velocity,cruise_velocity,exit_velocity,braking_velocity,"
+ "entry_vmax,cruise_vmax,exit_vmax\"}\n"));
+
+ int i = 0;
+ mp_buffer_t *bp = bf;
+ while (bp) {
+ printf_P(PSTR("{\"msg\":\"%d,%d,0x%04x,"
+ "%0.2f,%0.2f,%0.2f,%0.2f,"
+ "%0.2f,%0.2f,%0.2f,%0.2f,"
+ "%0.2f,%0.2f,%0.2f\"}\n"),
+ i++, bp->replannable, bp->cb,
+ bp->length, bp->head_length, bp->body_length, bp->tail_length,
+ bp->entry_velocity, bp->cruise_velocity, bp->exit_velocity,
+ bp->braking_velocity,
+ bp->entry_vmax, bp->cruise_vmax, bp->exit_vmax);
+
+ bp = mp_buffer_prev(bp);
+ if (bp == bf || bp->state == BUFFER_OFF) break;
+ }
+
+ while (!usart_tx_empty()) continue;
+}
+#endif
+
+
+/*** Plans the entire queue
+ *
+ * The block list is the circular buffer of planner buffers (bl's). The block
+ * currently being planned is the "bl" block. The "first block" is the next
+ * block to execute; queued immediately behind the currently executing block,
+ * aka the "running" block. In some cases, there is no first block because the
+ * list is empty or there is only one block and it is already running.
+ *
+ * If blocks following the first block are already optimally planned (non
+ * replannable) the first block that is not optimally planned becomes the
+ * effective first block.
+ *
+ * mp_plan() plans all blocks between and including the (effective)
+ * first block and the bl. It sets entry, exit and cruise v's from vmax's then
+ * calls trapezoid generation.
+ *
+ * Variables that must be provided in the mp_buffer_t that will be processed:
+ *
+ * bl (function arg) - end of block list (last block in time)
+ * bl->replannable - start of block list set by last FALSE value
+ * [Note 1]
+ * bl->move_type - typically MOVE_TYPE_ALINE. Other move_types should
+ * be set to length=0, entry_vmax=0 and exit_vmax=0
+ * and are treated as a momentary stop (plan to zero
+ * and from zero).
+ * bl->length - provides block length
+ * bl->entry_vmax - used during forward planning to set entry velocity
+ * bl->cruise_vmax - used during forward planning to set cruise velocity
+ * bl->exit_vmax - used during forward planning to set exit velocity
+ * bl->delta_vmax - used during forward planning to set exit velocity
+ * bl->cbrt_jerk - used during trapezoid generation
+ *
+ * Variables that will be set during processing:
+ *
+ * bl->replannable - set if the block becomes optimally planned
+ * bl->braking_velocity - set during backward planning
+ * bl->entry_velocity - set during forward planning
+ * bl->cruise_velocity - set during forward planning
+ * bl->exit_velocity - set during forward planning
+ * bl->head_length - set during trapezoid generation
+ * bl->body_length - set during trapezoid generation
+ * bl->tail_length - set during trapezoid generation
+ *
+ * Variables that are ignored but here's what you would expect them to be:
+ *
+ * bl->state - BUFFER_NEW for all blocks but the earliest
+ * bl->target[] - block target position
+ * bl->unit[] - block unit vector
+ * bl->jerk - source of the other jerk variables.
+ *
+ * Notes:
+ *
+ * [1] Whether or not a block is planned is controlled by the bl->replannable
+ * setting. Replan flags are checked during the backwards pass. They prune
+ * the replan list to include only the latest blocks that require planning.
+ *
+ * In normal operation, the first block (currently running block) is not
+ * replanned, but may be for feedholds and feed overrides. In these cases,
+ * the prep routines modify the contents of the (ex) buffer and re-shuffle
+ * the block list, re-enlisting the current bl buffer with new parameters.
+ * These routines also set all blocks in the list to be replannable so the
+ * list can be recomputed regardless of exact stops and previous replanning
+ * optimizations.
+ */
+void mp_plan(mp_buffer_t *bl) {
+ mp_buffer_t *bp = bl;
+
+ // Backward planning pass. Find first block and update braking velocities.
+ // By the end bp points to the buffer before the first block.
+ mp_buffer_t *next = bp;
+ while ((bp = mp_buffer_prev(bp)) != bl) {
+ if (!bp->replannable) break;
+
+ bp->braking_velocity =
+ min(next->entry_vmax, next->braking_velocity) + bp->delta_vmax;
+
+ next = bp;
+ }
+
+ // Forward planning pass. Recompute trapezoids from the first block to bl.
+ mp_buffer_t *prev = bp;
+ while ((bp = mp_buffer_next(bp)) != bl) {
+ mp_buffer_t *next = mp_buffer_next(bp);
+
+ bp->entry_velocity = prev == bl ? bp->entry_vmax : prev->exit_velocity;
+ bp->cruise_velocity = bp->cruise_vmax;
+ bp->exit_velocity = min4(bp->exit_vmax, next->entry_vmax,
+ next->braking_velocity,
+ bp->entry_velocity + bp->delta_vmax);
+
+ if (mp.plan_steps && bp->line != next->line) {
+ bp->exit_velocity = 0;
+ bp->hold = true;
+
+ } else bp->hold = false;
+
+ mp_calculate_trapezoid(bp);
+
+ // Test for optimally planned trapezoids by checking exit conditions
+ if ((fp_EQ(bp->exit_velocity, bp->exit_vmax) ||
+ fp_EQ(bp->exit_velocity, next->entry_vmax)) ||
+ (!prev->replannable &&
+ fp_EQ(bp->exit_velocity, (bp->entry_velocity + bp->delta_vmax))))
+ bp->replannable = false;
+
+ prev = bp;
+ }
+
+ // Finish last block
+ bl->entry_velocity = prev->exit_velocity;
+ bl->cruise_velocity = bl->cruise_vmax;
+ bl->exit_velocity = 0;
+
+ mp_calculate_trapezoid(bl);
+}
+
+
+void mp_replan_all() {
+ ASSERT(mp_get_state() == STATE_READY || mp_get_state() == STATE_HOLDING);
+
+ // Get next buffer
+ mp_buffer_t *bf = mp_queue_get_head();
+ if (!bf) return;
+
+ mp_buffer_t *bp = bf;
+
+ // Mark all blocks replanable
+ while (true) {
+ bp->replannable = true;
+ mp_buffer_t *next = mp_buffer_next(bp);
+ if (next->state == BUFFER_OFF || next == bf) break; // Avoid wrap around
+ bp = next;
+ }
+
+ // Plan blocks
+ mp_plan(bp);
+}
+
+
+/// Push a non-stop command to the queue. I.e. one that does not cause the
+/// planner to plan to zero.
+void mp_queue_push_nonstop(buffer_cb_t cb, uint32_t line) {
+ mp_buffer_t *bp = mp_queue_get_tail();
+
+ bp->entry_vmax = bp->cruise_vmax = bp->exit_vmax = INFINITY;
+ copy_vector(bp->unit, bp->prev->unit);
+ bp->replannable = true;
+
+ mp_queue_push(cb, line);
+}
+
+
+/*** Derive accel/decel length from delta V and jerk
+ *
+ * A convenient function for determining the optimal_length (L) of a line
+ * given the initial velocity (Vi), final velocity (Vf) and maximum jerk (Jm).
+ *
+ * Definitions:
+ *
+ * Jm = the given maximum jerk
+ * T = time of the entire move
+ * Vi = initial velocity
+ * Vf = final velocity
+ * T = time
+ * As = accel at inflection point between convex & concave portions of S-curve
+ * Ar = ramp acceleration
+ *
+ * Formulas:
+ *
+ * T = 2 * sqrt((Vt - Vi) / Jm)
+ * As = (Jm * T) / 2
+ * Ar = As / 2 = (Jm * T) / 4
+ *
+ * Then the length (distance) equation is:
+ *
+ * a) L = (Vf - Vi) * T - (Ar * T^2) / 2
+ *
+ * Substituting for Ar and T:
+ *
+ * b) L = (Vf - Vi) * 2 * sqrt((Vf - Vi) / Jm) -
+ * (2 * sqrt((Vf - Vi) / Jm) * (Vf - Vi)) / 2
+ *
+ * Reducing b). See Wolfram Alpha:
+ *
+ * c) L = (Vf - Vi)^(3/2) / sqrt(Jm)
+ *
+ * Assuming Vf >= Vi [Note 2]:
+ *
+ * d) L = (Vf - Vi) * sqrt((Vf - Vi) / Jm)
+ *
+ * Notes:
+ *
+ * [1] Assuming Vi, Vf and L are positive or zero.
+ * [2] Cannot assume Vf >= Vi due to rounding errors and use of
+ * PLANNER_VELOCITY_TOLERANCE necessitating the introduction of fabs()
+ */
+float mp_get_target_length(float Vi, float Vf, float jerk) {
+ return fp_EQ(Vi, Vf) ? 0 : fabs(Vi - Vf) * invsqrt(jerk / fabs(Vi - Vf));
+}
+
+
+#define GET_VELOCITY_ITERATIONS 0 // must be zero or more
+
+/*** Derive velocity achievable from delta V and length
+ *
+ * A convenient function for determining Vf target velocity for a given
+ * initial velocity (Vi), length (L), and maximum jerk (Jm). Equation e) is
+ * b) solved for Vf. Equation f) is c) solved for Vf. Use f) (obviously)
+ *
+ * e) Vf = (sqrt(L) * (L / sqrt(1 / Jm))^(1/6) +
+ * (1 / Jm)^(1/4) * Vi) / (1 / Jm)^(1/4)
+ *
+ * f) Vf = L^(2/3) * Jm^(1/3) + Vi
+ *
+ * FYI: Here's an expression that returns the jerk for a given deltaV and L:
+ *
+ * cube(deltaV / (pow(L, 0.66666666)));
+ *
+ * We do some Newton-Raphson iterations to narrow it down.
+ * We need a formula that includes known variables except the one we want to
+ * find, and has a root [Z(x) = 0] at the value (x) we are looking for.
+ *
+ * Z(x) = zero at x
+ *
+ * We calculate the value from the knowns and the estimate (see below) and then
+ * subtract the known value to get zero (root) if x is the correct value.
+ *
+ * Vi = initial velocity (known)
+ * Vf = estimated final velocity
+ * J = jerk (known)
+ * L = length (know)
+ *
+ * There are (at least) two such functions we can use:
+ *
+ * L from J, Vi, and Vf:
+ *
+ * L = sqrt((Vf - Vi) / J) * (Vi + Vf)
+ *
+ * Replacing Vf with x, and subtracting the known L we get:
+ *
+ * 0 = sqrt((x - Vi) / J) * (Vi + x) - L
+ * Z(x) = sqrt((x - Vi) / J) * (Vi + x) - L
+ *
+ * Or J from L, Vi, and Vf:
+ *
+ * J = ((Vf - Vi) * (Vi + Vf)^2) / L^2
+ *
+ * Replacing Vf with x, and subtracting the known J we get:
+ *
+ * 0 = ((x - Vi) * (Vi + x)^2) / L^2 - J
+ * Z(x) = ((x - Vi) * (Vi + x)^2) / L^2 - J
+ *
+ * L doesn't resolve to the value very quickly (its graph is nearly vertical).
+ * So, we'll use J, which resolves in < 10 iterations, often in only two or
+ * three with a good estimate.
+ *
+ * In order to do a Newton-Raphson iteration, we need the derivative. Here
+ * they are for both the (unused) L and the (used) J formulas above:
+ *
+ * J > 0, Vi > 0, Vf > 0
+ * A = sqrt((x - Vi) * J)
+ * B = sqrt((x - Vi) / J)
+ *
+ * L'(x) = B + (Vi + x) / (2 * J) + (Vi + x) / (2 * A)
+ *
+ * J'(x) = (2 * Vi * x - Vi^2 + 3 * x^2) / L^2
+ */
+float mp_get_target_velocity(float Vi, float L, const mp_buffer_t *bf) {
+ // 0 iterations (a reasonable estimate)
+ float x = pow(L, 0.66666666) * bf->cbrt_jerk + Vi; // First estimate
+
+#if (GET_VELOCITY_ITERATIONS > 0)
+ const float L2 = L * L;
+ const float Vi2 = Vi * Vi;
+
+ for (int i = 0; i < GET_VELOCITY_ITERATIONS; i++)
+ // x' = x - Z(x) / J'(x)
+ x = x - ((x - Vi) * square(Vi + x) / L2 - bf->jerk) /
+ ((2 * Vi * x - Vi2 + 3 * x * x) / L2);
+#endif
+
+ return x;
+}
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2016 Buildbotics LLC
+ Copyright (c) 2013 - 2015 Alden S. Hart, Jr.
+ Copyright (c) 2013 - 2015 Robert Giseburt
+ All rights reserved.
+
+ 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/>.
+
+ The software is distributed in the hope that it will be useful, but
+ WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public
+ License along with the software. If not, see
+ <http://www.gnu.org/licenses/>.
+
+ For information regarding this software email:
+ "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#pragma once
+
+
+#include "machine.h" // used for gcode_state_t
+#include "buffer.h"
+#include "util.h"
+#include "config.h"
+
+#include <stdbool.h>
+
+
+// Most of these factors are the result of a lot of tweaking.
+// Change with caution.
+#define JERK_MULTIPLIER 1000000.0
+#define JERK_MATCH_PRECISION 1000.0 // jerk precision to be considered same
+
+#define NOM_SEGMENT_TIME (NOM_SEGMENT_USEC / MICROSECONDS_PER_MINUTE)
+#define MIN_SEGMENT_TIME (MIN_SEGMENT_USEC / MICROSECONDS_PER_MINUTE)
+
+#define MIN_SEGMENT_TIME_PLUS_MARGIN \
+ ((MIN_SEGMENT_USEC + 1) / MICROSECONDS_PER_MINUTE)
+
+/// Error percentage for iteration convergence. As percent - 0.01 = 1%
+#define TRAPEZOID_ITERATION_ERROR_PERCENT 0.1
+
+/// Adaptive velocity tolerance term
+#define TRAPEZOID_VELOCITY_TOLERANCE (max(2, bf->entry_velocity / 100))
+
+/*** If the absolute value of the remaining deceleration length would be less
+ * than this value then finish the deceleration in the current move. This is
+ * used to avoid creating segements before or after the hold which are too
+ * short to process correctly.
+ */
+#define HOLD_DECELERATION_TOLERANCE 1 // In mm
+
+
+typedef enum {
+ SECTION_HEAD, // acceleration
+ SECTION_BODY, // cruise
+ SECTION_TAIL, // deceleration
+} move_section_t;
+
+
+void mp_init();
+
+void mp_set_axis_position(int axis, float position);
+float mp_get_axis_position(int axis);
+void mp_set_position(const float p[]);
+void mp_set_plan_steps(bool plan_steps);
+
+void mp_flush_planner();
+void mp_kinematics(const float travel[], float steps[]);
+
+void mp_plan(mp_buffer_t *bf);
+void mp_replan_all();
+
+void mp_queue_push_nonstop(buffer_cb_t cb, uint32_t line);
+
+float mp_get_target_length(float Vi, float Vf, float jerk);
+float mp_get_target_velocity(float Vi, float L, const mp_buffer_t *bf);
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2016 Buildbotics LLC
+ Copyright (c) 2010 - 2015 Alden S. Hart, Jr.
+ Copyright (c) 2012 - 2015 Rob Giseburt
+ All rights reserved.
+
+ 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/>.
+
+ The software is distributed in the hope that it will be useful, but
+ WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public
+ License along with the software. If not, see
+ <http://www.gnu.org/licenses/>.
+
+ For information regarding this software email:
+ "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#include "planner.h"
+#include "buffer.h"
+#include "stepper.h"
+#include "motor.h"
+#include "util.h"
+#include "report.h"
+#include "state.h"
+#include "config.h"
+
+#include <string.h>
+#include <stdbool.h>
+#include <math.h>
+#include <stdio.h>
+
+
+typedef struct {
+ bool busy; // True if a move is running
+ float position[AXES]; // Current move position
+ float work_offset[AXES]; // Current move work offset
+ float velocity; // Current move velocity
+
+ int32_t line; // Current move GCode line number
+ uint8_t tool; // Active tool
+
+ float feed;
+ feed_mode_t feed_mode;
+ float feed_override;
+ float spindle_override;
+
+ plane_t plane;
+ units_t units;
+ coord_system_t coord_system;
+ bool absolute_mode;
+ path_mode_t path_mode;
+ distance_mode_t distance_mode;
+ distance_mode_t arc_distance_mode;
+
+ float previous_error[MOTORS];
+} mp_runtime_t;
+
+static mp_runtime_t rt = {0};
+
+
+bool mp_runtime_is_busy() {return rt.busy;}
+void mp_runtime_set_busy(bool busy) {rt.busy = busy;}
+int32_t mp_runtime_get_line() {return rt.line;}
+void mp_runtime_set_line(int32_t line) {rt.line = line; report_request();}
+uint8_t mp_runtime_get_tool() {return rt.tool;}
+void mp_runtime_set_tool(uint8_t tool) {rt.tool = tool; report_request();}
+
+
+/// Returns current segment velocity
+float mp_runtime_get_velocity() {return rt.velocity;}
+
+
+void mp_runtime_set_velocity(float velocity) {
+ rt.velocity = velocity;
+ report_request();
+}
+
+
+/// Set encoder counts to the runtime position
+void mp_runtime_set_steps_from_position() {
+ // Convert lengths to steps in floating point
+ float steps[MOTORS];
+ mp_kinematics(rt.position, steps);
+
+ for (int motor = 0; motor < MOTORS; motor++)
+ // Write steps to encoder register
+ motor_set_encoder(motor, steps[motor]);
+}
+
+
+/* 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.
+ *
+ * Keeping track of position is complicated by the fact that moves
+ * exist in several reference frames. The scheme to keep this
+ * straight is:
+ *
+ * - mp_position - start and end position for planning
+ * - rt.position - current position of runtime segment
+ * - rt.steps.* - position in steps
+ *
+ * Note that position is set immediately when called and may not be
+ * 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.
+ */
+
+
+/// Set runtime position for a single axis
+void mp_runtime_set_axis_position(uint8_t axis, const float position) {
+ rt.position[axis] = position;
+ report_request();
+}
+
+
+/// Returns current axis position in machine coordinates
+float mp_runtime_get_axis_position(uint8_t axis) {return rt.position[axis];}
+float *mp_runtime_get_position() {return rt.position;}
+
+
+void mp_runtime_set_position(float position[]) {
+ copy_vector(rt.position, position);
+ report_request();
+}
+
+
+/// Returns axis position in work coordinates that were in effect at plan time
+float mp_runtime_get_work_position(uint8_t axis) {
+ return rt.position[axis] - rt.work_offset[axis];
+}
+
+
+/// Set offsets
+void mp_runtime_set_work_offsets(float offset[]) {
+ copy_vector(rt.work_offset, offset);
+}
+
+
+/// Segment runner
+stat_t mp_runtime_move_to_target(float target[], float time) {
+ // Convert target position to steps.
+ float steps[MOTORS];
+ mp_kinematics(target, steps);
+
+#ifdef STEP_CORRECTION
+ int32_t error[MOTORS];
+ st_get_error(error);
+
+ float travel[MOTORS];
+ float new_length_sqr = 0;
+ float old_length_sqr = 0;
+
+ for (int motor = 0; motor < MOTORS; motor++) {
+ travel[motor] = steps[motor] - motor_get_position(motor);
+
+ if (fp_ZERO(travel[motor])) {
+ motor[travel] = 0;
+ motor[error] = 0;
+ }
+
+ error[motor] = 0.5 * (error[motor] - rt.previous_error[motor]);
+ rt.previous_error[motor] = error[motor];
+
+ if (error[motor] < -MAX_STEP_CORRECTION)
+ error[motor] = -MAX_STEP_CORRECTION;
+ else if (MAX_STEP_CORRECTION < error[motor])
+ error[motor] = MAX_STEP_CORRECTION;
+
+ old_length_sqr += square(travel[motor]);
+ new_length_sqr += square(travel[motor] - error[motor]);
+ }
+
+ bool use_error = false;
+ if (!fp_ZERO(new_length_sqr)) {
+ float new_time = time * invsqrt(old_length_sqr / new_length_sqr);
+
+ if (!isnan(new_time) && !isinf(new_time) &&
+ EPSILON <= new_time && new_time <= MAX_SEGMENT_TIME) {
+ time = new_time;
+ use_error = true;
+ }
+ }
+
+ if (!use_error) memset(error, 0, sizeof(error));
+
+#else
+ const int32_t error[MOTORS] = {0};
+#endif
+
+ // Call the stepper prep function
+ RITORNO(st_prep_line(time, steps, error));
+
+ // Update positions
+ mp_runtime_set_position(target);
+
+ return STAT_OK;
+}
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2016 Buildbotics LLC
+ All rights reserved.
+
+ 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/>.
+
+ The software is distributed in the hope that it will be useful, but
+ WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public
+ License along with the software. If not, see
+ <http://www.gnu.org/licenses/>.
+
+ For information regarding this software email:
+ "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#pragma once
+
+#include "status.h"
+
+#include <stdbool.h>
+
+
+bool mp_runtime_is_busy();
+void mp_runtime_set_busy(bool busy);
+
+int32_t mp_runtime_get_line();
+void mp_runtime_set_line(int32_t line);
+
+uint8_t mp_runtime_get_tool();
+void mp_runtime_set_tool(uint8_t tool);
+
+float mp_runtime_get_velocity();
+void mp_runtime_set_velocity(float velocity);
+
+void mp_runtime_set_steps_from_position();
+
+void mp_runtime_set_axis_position(uint8_t axis, const float position);
+float mp_runtime_get_axis_position(uint8_t axis);
+
+float *mp_runtime_get_position();
+void mp_runtime_set_position(float position[]);
+
+float mp_runtime_get_work_position(uint8_t axis);
+void mp_runtime_set_work_offsets(float offset[]);
+
+stat_t mp_runtime_move_to_target(float target[], float time);
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2016 Buildbotics LLC
+ Copyright (c) 2013 - 2015 Alden S. Hart, Jr.
+ Copyright (c) 2013 - 2015 Robert Giseburt
+ All rights reserved.
+
+ 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/>.
+
+ The software is distributed in the hope that it will be useful, but
+ WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public
+ License along with the software. If not, see
+ <http://www.gnu.org/licenses/>.
+
+ For information regarding this software email:
+ "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#include "state.h"
+#include "machine.h"
+#include "planner.h"
+#include "runtime.h"
+#include "buffer.h"
+#include "arc.h"
+#include "stepper.h"
+
+#include "report.h"
+
+#include <stdbool.h>
+
+
+typedef struct {
+ mp_state_t state;
+ mp_cycle_t cycle;
+ mp_hold_reason_t hold_reason;
+
+ bool hold_requested;
+ bool flush_requested;
+ bool start_requested;
+ bool resume_requested;
+ bool optional_pause_requested;
+} planner_state_t;
+
+
+static planner_state_t ps = {
+ .flush_requested = true, // Start out flushing
+};
+
+
+PGM_P mp_get_state_pgmstr(mp_state_t state) {
+ switch (state) {
+ case STATE_READY: return PSTR("READY");
+ case STATE_ESTOPPED: return PSTR("ESTOPPED");
+ case STATE_RUNNING: return PSTR("RUNNING");
+ case STATE_STOPPING: return PSTR("STOPPING");
+ case STATE_HOLDING: return PSTR("HOLDING");
+ }
+
+ return PSTR("INVALID");
+}
+
+
+PGM_P mp_get_cycle_pgmstr(mp_cycle_t cycle) {
+ switch (cycle) {
+ case CYCLE_MACHINING: return PSTR("MACHINING");
+ case CYCLE_HOMING: return PSTR("HOMING");
+ case CYCLE_PROBING: return PSTR("PROBING");
+ case CYCLE_CALIBRATING: return PSTR("CALIBRATING");
+ case CYCLE_JOGGING: return PSTR("JOGGING");
+ }
+
+ return PSTR("INVALID");
+}
+
+
+PGM_P mp_get_hold_reason_pgmstr(mp_hold_reason_t reason) {
+ switch (reason) {
+ case HOLD_REASON_USER_PAUSE: return PSTR("USER");
+ case HOLD_REASON_PROGRAM_PAUSE: return PSTR("PROGRAM");
+ case HOLD_REASON_PROGRAM_END: return PSTR("END");
+ case HOLD_REASON_PALLET_CHANGE: return PSTR("PALLET");
+ case HOLD_REASON_TOOL_CHANGE: return PSTR("TOOL");
+ }
+
+ return PSTR("INVALID");
+}
+
+
+mp_state_t mp_get_state() {return ps.state;}
+mp_cycle_t mp_get_cycle() {return ps.cycle;}
+
+
+static void _set_state(mp_state_t state) {
+ if (ps.state == state) return; // No change
+ if (ps.state == STATE_ESTOPPED) return; // Can't leave EStop state
+ if (state == STATE_READY) mp_runtime_set_line(0);
+ ps.state = state;
+ report_request();
+}
+
+
+void mp_set_cycle(mp_cycle_t cycle) {
+ if (ps.cycle == cycle) return; // No change
+
+ if (ps.state != STATE_READY && cycle != CYCLE_MACHINING) {
+ STATUS_ERROR(STAT_INTERNAL_ERROR, "Cannot transition to %S while %S",
+ mp_get_cycle_pgmstr(cycle),
+ mp_get_state_pgmstr(ps.state));
+ return;
+ }
+
+ if (ps.cycle != CYCLE_MACHINING && cycle != CYCLE_MACHINING) {
+ STATUS_ERROR(STAT_INTERNAL_ERROR,
+ "Cannot transition to cycle %S while in %S",
+ mp_get_cycle_pgmstr(cycle),
+ mp_get_cycle_pgmstr(ps.cycle));
+ return;
+ }
+
+ ps.cycle = cycle;
+ report_request();
+}
+
+
+mp_hold_reason_t mp_get_hold_reason() {return ps.hold_reason;}
+
+
+void mp_set_hold_reason(mp_hold_reason_t reason) {
+ if (ps.hold_reason == reason) return; // No change
+ ps.hold_reason = reason;
+ report_request();
+}
+
+
+bool mp_is_flushing() {return ps.flush_requested && !ps.resume_requested;}
+bool mp_is_resuming() {return ps.resume_requested;}
+
+
+bool mp_is_quiescent() {
+ return (mp_get_state() == STATE_READY || mp_get_state() == STATE_HOLDING) &&
+ !st_is_busy() && !mp_runtime_is_busy();
+}
+
+
+void mp_state_optional_pause() {
+ if (ps.optional_pause_requested) {
+ mp_set_hold_reason(HOLD_REASON_USER_PAUSE);
+ mp_state_holding();
+ }
+}
+
+
+void mp_state_holding() {
+ _set_state(STATE_HOLDING);
+ mp_set_plan_steps(false);
+}
+
+
+void mp_state_running() {
+ if (mp_get_state() == STATE_READY) _set_state(STATE_RUNNING);
+}
+
+
+void mp_state_idle() {
+ if (mp_get_state() == STATE_RUNNING) _set_state(STATE_READY);
+}
+
+
+void mp_state_estop() {_set_state(STATE_ESTOPPED);}
+
+
+void mp_request_hold() {ps.hold_requested = true;}
+void mp_request_start() {ps.start_requested = true;}
+void mp_request_flush() {ps.flush_requested = true;}
+void mp_request_resume() {if (ps.flush_requested) ps.resume_requested = true;}
+void mp_request_optional_pause() {ps.optional_pause_requested = true;}
+
+
+void mp_request_step() {
+ mp_set_plan_steps(true);
+ ps.start_requested = true;
+}
+
+
+/*** Feedholds, queue flushes and starts are all related. Request functions
+ * set flags. The callback interprets the flags according to these rules:
+ *
+ * A hold request received:
+ * - during motion is honored
+ * - during a feedhold is ignored and reset
+ * - when already stopped is ignored and reset
+ *
+ * A flush request received:
+ * - during motion is ignored but not reset
+ * - during a feedhold is deferred until the feedhold enters HOLDING state.
+ * I.e. until deceleration is complete.
+ * - when stopped or holding and the planner is not busy, is honored
+ *
+ * A start request received:
+ * - during motion is ignored and reset
+ * - during a feedhold is deferred until the feedhold enters HOLDING state.
+ * I.e. until deceleration is complete. If a queue flush request is also
+ * present the queue flush is done first
+ * - when stopped is honored and starts to run anything in the planner queue
+ */
+void mp_state_callback() {
+ if (ps.hold_requested || ps.flush_requested) {
+ ps.hold_requested = false;
+ mp_set_hold_reason(HOLD_REASON_USER_PAUSE);
+
+ if (mp_get_state() == STATE_RUNNING) _set_state(STATE_STOPPING);
+ }
+
+ // Only flush queue when idle or holding.
+ if (ps.flush_requested && mp_is_quiescent()) {
+ mach_abort_arc();
+
+ if (!mp_queue_is_empty()) {
+ mp_flush_planner();
+
+ // NOTE The following uses low-level mp calls for absolute position.
+ // Reset to actual machine position. Otherwise machine is set to the
+ // position of the last queued move.
+ for (int axis = 0; axis < AXES; axis++)
+ mach_set_axis_position(axis, mp_runtime_get_axis_position(axis));
+ }
+
+ // Resume
+ if (ps.resume_requested) {
+ ps.flush_requested = ps.resume_requested = false;
+ _set_state(STATE_READY);
+ }
+ }
+
+ // Don't start while flushing or stopping
+ if (ps.start_requested && !ps.flush_requested &&
+ mp_get_state() != STATE_STOPPING) {
+ ps.start_requested = false;
+ ps.optional_pause_requested = false;
+
+ if (mp_get_state() == STATE_HOLDING) {
+ // Check if any moves are buffered
+ if (!mp_queue_is_empty()) {
+ // Always replan when coming out of a hold
+ mp_replan_all();
+ _set_state(STATE_RUNNING);
+
+ } else _set_state(STATE_READY);
+ }
+ }
+}
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2016 Buildbotics LLC
+ Copyright (c) 2013 - 2015 Alden S. Hart, Jr.
+ Copyright (c) 2013 - 2015 Robert Giseburt
+ All rights reserved.
+
+ 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/>.
+
+ The software is distributed in the hope that it will be useful, but
+ WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public
+ License along with the software. If not, see
+ <http://www.gnu.org/licenses/>.
+
+ For information regarding this software email:
+ "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#pragma once
+
+#include <avr/pgmspace.h>
+
+#include <stdbool.h>
+
+
+typedef enum {
+ STATE_READY,
+ STATE_ESTOPPED,
+ STATE_RUNNING,
+ STATE_STOPPING,
+ STATE_HOLDING,
+} mp_state_t;
+
+
+typedef enum {
+ CYCLE_MACHINING,
+ CYCLE_HOMING,
+ CYCLE_PROBING,
+ CYCLE_CALIBRATING,
+ CYCLE_JOGGING,
+} mp_cycle_t;
+
+
+typedef enum {
+ HOLD_REASON_USER_PAUSE,
+ HOLD_REASON_PROGRAM_PAUSE,
+ HOLD_REASON_PROGRAM_END,
+ HOLD_REASON_PALLET_CHANGE,
+ HOLD_REASON_TOOL_CHANGE,
+} mp_hold_reason_t;
+
+
+PGM_P mp_get_state_pgmstr(mp_state_t state);
+PGM_P mp_get_cycle_pgmstr(mp_cycle_t cycle);
+PGM_P mp_get_hold_reason_pgmstr(mp_hold_reason_t reason);
+
+mp_state_t mp_get_state();
+
+mp_cycle_t mp_get_cycle();
+void mp_set_cycle(mp_cycle_t cycle);
+
+mp_hold_reason_t mp_get_hold_reason();
+void mp_set_hold_reason(mp_hold_reason_t reason);
+
+bool mp_is_flushing();
+bool mp_is_resuming();
+bool mp_is_quiescent();
+
+void mp_state_optional_pause();
+void mp_state_holding();
+void mp_state_running();
+void mp_state_idle();
+void mp_state_estop();
+
+void mp_request_hold();
+void mp_request_start();
+void mp_request_flush();
+void mp_request_resume();
+void mp_request_optional_pause();
+void mp_request_step();
+
+void mp_state_callback();
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2016 Buildbotics LLC
+ Copyright (c) 2010 - 2015 Alden S Hart, Jr.
+ All rights reserved.
+
+ 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/>.
+
+ The software is distributed in the hope that it will be useful, but
+ WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public
+ License along with the software. If not, see
+ <http://www.gnu.org/licenses/>.
+
+ For information regarding this software email:
+ "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#include "machine.h"
+#include "axis.h"
+#include "spindle.h"
+#include "switch.h"
+#include "util.h"
+
+#include "plan/planner.h"
+#include "plan/runtime.h"
+#include "plan/state.h"
+
+#include <avr/pgmspace.h>
+
+#include <math.h>
+#include <string.h>
+#include <stdbool.h>
+#include <stdio.h>
+
+
+#define MINIMUM_PROBE_TRAVEL 0.254
+
+
+typedef enum {
+ PROBE_FAILED, // probe reached endpoint without triggering
+ PROBE_SUCCEEDED, // probe was triggered, pb.results has position
+ PROBE_WAITING, // probe is waiting to be started
+} probe_state_t;
+
+
+typedef struct {
+ probe_state_t state;
+ float results[AXES]; // probing results
+
+ void (*func)(); // binding for callback function state machine
+
+ // state saved from gcode model
+ uint8_t saved_distance_mode; // G90, G91 global setting
+ uint8_t saved_coord_system; // G54 - G59 setting
+
+ // probe destination
+ float start_position[AXES];
+ float target[AXES];
+ bool flags[AXES];
+} probing_t;
+
+
+static probing_t pb = {0};
+
+
+/* Note: When coding a cycle (like this one) you get to perform one
+ * queued move per entry into the continuation, then you must exit.
+ *
+ * Another Note: When coding a cycle (like this one) you must wait
+ * until the last move has actually been queued (or has finished)
+ * before declaring the cycle to be done. Otherwise there is a nasty
+ * race condition that will accept the next command before the position
+ * of the final move has been recorded in the Gcode model.
+ */
+
+
+static void _probe_restore_settings() {
+ // we should be stopped now, but in case of switch closure
+ mp_flush_planner();
+
+ // restore coordinate system and distance mode
+ mach_set_coord_system(pb.saved_coord_system);
+ mach_set_distance_mode(pb.saved_distance_mode);
+
+ // update the model with actual position
+ mach_set_motion_mode(MOTION_MODE_CANCEL_MOTION_MODE);
+
+ mp_set_cycle(CYCLE_MACHINING); // Default cycle
+}
+
+
+static void _probing_error_exit(stat_t status) {
+ _probe_restore_settings(); // clean up and exit
+ status_error(status);
+}
+
+
+static void _probing_finish() {
+ bool closed = switch_is_active(SW_PROBE);
+ pb.state = closed ? PROBE_SUCCEEDED : PROBE_FAILED;
+
+ for (int axis = 0; axis < AXES; axis++) {
+ // if we got here because of a feed hold keep the model position correct
+ mach_set_axis_position(axis, mp_runtime_get_work_position(axis));
+
+ // store the probe results
+ pb.results[axis] = mach_get_axis_position(axis);
+ }
+
+ _probe_restore_settings();
+}
+
+
+static void _probing_start() {
+ // initial probe state, don't probe if we're already contacted!
+ bool closed = switch_is_active(SW_PROBE);
+
+ if (!closed) {
+ stat_t status = mach_feed(pb.target, pb.flags);
+ if (status) return _probing_error_exit(status);
+ }
+
+ pb.func = _probing_finish;
+}
+
+
+/* G38.2 homing cycle using limit switches
+ *
+ * These initializations are required before starting the probing cycle.
+ * They must be done after the planner has exhasted all current CYCLE moves as
+ * they affect the runtime (specifically the switch modes). Side effects would
+ * include limit switches initiating probe actions instead of just killing
+ * movement
+ */
+static void _probing_init() {
+ // NOTE: it is *not* an error condition for the probe not to trigger.
+ // it is an error for the limit or homing switches to fire, or for some other
+ // configuration error.
+ pb.state = PROBE_FAILED;
+ mp_set_cycle(CYCLE_PROBING);
+
+ // initialize the axes
+ for (int axis = 0; axis < AXES; axis++)
+ pb.start_position[axis] = mach_get_axis_position(axis);
+
+ // error if the probe target is too close to the current position
+ if (axis_get_vector_length(pb.start_position, pb.target) <
+ MINIMUM_PROBE_TRAVEL)
+ _probing_error_exit(STAT_PROBE_INVALID_DEST);
+
+ // error if the probe target requires a move along the A/B/C axes
+ for (int axis = 0; axis < AXES; axis++)
+ if (fp_NE(pb.start_position[axis], pb.target[axis]))
+ _probing_error_exit(STAT_MOVE_DURING_PROBE);
+
+ // probe in absolute machine coords
+ pb.saved_coord_system = mach_get_coord_system();
+ pb.saved_distance_mode = mach_get_distance_mode();
+ mach_set_distance_mode(ABSOLUTE_MODE);
+ mach_set_coord_system(ABSOLUTE_COORDS);
+
+ mach_set_spindle_mode(SPINDLE_OFF);
+
+ // start the move
+ pb.func = _probing_start;
+}
+
+
+bool mach_is_probing() {
+ return mp_get_cycle() == CYCLE_PROBING || pb.state == PROBE_WAITING;
+}
+
+
+/// G38.2 homing cycle using limit switches
+stat_t mach_probe(float target[], bool flags[]) {
+ // trap zero feed rate condition
+ if (mach_get_feed_mode() != INVERSE_TIME_MODE &&
+ fp_ZERO(mach_get_feed_rate()))
+ return STAT_GCODE_FEEDRATE_NOT_SPECIFIED;
+
+ // trap no axes specified
+ if (!flags[AXIS_X] && !flags[AXIS_Y] && !flags[AXIS_Z])
+ return STAT_GCODE_AXIS_IS_MISSING;
+
+ // set probe move endpoint
+ copy_vector(pb.target, target); // set probe move endpoint
+ copy_vector(pb.flags, flags); // set axes involved on the move
+ clear_vector(pb.results); // clear the old probe position.
+ // NOTE: relying on pb.results will not detect a probe to (0, 0, 0).
+
+ // wait until planner queue empties before completing initialization
+ pb.state = PROBE_WAITING;
+ pb.func = _probing_init; // bind probing initialization function
+
+ return STAT_OK;
+}
+
+
+/// main loop callback for running the homing cycle
+void mach_probe_callback() {
+ // sync to planner move ends
+ if (!mach_is_probing() || mp_get_state() != STATE_READY) return;
+
+ pb.func(); // execute the current homing move
+}
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2016 Buildbotics LLC
+ All rights reserved.
+
+ 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/>.
+
+ The software is distributed in the hope that it will be useful, but
+ WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public
+ License along with the software. If not, see
+ <http://www.gnu.org/licenses/>.
+
+ For information regarding this software email:
+ "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#pragma once
+
+#include "status.h"
+
+#include <stdbool.h>
+
+
+bool mach_is_probing();
+stat_t mach_probe(float target[], bool flags[]);
+void mach_probe_callback();
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2016 Buildbotics LLC
+ All rights reserved.
+
+ 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/>.
+
+ The software is distributed in the hope that it will be useful, but
+ WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public
+ License along with the software. If not, see
+ <http://www.gnu.org/licenses/>.
+
+ For information regarding this software email:
+ "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+
+#include "pwm_spindle.h"
+
+#include "config.h"
+
+
+typedef struct {
+ uint16_t freq; // base frequency for PWM driver, in Hz
+ float min_rpm;
+ float max_rpm;
+ float min_duty;
+ float max_duty;
+ bool reverse;
+ bool enable_invert;
+ bool estop;
+} pwm_spindle_t;
+
+
+static pwm_spindle_t spindle = {
+ .freq = SPINDLE_PWM_FREQUENCY,
+ .min_rpm = SPINDLE_MIN_RPM,
+ .max_rpm = SPINDLE_MAX_RPM,
+ .min_duty = SPINDLE_MIN_DUTY,
+ .max_duty = SPINDLE_MAX_DUTY,
+ .reverse = SPINDLE_REVERSE,
+ .enable_invert = false,
+ .estop = false,
+};
+
+
+static void _spindle_set_pwm(spindle_mode_t mode, float speed) {
+ if (mode == SPINDLE_OFF || speed < spindle.min_rpm || spindle.estop) {
+ TIMER_PWM.CTRLA = 0;
+ return;
+ }
+
+ // Clamp speed
+ if (spindle.max_rpm < speed) speed = spindle.max_rpm;
+
+ // Set clock period and optimal prescaler value
+ float prescale = F_CPU / 65536.0 / spindle.freq;
+ if (prescale <= 1) {
+ TIMER_PWM.PER = F_CPU / spindle.freq;
+ TIMER_PWM.CTRLA = TC_CLKSEL_DIV1_gc;
+
+ } else if (prescale <= 2) {
+ TIMER_PWM.PER = F_CPU / 2 / spindle.freq;
+ TIMER_PWM.CTRLA = TC_CLKSEL_DIV2_gc;
+
+ } else if (prescale <= 4) {
+ TIMER_PWM.PER = F_CPU / 4 / spindle.freq;
+ TIMER_PWM.CTRLA = TC_CLKSEL_DIV4_gc;
+
+ } else if (prescale <= 8) {
+ TIMER_PWM.PER = F_CPU / 8 / spindle.freq;
+ TIMER_PWM.CTRLA = TC_CLKSEL_DIV8_gc;
+
+ } else if (prescale <= 64) {
+ TIMER_PWM.PER = F_CPU / 64 / spindle.freq;
+ TIMER_PWM.CTRLA = TC_CLKSEL_DIV64_gc;
+
+ } else TIMER_PWM.CTRLA = 0;
+
+ // Map RPM to duty cycle
+ float duty = (speed - spindle.min_rpm) / (spindle.max_rpm - spindle.min_rpm) *
+ (spindle.max_duty - spindle.min_duty) + spindle.min_duty;
+
+ TIMER_PWM.CCB = TIMER_PWM.PER * duty;
+}
+
+
+static void _spindle_set_enable(bool enable) {
+ if (enable ^ spindle.enable_invert)
+ PORT(SPIN_ENABLE_PIN)->OUTSET = BM(SPIN_ENABLE_PIN);
+ else PORT(SPIN_ENABLE_PIN)->OUTCLR = BM(SPIN_ENABLE_PIN);
+}
+
+
+static void _spindle_set_dir(bool forward) {
+ if (forward ^ spindle.reverse) PORT(SPIN_DIR_PIN)->OUTCLR = BM(SPIN_DIR_PIN);
+ else PORT(SPIN_DIR_PIN)->OUTSET = BM(SPIN_DIR_PIN);
+}
+
+
+void pwm_spindle_init() {
+ // Configure IO
+ PORT(SPIN_PWM_PIN)->DIRSET = BM(SPIN_PWM_PIN); // PWM Output
+ _spindle_set_dir(true);
+ PORT(SPIN_DIR_PIN)->DIRSET = BM(SPIN_DIR_PIN); // Dir Output
+ _spindle_set_enable(false);
+ PORT(SPIN_ENABLE_PIN)->DIRSET = BM(SPIN_ENABLE_PIN); // Enable output
+
+ // Configure clock
+ TIMER_PWM.CTRLB = TC1_CCBEN_bm | TC_WGMODE_SINGLESLOPE_gc;
+}
+
+
+void pwm_spindle_set(spindle_mode_t mode, float speed) {
+ _spindle_set_dir(mode == SPINDLE_CW);
+ _spindle_set_pwm(mode, speed);
+ _spindle_set_enable(mode != SPINDLE_OFF && TIMER_PWM.CTRLA);
+}
+
+
+void pwm_spindle_estop() {
+ spindle.estop = true;
+ _spindle_set_pwm(SPINDLE_OFF, 0);
+}
+
+
+// TODO these need more effort and should work with the huanyang spindle too
+float get_max_spin(int index) {return spindle.max_rpm;}
+void set_max_spin(int axis, float value) {spindle.max_rpm = value;}
+float get_min_spin(int index) {return spindle.min_rpm;}
+void set_min_spin(int axis, float value) {spindle.min_rpm = value;}
+float get_spin_min_pulse(int index) {return spindle.min_duty;}
+void set_spin_min_pulse(int axis, float value) {spindle.min_duty = value;}
+float get_spin_max_pulse(int index) {return spindle.max_duty;}
+void set_spin_max_pulse(int axis, float value) {spindle.max_duty = value;}
+uint8_t get_spin_reverse(int index) {return spindle.reverse;}
+void set_spin_reverse(int axis, uint8_t value) {spindle.reverse = value;}
+float get_spin_up(int index) {return 0;}
+void set_spin_up(int axis, float value) {}
+float get_spin_down(int index) {return 0;}
+void set_spin_down(int axis, float value) {}
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2016 Buildbotics LLC
+ All rights reserved.
+
+ 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/>.
+
+ The software is distributed in the hope that it will be useful, but
+ WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public
+ License along with the software. If not, see
+ <http://www.gnu.org/licenses/>.
+
+ For information regarding this software email:
+ "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#pragma once
+
+#include "machine.h"
+
+
+void pwm_spindle_init();
+void pwm_spindle_set(spindle_mode_t mode, float speed);
+void pwm_spindle_estop();
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2016 Buildbotics LLC
+ All rights reserved.
+
+ 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/>.
+
+ The software is distributed in the hope that it will be useful, but
+ WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public
+ License along with the software. If not, see
+ <http://www.gnu.org/licenses/>.
+
+ For information regarding this software email:
+ "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#include "report.h"
+#include "config.h"
+#include "usart.h"
+#include "rtc.h"
+#include "vars.h"
+
+#include <avr/pgmspace.h>
+
+#include <stdio.h>
+#include <stdbool.h>
+
+
+static bool report_requested = false;
+static bool report_full = false;
+static uint32_t last_report = 0;
+
+
+void report_request() {
+ report_requested = true;
+}
+
+
+void report_request_full() {
+ report_requested = report_full = true;
+}
+
+
+void report_callback() {
+ if (usart_tx_full()) return; // Wait for buffer space
+
+ if (report_requested && usart_tx_empty()) {
+ uint32_t now = rtc_get_time();
+ if (now - last_report < 100) return;
+ last_report = now;
+
+ vars_report(report_full);
+ report_requested = report_full = false;
+ }
+}
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2016 Buildbotics LLC
+ All rights reserved.
+
+ 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/>.
+
+ The software is distributed in the hope that it will be useful, but
+ WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public
+ License along with the software. If not, see
+ <http://www.gnu.org/licenses/>.
+
+ For information regarding this software email:
+ "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#pragma once
+
+
+#include "status.h"
+
+void report_request();
+void report_request_full();
+void report_callback();
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2016 Buildbotics LLC
+ All rights reserved.
+
+ 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/>.
+
+ The software is distributed in the hope that it will be useful, but
+ WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public
+ License along with the software. If not, see
+ <http://www.gnu.org/licenses/>.
+
+ For information regarding this software email:
+ "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+/* This file defines an X-Macro ring buffer. It can be used like this:
+ *
+ * #define RING_BUF_NAME tx_buf
+ * #define RING_BUF_SIZE 256
+ * #include "ringbuf.def"
+ *
+ * This will define the following functions:
+ *
+ * void <name>_init();
+ * int <name>_empty();
+ * int <name>_full();
+ * <type> <name>_peek();
+ * void <name>_pop();
+ * void <name>_push(<type> data);
+ *
+ * Where <name> is defined by RING_BUF_NAME and <type> by RING_BUF_TYPE.
+ * RING_BUF_SIZE defines the length of the ring buffer and must be a power of 2.
+ *
+ * The data type and index type both default to uint8_t but can be changed by
+ * defining RING_BUF_TYPE and RING_BUF_INDEX_TYPE respectively.
+ *
+ * By default these functions are declared static inline but this can be changed
+ * by defining RING_BUF_FUNC.
+ */
+
+#include <stdint.h>
+
+#ifndef RING_BUF_NAME
+#error Must define RING_BUF_NAME
+#endif
+
+#ifndef RING_BUF_SIZE
+#error Must define RING_BUF_SIZE
+#endif
+
+#ifndef RING_BUF_TYPE
+#define RING_BUF_TYPE uint8_t
+#endif
+
+#ifndef RING_BUF_INDEX_TYPE
+#define RING_BUF_INDEX_TYPE uint8_t
+#endif
+
+#ifndef RING_BUF_FUNC
+#define RING_BUF_FUNC static inline
+#endif
+
+#define RING_BUF_MASK (RING_BUF_SIZE - 1)
+#if (RING_BUF_SIZE & RING_BUF_MASK)
+#error RING_BUF_SIZE is not a power of 2
+#endif
+
+#ifndef CONCAT
+#define _CONCAT(prefix, name) prefix##name
+#define CONCAT(prefix, name) _CONCAT(prefix, name)
+#endif
+
+#define RING_BUF_STRUCT CONCAT(RING_BUF_NAME, _ring_buf_t)
+#define RING_BUF CONCAT(RING_BUF_NAME, _ring_buf)
+
+typedef struct {
+ RING_BUF_TYPE buf[RING_BUF_SIZE];
+ volatile RING_BUF_INDEX_TYPE head;
+ volatile RING_BUF_INDEX_TYPE tail;
+} RING_BUF_STRUCT;
+
+static RING_BUF_STRUCT RING_BUF;
+
+
+RING_BUF_FUNC void CONCAT(RING_BUF_NAME, _init)() {
+ RING_BUF.head = RING_BUF.tail = 0;
+}
+
+
+#define RING_BUF_INC CONCAT(RING_BUF_NAME, _inc)
+RING_BUF_FUNC RING_BUF_INDEX_TYPE RING_BUF_INC(RING_BUF_INDEX_TYPE x) {
+ return (x + 1) & RING_BUF_MASK;
+}
+
+
+RING_BUF_FUNC int CONCAT(RING_BUF_NAME, _empty)() {
+ return RING_BUF.head == RING_BUF.tail;
+}
+
+
+RING_BUF_FUNC int CONCAT(RING_BUF_NAME, _full)() {
+ return RING_BUF.head == RING_BUF_INC(RING_BUF.tail);
+}
+
+
+RING_BUF_FUNC int CONCAT(RING_BUF_NAME, _fill)() {
+ return (RING_BUF.tail - RING_BUF.head) & RING_BUF_MASK;
+}
+
+
+RING_BUF_FUNC int CONCAT(RING_BUF_NAME, _space)() {
+ return RING_BUF_SIZE - CONCAT(RING_BUF_NAME, _fill)();
+}
+
+
+RING_BUF_FUNC RING_BUF_TYPE CONCAT(RING_BUF_NAME, _peek)() {
+ return RING_BUF.buf[RING_BUF.head];
+}
+
+
+RING_BUF_FUNC RING_BUF_TYPE CONCAT(RING_BUF_NAME, _get)(int offset) {
+ return RING_BUF.buf[(RING_BUF.head + offset) & RING_BUF_MASK];
+}
+
+
+RING_BUF_FUNC void CONCAT(RING_BUF_NAME, _pop)() {
+ RING_BUF.head = RING_BUF_INC(RING_BUF.head);
+}
+
+
+RING_BUF_FUNC void CONCAT(RING_BUF_NAME, _push)(RING_BUF_TYPE data) {
+ RING_BUF.buf[RING_BUF.tail] = data;
+ RING_BUF.tail = RING_BUF_INC(RING_BUF.tail);
+}
+
+
+#undef RING_BUF
+#undef RING_BUF_STRUCT
+#undef RING_BUF_INC
+#undef RING_BUF_MASK
+
+#undef RING_BUF_NAME
+#undef RING_BUF_SIZE
+#undef RING_BUF_TYPE
+#undef RING_BUF_INDEX_TYPE
+#undef RING_BUF_FUNC
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2016 Buildbotics LLC
+ Copyright (c) 2010 - 2013 Alden S. Hart Jr.
+ All rights reserved.
+
+ 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/>.
+
+ The software is distributed in the hope that it will be useful, but
+ WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public
+ License along with the software. If not, see
+ <http://www.gnu.org/licenses/>.
+
+ For information regarding this software email:
+ "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#include "rtc.h"
+
+#include "switch.h"
+#include "huanyang.h"
+#include "motor.h"
+
+#include <avr/io.h>
+#include <avr/interrupt.h>
+
+#include <string.h>
+
+
+static uint32_t ticks;
+
+
+ISR(RTC_OVF_vect) {
+ ticks++;
+
+ switch_rtc_callback();
+ huanyang_rtc_callback();
+ if (!(ticks & 255)) motor_rtc_callback();
+}
+
+
+/// Initialize and start the clock
+/// This routine follows the code in app note AVR1314.
+void rtc_init() {
+ ticks = 0;
+
+ OSC.CTRL |= OSC_RC32KEN_bm; // enable internal 32kHz.
+ while (!(OSC.STATUS & OSC_RC32KRDY_bm)); // 32kHz osc stabilize
+ while (RTC.STATUS & RTC_SYNCBUSY_bm); // wait RTC not busy
+
+ CLK.RTCCTRL = CLK_RTCSRC_RCOSC32_gc | CLK_RTCEN_bm; // 32kHz clock as RTC src
+ while (RTC.STATUS & RTC_SYNCBUSY_bm); // wait RTC not busy
+
+ // the following must be in this order or it doesn't work
+ RTC.PER = 33; // overflow period ~1ms
+ RTC.INTCTRL = RTC_OVFINTLVL_LO_gc; // overflow LO interrupt
+ RTC.CTRL = RTC_PRESCALER_DIV1_gc; // no prescale
+}
+
+
+uint32_t rtc_get_time() {return ticks;}
+
+
+bool rtc_expired(uint32_t t) {
+ return 0 <= (int32_t)(ticks - t);
+}
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2016 Buildbotics LLC
+ Copyright (c) 2010 - 2013 Alden S. Hart Jr.
+ All rights reserved.
+
+ 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/>.
+
+ The software is distributed in the hope that it will be useful, but
+ WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public
+ License along with the software. If not, see
+ <http://www.gnu.org/licenses/>.
+
+ For information regarding this software email:
+ "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#pragma once
+
+
+#include <stdint.h>
+#include <stdbool.h>
+
+void rtc_init();
+uint32_t rtc_get_time();
+int32_t rtc_diff(uint32_t t);
+bool rtc_expired(uint32_t t);
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2016 Buildbotics LLC
+ Copyright (c) 2010 - 2015 Alden S. Hart, Jr.
+ All rights reserved.
+
+ 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/>.
+
+ The software is distributed in the hope that it will be useful, but
+ WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public
+ License along with the software. If not, see
+ <http://www.gnu.org/licenses/>.
+
+ For information regarding this software email:
+ "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#include "spindle.h"
+
+#include "config.h"
+#include "pwm_spindle.h"
+#include "huanyang.h"
+
+
+typedef enum {
+ SPINDLE_TYPE_PWM,
+ SPINDLE_TYPE_HUANYANG,
+} spindle_type_t;
+
+
+typedef struct {
+ spindle_type_t type;
+ spindle_mode_t mode;
+ float speed;
+} spindle_t;
+
+
+static spindle_t spindle = {.type = SPINDLE_TYPE};
+
+
+void spindle_init() {
+ pwm_spindle_init();
+ huanyang_init();
+}
+
+
+void _spindle_set(spindle_mode_t mode, float speed) {
+ spindle.mode = mode;
+ spindle.speed = speed;
+
+ switch (spindle.type) {
+ case SPINDLE_TYPE_PWM: pwm_spindle_set(mode, speed); break;
+ case SPINDLE_TYPE_HUANYANG: huanyang_set(mode, speed); break;
+ }
+}
+
+
+void spindle_set_mode(spindle_mode_t mode) {
+ spindle.mode = mode;
+
+ switch (spindle.type) {
+ case SPINDLE_TYPE_PWM: pwm_spindle_set(mode, spindle.speed); break;
+ case SPINDLE_TYPE_HUANYANG: huanyang_set(mode, spindle.speed); break;
+ }
+}
+
+
+void spindle_set_speed(float speed) {
+ spindle.speed = speed;
+
+ switch (spindle.type) {
+ case SPINDLE_TYPE_PWM: pwm_spindle_set(spindle.mode, speed); break;
+ case SPINDLE_TYPE_HUANYANG: huanyang_set(spindle.mode, speed); break;
+ }
+}
+
+
+spindle_mode_t spindle_get_mode() {return spindle.mode;}
+float spindle_get_speed() {return spindle.speed;}
+
+
+void spindle_estop() {
+ switch (spindle.type) {
+ case SPINDLE_TYPE_PWM: pwm_spindle_estop(); break;
+ case SPINDLE_TYPE_HUANYANG: huanyang_estop(); break;
+ }
+}
+
+
+uint8_t get_spindle_type() {return spindle.type;}
+
+
+void set_spindle_type(uint8_t value) {
+ if (value != spindle.type) {
+ spindle_mode_t mode = spindle.mode;
+ float speed = spindle.speed;
+
+ _spindle_set(SPINDLE_OFF, 0);
+ spindle.type = value;
+ _spindle_set(mode, speed);
+ }
+}
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2016 Buildbotics LLC
+ Copyright (c) 2010 - 2015 Alden S. Hart, Jr.
+ All rights reserved.
+
+ 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/>.
+
+ The software is distributed in the hope that it will be useful, but
+ WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public
+ License along with the software. If not, see
+ <http://www.gnu.org/licenses/>.
+
+ For information regarding this software email:
+ "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#pragma once
+
+#include "machine.h"
+
+
+void spindle_init();
+void spindle_set_mode(spindle_mode_t mode);
+void spindle_set_speed(float speed);
+spindle_mode_t spindle_get_mode();
+float spindle_get_speed();
+void spindle_estop();
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2016 Buildbotics LLC
+ All rights reserved.
+
+ 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/>.
+
+ The software is distributed in the hope that it will be useful, but
+ WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public
+ License along with the software. If not, see
+ <http://www.gnu.org/licenses/>.
+
+ For information regarding this software email:
+ "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#include "status.h"
+#include "estop.h"
+#include "usart.h"
+
+#include <stdio.h>
+#include <stdarg.h>
+
+
+stat_t status_code; // allocate a variable for the RITORNO macro
+
+#define STAT_MSG(NAME, TEXT) static const char stat_##NAME[] PROGMEM = TEXT;
+#include "messages.def"
+#undef STAT_MSG
+
+static const char *const stat_msg[] PROGMEM = {
+#define STAT_MSG(NAME, TEXT) stat_##NAME,
+#include "messages.def"
+#undef STAT_MSG
+};
+
+
+const char *status_to_pgmstr(stat_t code) {
+ return pgm_read_ptr(&stat_msg[code]);
+}
+
+
+const char *status_level_pgmstr(status_level_t level) {
+ switch (level) {
+ case STAT_LEVEL_INFO: return PSTR("info");
+ case STAT_LEVEL_DEBUG: return PSTR("debug");
+ case STAT_LEVEL_WARNING: return PSTR("warning");
+ default: return PSTR("error");
+ }
+}
+
+
+stat_t status_error(stat_t code) {
+ return status_message_P(0, STAT_LEVEL_ERROR, code, 0);
+}
+
+
+stat_t status_message_P(const char *location, status_level_t level,
+ stat_t code, const char *msg, ...) {
+ va_list args;
+
+ // Type
+ printf_P(PSTR("\n{\"level\":\"%S\",\"msg\":\""), status_level_pgmstr(level));
+
+ // Message
+ if (msg) {
+ va_start(args, msg);
+ vfprintf_P(stdout, msg, args);
+ va_end(args);
+
+ } else printf_P(status_to_pgmstr(code));
+
+ putchar('"');
+
+ // Code
+ if (code) printf_P(PSTR(", \"code\": %d"), code);
+
+ // Location
+ if (location) printf_P(PSTR(", \"where\": \"%S\""), location);
+
+ putchar('}');
+ putchar('\n');
+
+ return code;
+}
+
+
+void status_help() {
+ putchar('{');
+
+ for (int i = 0; i < STAT_MAX; i++) {
+ if (i) putchar(',');
+ putchar('\n');
+ printf_P(PSTR(" \"%d\": \"%S\""), i, status_to_pgmstr(i));
+ }
+
+ putchar('\n');
+ putchar('}');
+ putchar('\n');
+}
+
+
+/// Alarm state; send an exception report and stop processing input
+stat_t status_alarm(const char *location, stat_t code) {
+ status_message_P(location, STAT_LEVEL_ERROR, code, 0);
+ estop_trigger(code);
+ while (!usart_tx_empty()) continue;
+ return code;
+}
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2016 Buildbotics LLC
+ All rights reserved.
+
+ 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/>.
+
+ The software is distributed in the hope that it will be useful, but
+ WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public
+ License along with the software. If not, see
+ <http://www.gnu.org/licenses/>.
+
+ For information regarding this software email:
+ "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#pragma once
+
+#include <avr/pgmspace.h>
+
+
+// RITORNO is a handy way to provide exception returns
+// It returns only if an error occurred. (ritorno is Italian for return)
+#define RITORNO(a) if ((status_code = a) != STAT_OK) {return status_code;}
+
+typedef enum {
+#define STAT_MSG(NAME, TEXT) STAT_##NAME,
+#include "messages.def"
+#undef STAT_MSG
+
+ STAT_DO_NOT_EXCEED = 255 // Do not exceed 255
+} stat_t;
+
+
+typedef enum {
+ STAT_LEVEL_INFO,
+ STAT_LEVEL_DEBUG,
+ STAT_LEVEL_WARNING,
+ STAT_LEVEL_ERROR,
+} status_level_t;
+
+
+extern stat_t status_code;
+
+const char *status_to_pgmstr(stat_t code);
+const char *status_level_pgmstr(status_level_t level);
+stat_t status_error(stat_t code);
+stat_t status_message_P(const char *location, status_level_t level,
+ stat_t code, const char *msg, ...);
+void status_help();
+
+/// Enter alarm state. returns same status code
+stat_t status_alarm(const char *location, stat_t status);
+
+#define TO_STRING(x) _TO_STRING(x)
+#define _TO_STRING(x) #x
+
+#define STATUS_LOCATION PSTR(__FILE__ ":" TO_STRING(__LINE__))
+
+#define STATUS_MESSAGE(LEVEL, CODE, MSG, ...) \
+ status_message_P(STATUS_LOCATION, LEVEL, CODE, PSTR(MSG), ##__VA_ARGS__)
+
+#define STATUS_INFO(MSG, ...) \
+ STATUS_MESSAGE(STAT_LEVEL_INFO, STAT_OK, MSG, ##__VA_ARGS__)
+
+#define STATUS_DEBUG(MSG, ...) \
+ STATUS_MESSAGE(STAT_LEVEL_WARNING, STAT_OK, MSG, ##__VA_ARGS__)
+
+#define STATUS_WARNING(MSG, ...) \
+ STATUS_MESSAGE(STAT_LEVEL_WARNING, STAT_OK, MSG, ##__VA_ARGS__)
+
+#define STATUS_ERROR(CODE, MSG, ...) \
+ STATUS_MESSAGE(STAT_LEVEL_ERROR, CODE, MSG, ##__VA_ARGS__)
+
+#define ALARM(CODE) status_alarm(STATUS_LOCATION, CODE)
+#define ASSERT(COND) do {if (!(COND)) ALARM(STAT_INTERNAL_ERROR);} while (0)
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2016 Buildbotics LLC
+ Copyright (c) 2010 - 2015 Alden S. Hart, Jr.
+ Copyright (c) 2013 - 2015 Robert Giseburt
+ All rights reserved.
+
+ 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/>.
+
+ The software is distributed in the hope that it will be useful, but
+ WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public
+ License along with the software. If not, see
+ <http://www.gnu.org/licenses/>.
+
+ For information regarding this software email:
+ "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#include "stepper.h"
+
+#include "config.h"
+#include "machine.h"
+#include "plan/runtime.h"
+#include "plan/exec.h"
+#include "motor.h"
+#include "hardware.h"
+#include "estop.h"
+#include "util.h"
+#include "cpp_magic.h"
+
+#include <string.h>
+#include <stdio.h>
+
+
+typedef enum {
+ MOVE_TYPE_NULL, // null move - does a no-op
+ MOVE_TYPE_ALINE, // acceleration planned line
+ MOVE_TYPE_DWELL, // delay with no movement
+} move_type_t;
+
+
+typedef struct {
+ // Runtime
+ bool busy;
+ bool requesting;
+ uint16_t dwell;
+
+ // Move prep
+ bool move_ready; // prepped move ready for loader
+ bool move_queued; // prepped move queued
+ move_type_t move_type;
+ uint16_t seg_period;
+ uint32_t prep_dwell;
+} stepper_t;
+
+
+static stepper_t st = {0};
+
+
+void stepper_init() {
+ // Setup step timer
+ TIMER_STEP.CTRLB = STEP_TIMER_WGMODE; // waveform mode
+ TIMER_STEP.INTCTRLA = STEP_TIMER_INTLVL; // interrupt mode
+ TIMER_STEP.PER = STEP_TIMER_POLL; // timer idle rate
+ TIMER_STEP.CTRLA = STEP_TIMER_ENABLE; // start step timer
+}
+
+
+void st_shutdown() {
+ for (int motor = 0; motor < MOTORS; motor++)
+ motor_enable(motor, false);
+
+ st.dwell = 0;
+ st.move_type = MOVE_TYPE_NULL;
+}
+
+
+/// Return true if motors or dwell are running
+bool st_is_busy() {return st.busy;}
+
+
+/// Interrupt handler for calling move exec function.
+/// ADC channel 0 triggered by load ISR as a "software" interrupt.
+ISR(ADCB_CH0_vect) {
+ while (true) {
+ stat_t status = mp_exec_move();
+
+ switch (status) {
+ case STAT_NOOP: st.busy = false; break; // No command executed
+ case STAT_EAGAIN: continue; // No command executed, try again
+
+ case STAT_OK: // Move executed
+ if (!st.move_queued) ALARM(STAT_EXPECTED_MOVE); // No move was queued
+ st.move_queued = false;
+ st.move_ready = true;
+ break;
+
+ default: ALARM(status); break;
+ }
+
+ break;
+ }
+
+ ADCB_CH0_INTCTRL = 0;
+ st.requesting = false;
+}
+
+
+static void _request_exec_move() {
+ if (st.requesting) return;
+ st.requesting = true;
+
+ // Use ADC as a "software" interrupt to trigger next move exec
+ ADCB_CH0_INTCTRL = ADC_CH_INTLVL_LO_gc; // LO level interrupt
+ ADCB_CTRLA = ADC_ENABLE_bm | ADC_CH0START_bm;
+}
+
+
+/// Step timer interrupt routine
+/// Dequeue move and load into stepper struct
+ISR(STEP_TIMER_ISR) {
+ // Dwell
+ if (st.dwell && --st.dwell) return;
+
+ // End last move
+ TIMER_STEP.PER = STEP_TIMER_POLL;
+
+ DMA.INTFLAGS = 0xff; // clear all interrups
+ for (int motor = 0; motor < MOTORS; motor++)
+ motor_end_move(motor);
+
+ if (estop_triggered()) {
+ st.move_type = MOVE_TYPE_NULL;
+ return;
+ }
+
+ // If the next move is not ready try to load it
+ if (!st.move_ready) {
+ _request_exec_move();
+ return;
+ }
+
+ // Wait until all motors have energized
+ if (motor_energizing()) return;
+
+ // Start move
+ if (st.seg_period) {
+ for (int motor = 0; motor < MOTORS; motor++)
+ motor_load_move(motor);
+
+ TIMER_STEP.PER = st.seg_period;
+ st.busy = true;
+
+ // Start dwell
+ st.dwell = st.prep_dwell;
+ }
+
+ // We are done with this move
+ st.move_type = MOVE_TYPE_NULL;
+ st.seg_period = 0; // clear timer
+ st.prep_dwell = 0; // clear dwell
+ st.move_ready = false; // flip the flag back
+
+ // Request next move if not currently in a dwell. Requesting the next move
+ // may power up motors and the motors should not be powered up during a dwell.
+ if (!st.dwell) _request_exec_move();
+}
+
+
+/* Prepare the next move
+ *
+ * This function precomputes the next pulse segment (move) so it can
+ * be executed quickly in the ISR. It works in steps, rather than
+ * length units. All args are provided as floats which converted here
+ * to integer values.
+ *
+ * Args:
+ * @param target signed position in steps for each motor.
+ * Steps are fractional. Their sign indicates direction. Motors not in the
+ * move have 0 steps.
+ *
+ * @param time is segment run time in minutes. If timing is not 100%
+ * accurate this will affect the move velocity but not travel distance.
+ */
+stat_t st_prep_line(float time, const float target[], const int32_t error[]) {
+ // Trap conditions that would prevent queueing the line
+ if (st.move_ready) return ALARM(STAT_INTERNAL_ERROR);
+ if (isinf(time)) return ALARM(STAT_PREP_LINE_MOVE_TIME_INFINITE);
+ if (isnan(time)) return ALARM(STAT_PREP_LINE_MOVE_TIME_NAN);
+ if (time < EPSILON) return ALARM(STAT_MINIMUM_TIME_MOVE);
+ if (MAX_SEGMENT_TIME < time) return ALARM(STAT_MAXIMUM_TIME_MOVE);
+
+ // Setup segment parameters
+ st.move_type = MOVE_TYPE_ALINE;
+ st.seg_period = round(time * 60 * STEP_TIMER_FREQ); // Must fit 16-bit
+ int32_t seg_clocks = (int32_t)st.seg_period * STEP_TIMER_DIV;
+
+ // Prepare motor moves
+ for (int motor = 0; motor < MOTORS; motor++)
+ RITORNO
+ (motor_prep_move(motor, seg_clocks, target[motor], error[motor], time));
+
+ st.move_queued = true; // signal prep buffer ready (do this last)
+
+ return STAT_OK;
+}
+
+
+/// Add a dwell to the move buffer
+void st_prep_dwell(float seconds) {
+ if (st.move_ready) ALARM(STAT_INTERNAL_ERROR);
+ st.move_type = MOVE_TYPE_DWELL;
+ st.seg_period = STEP_TIMER_FREQ * 0.001; // 1 ms
+ st.prep_dwell = seconds * 1000; // convert to ms
+ st.move_queued = true; // signal prep buffer ready
+}
+
+
+void st_get_error(int32_t error[]) {
+ for (int motor = 0; motor < MOTORS; motor++)
+ error[motor] = motor_get_error(motor);
+}
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2016 Buildbotics LLC
+ Copyright (c) 2010 - 2015 Alden S. Hart, Jr.
+ Copyright (c) 2012 - 2015 Rob Giseburt
+ All rights reserved.
+
+ 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/>.
+
+ The software is distributed in the hope that it will be useful, but
+ WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public
+ License along with the software. If not, see
+ <http://www.gnu.org/licenses/>.
+
+ For information regarding this software email:
+ "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#pragma once
+
+#include "status.h"
+
+#include <stdbool.h>
+#include <stdint.h>
+
+
+void stepper_init();
+void st_shutdown();
+bool st_is_busy();
+stat_t st_prep_line(float time, const float target[], const int32_t error[]);
+void st_prep_dwell(float seconds);
+void st_get_error(int32_t error[]);
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2016 Buildbotics LLC
+ Copyright (c) 2010 - 2015 Alden S. Hart, Jr.
+ All rights reserved.
+
+ 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/>.
+
+ The software is distributed in the hope that it will be useful, but
+ WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public
+ License along with the software. If not, see
+ <http://www.gnu.org/licenses/>.
+
+ For information regarding this software email:
+ "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+/* Normally open switches (NO) trigger an interrupt on the
+ * falling edge and lockout subsequent interrupts for the defined
+ * lockout period. This approach beats doing debouncing as an
+ * integration as switches fire immediately.
+ *
+ * Normally closed switches (NC) trigger an interrupt on the
+ * rising edge and lockout subsequent interrupts for the defined
+ * lockout period.
+ *
+ * These functions interact with each other to process switch closures
+ * and firing. Each switch has a counter which is initially set to
+ * negative SW_DEGLITCH_TICKS. When a switch closure is DETECTED the
+ * count increments for each RTC tick. When the count reaches zero
+ * the switch is tripped and action occurs. The counter continues to
+ * increment positive until the lockout is exceeded.
+ */
+
+#include "switch.h"
+#include "config.h"
+
+#include <avr/interrupt.h>
+
+#include <stdbool.h>
+
+
+typedef enum {
+ SW_IDLE,
+ SW_DEGLITCHING,
+ SW_LOCKOUT
+} switch_debounce_t;
+
+
+typedef struct {
+ uint8_t pin;
+ switch_type_t type;
+
+ switch_callback_t cb;
+ bool state;
+ switch_debounce_t debounce;
+ int16_t count;
+} switch_t;
+
+
+
+static switch_t switches[SWITCHES] = {
+ {.pin = MIN_X_PIN, .type = SW_NORMALLY_OPEN},
+ {.pin = MAX_X_PIN, .type = SW_NORMALLY_OPEN},
+ {.pin = MIN_Y_PIN, .type = SW_NORMALLY_OPEN},
+ {.pin = MAX_X_PIN, .type = SW_NORMALLY_OPEN},
+ {.pin = MIN_Z_PIN, .type = SW_NORMALLY_OPEN},
+ {.pin = MAX_Z_PIN, .type = SW_NORMALLY_OPEN},
+ {.pin = MIN_A_PIN, .type = SW_NORMALLY_OPEN},
+ {.pin = MAX_A_PIN, .type = SW_NORMALLY_OPEN},
+ {.pin = ESTOP_PIN, .type = SW_NORMALLY_OPEN},
+ // {.pin = PROBE_PIN, .type = SW_NORMALLY_OPEN},
+};
+
+
+static bool _read_state(const switch_t *s) {
+ // A normally open switch drives the pin low when thrown
+ return (s->type == SW_NORMALLY_OPEN) ^ IN_PIN(s->pin);
+}
+
+
+static void _switch_isr() {
+ for (int i = 0; i < SWITCHES; i++) {
+ switch_t *s = &switches[i];
+ bool state = _read_state(s);
+
+ if (state == s->state || s->type == SW_DISABLED) continue;
+
+ s->debounce = SW_DEGLITCHING;
+ s->count = -SW_DEGLITCH_TICKS; // reset deglitch count
+ s->state = state;
+ PORT(s->pin)->INT0MASK &= ~BM(s->pin); // Disable INT0
+ }
+}
+
+
+// Switch interrupt handler vectors
+ISR(PORTA_INT0_vect) {_switch_isr();}
+ISR(PORTB_INT0_vect) {_switch_isr();}
+ISR(PORTC_INT0_vect) {_switch_isr();}
+ISR(PORTD_INT0_vect) {_switch_isr();}
+ISR(PORTE_INT0_vect) {_switch_isr();}
+ISR(PORTF_INT0_vect) {_switch_isr();}
+
+
+void _switch_enable(switch_t *s, bool enable) {
+ if (enable) {
+ PORT(s->pin)->INT0MASK |= BM(s->pin); // Enable INT0
+ s->state = _read_state(s); // Initialize state
+ s->debounce = SW_IDLE;
+
+ } else PORT(s->pin)->INT0MASK &= ~BM(s->pin); // Disable INT0
+}
+
+
+void switch_init() {
+ for (int i = 0; i < SWITCHES; i++) {
+ switch_t *s = &switches[i];
+
+ // Pull up and trigger on both edges
+ PINCTRL_PIN(s->pin) = PORT_OPC_PULLUP_gc | PORT_ISC_BOTHEDGES_gc;
+ DIRCLR_PIN(s->pin); // Input
+ PORT(s->pin)->INTCTRL |= SWITCH_INTLVL; // Set interrupt level
+
+ _switch_enable(s, s->type != SW_DISABLED);
+ }
+}
+
+
+/// Called from RTC on each tick
+void switch_rtc_callback() {
+ for (int i = 0; i < SWITCHES; i++) {
+ switch_t *s = &switches[i];
+
+ if (s->type == SW_DISABLED || s->debounce == SW_IDLE) continue;
+
+ // state is either lockout or deglitching
+ if (++s->count == SW_LOCKOUT_TICKS) {
+ PORT(s->pin)->INT0MASK |= BM(s->pin); // Renable INT0
+ bool state = _read_state(s);
+ s->debounce = SW_IDLE;
+
+ // check if the state has changed while we were in lockout
+ if (s->state != state) {
+ s->state = state;
+ s->debounce = SW_DEGLITCHING;
+ s->count = -SW_DEGLITCH_TICKS;
+ }
+
+ continue;
+ }
+
+ if (!s->count) { // switch triggered
+ s->debounce = SW_LOCKOUT;
+ if (s->cb) s->cb(i, s->state);
+ }
+ }
+}
+
+
+bool switch_is_active(int index) {
+ return switches[index].state;
+}
+
+
+bool switch_is_enabled(int index) {
+ return switch_get_type(index) != SW_DISABLED;
+}
+
+
+switch_type_t switch_get_type(int index) {
+ return switches[index].type;
+}
+
+
+void switch_set_type(int index, switch_type_t type) {
+ switch_t *s = &switches[index];
+
+ if (s->type != type) {
+ s->type = type;
+ _switch_enable(s, type != SW_DISABLED);
+ }
+}
+
+
+void switch_set_callback(int index, switch_callback_t cb) {
+ switches[index].cb = cb;
+}
+
+
+// Var callbacks
+uint8_t get_switch_type(int index) {return switch_get_type(index);}
+void set_switch_type(int index, uint8_t value) {switch_set_type(index, value);}
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2016 Buildbotics LLC
+ Copyright (c) 2010 - 2015 Alden S. Hart, Jr.
+ All rights reserved.
+
+ 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/>.
+
+ The software is distributed in the hope that it will be useful, but
+ WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public
+ License along with the software. If not, see
+ <http://www.gnu.org/licenses/>.
+
+ For information regarding this software email:
+ "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#pragma once
+
+
+#include "config.h"
+
+#include <stdint.h>
+#include <stdbool.h>
+
+
+// macros for finding the index into the switch table give the axis number
+#define MIN_SWITCH(axis) (axis * 2)
+#define MAX_SWITCH(axis) (axis * 2 + 1)
+
+
+typedef enum {
+ SW_DISABLED,
+ SW_NORMALLY_OPEN,
+ SW_NORMALLY_CLOSED
+} switch_type_t;
+
+
+/// Switch IDs
+typedef enum {
+ SW_MIN_X, SW_MAX_X,
+ SW_MIN_Y, SW_MAX_Y,
+ SW_MIN_Z, SW_MAX_Z,
+ SW_MIN_A, SW_MAX_A,
+ SW_ESTOP, SW_PROBE
+} switch_id_t;
+
+
+typedef void (*switch_callback_t)(switch_id_t sw, bool active);
+
+
+void switch_init();
+void switch_rtc_callback();
+bool switch_is_active(int index);
+bool switch_is_enabled(int index);
+switch_type_t switch_get_type(int index);
+void switch_set_type(int index, switch_type_t type);
+void switch_set_callback(int index, switch_callback_t cb);
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2016 Buildbotics LLC
+ All rights reserved.
+
+ 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/>.
+
+ The software is distributed in the hope that it will be useful, but
+ WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public
+ License along with the software. If not, see
+ <http://www.gnu.org/licenses/>.
+
+ For information regarding this software email:
+ "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#include "usart.h"
+#include "cpp_magic.h"
+#include "config.h"
+
+#include <avr/io.h>
+#include <avr/interrupt.h>
+
+#include <stdio.h>
+#include <stdbool.h>
+
+// Ring buffers
+#define RING_BUF_NAME tx_buf
+#define RING_BUF_SIZE USART_TX_RING_BUF_SIZE
+#include "ringbuf.def"
+
+#define RING_BUF_NAME rx_buf
+#define RING_BUF_SIZE USART_RX_RING_BUF_SIZE
+#include "ringbuf.def"
+
+static int usart_flags = USART_CRLF;
+
+
+static void _set_dre_interrupt(bool enable) {
+ if (enable) SERIAL_PORT.CTRLA |= USART_DREINTLVL_HI_gc;
+ else SERIAL_PORT.CTRLA &= ~USART_DREINTLVL_HI_gc;
+}
+
+
+static void _set_rxc_interrupt(bool enable) {
+ if (enable) {
+ SERIAL_PORT.CTRLA |= USART_RXCINTLVL_HI_gc;
+ if (4 <= rx_buf_space()) OUTCLR_PIN(SERIAL_CTS_PIN); // CTS Lo (enable)
+
+ } else SERIAL_PORT.CTRLA &= ~USART_RXCINTLVL_HI_gc;
+}
+
+
+// Data register empty interrupt vector
+ISR(SERIAL_DRE_vect) {
+ if (tx_buf_empty()) _set_dre_interrupt(false); // Disable interrupt
+
+ else {
+ SERIAL_PORT.DATA = tx_buf_peek();
+ tx_buf_pop();
+ }
+}
+
+
+// Data received interrupt vector
+ISR(SERIAL_RXC_vect) {
+ if (rx_buf_full()) _set_rxc_interrupt(false); // Disable interrupt
+
+ else {
+ uint8_t data = SERIAL_PORT.DATA;
+ rx_buf_push(data);
+ if (rx_buf_space() < 4) OUTSET_PIN(SERIAL_CTS_PIN); // CTS Hi (disable)
+ }
+}
+
+
+static int _usart_putchar(char c, FILE *f) {
+ usart_putc(c);
+ return 0;
+}
+
+static FILE _stdout = FDEV_SETUP_STREAM(_usart_putchar, 0, _FDEV_SETUP_WRITE);
+
+
+void usart_init(void) {
+ // Setup ring buffer
+ tx_buf_init();
+ rx_buf_init();
+
+ PR.PRPC &= ~PR_USART0_bm; // Disable power reduction
+
+ // Setup pins
+ OUTSET_PIN(SERIAL_CTS_PIN); // CTS Hi (disable)
+ DIRSET_PIN(SERIAL_CTS_PIN); // CTS Output
+ OUTSET_PIN(SERIAL_TX_PIN); // Tx High
+ DIRSET_PIN(SERIAL_TX_PIN); // Tx Output
+ DIRCLR_PIN(SERIAL_RX_PIN); // Rx Input
+
+ // Set baud rate
+ usart_set_baud(SERIAL_BAUD);
+
+ // No parity, 8 data bits, 1 stop bit
+ SERIAL_PORT.CTRLC = USART_CMODE_ASYNCHRONOUS_gc | USART_PMODE_DISABLED_gc |
+ USART_CHSIZE_8BIT_gc;
+
+ // Configure receiver and transmitter
+ SERIAL_PORT.CTRLB = USART_RXEN_bm | USART_TXEN_bm | USART_CLK2X_bm;
+
+ PMIC.CTRL |= PMIC_HILVLEN_bm; // Interrupt level on
+
+ // Connect IO
+ stdout = &_stdout;
+ stderr = &_stdout;
+
+ // Enable Rx
+ _set_rxc_interrupt(true);
+}
+
+
+static void _set_baud(uint16_t bsel, uint8_t bscale) {
+ SERIAL_PORT.BAUDCTRLB = (uint8_t)((bscale << 4) | (bsel >> 8));
+ SERIAL_PORT.BAUDCTRLA = bsel;
+}
+
+
+void usart_set_baud(int baud) {
+ // The BSEL / BSCALE values provided below assume a 32 Mhz clock
+ // Assumes CTRLB CLK2X bit (0x04) is set
+ // See http://www.avrcalc.elektronik-projekt.de/xmega/baud_rate_calculator
+
+ switch (baud) {
+ case USART_BAUD_9600: _set_baud(3325, 0b1101); break;
+ case USART_BAUD_19200: _set_baud(3317, 0b1100); break;
+ case USART_BAUD_38400: _set_baud(3301, 0b1011); break;
+ case USART_BAUD_57600: _set_baud(1095, 0b1100); break;
+ case USART_BAUD_115200: _set_baud(1079, 0b1011); break;
+ case USART_BAUD_230400: _set_baud(1047, 0b1010); break;
+ case USART_BAUD_460800: _set_baud(983, 0b1001); break;
+ case USART_BAUD_921600: _set_baud(107, 0b1011); break;
+ case USART_BAUD_500000: _set_baud(1, 0b0010); break;
+ case USART_BAUD_1000000: _set_baud(1, 0b0001); break;
+ }
+}
+
+
+void usart_set(int flag, bool enable) {
+ if (enable) usart_flags |= flag;
+ else usart_flags &= ~flag;
+}
+
+
+bool usart_is_set(int flags) {
+ return (usart_flags & flags) == flags;
+}
+
+
+void usart_putc(char c) {
+ while (tx_buf_full() || (usart_flags & USART_FLUSH)) continue;
+
+ tx_buf_push(c);
+
+ _set_dre_interrupt(true); // Enable interrupt
+
+ if ((usart_flags & USART_CRLF) && c == '\n') usart_putc('\r');
+}
+
+
+void usart_puts(const char *s) {
+ while (*s) usart_putc(*s++);
+}
+
+
+int8_t usart_getc() {
+ while (rx_buf_empty()) continue;
+
+ uint8_t data = rx_buf_peek();
+ rx_buf_pop();
+
+ _set_rxc_interrupt(true); // Enable interrupt
+
+ return data;
+}
+
+
+char *usart_readline() {
+ static char line[INPUT_BUFFER_LEN];
+ static int i = 0;
+ bool eol = false;
+
+ while (!rx_buf_empty()) {
+ char data = usart_getc();
+
+ if (usart_flags & USART_ECHO) usart_putc(data);
+
+ switch (data) {
+ case '\r': case '\n': eol = true; break;
+
+ case '\b': // BS - backspace
+ if (usart_flags & USART_ECHO) {
+ usart_putc(' ');
+ usart_putc('\b');
+ }
+ if (i) i--;
+ break;
+
+ case 0x18: // CAN - Cancel or CTRL-X
+ if (usart_flags & USART_ECHO)
+ while (i) {
+ usart_putc('\b');
+ usart_putc(' ');
+ usart_putc('\b');
+ i--;
+ }
+
+ i = 0;
+ break;
+
+ default:
+ line[i++] = data;
+ if (i == INPUT_BUFFER_LEN - 1) eol = true;
+ break;
+ }
+
+ if (eol) {
+ line[i] = 0;
+ i = 0;
+ return line;
+ }
+ }
+
+ return 0;
+}
+
+
+int16_t usart_peek() {
+ return rx_buf_empty() ? -1 : rx_buf_peek();
+}
+
+
+void usart_flush() {
+ usart_set(USART_FLUSH, true);
+
+ while (!tx_buf_empty() || !(SERIAL_PORT.STATUS & USART_DREIF_bm) ||
+ !(SERIAL_PORT.STATUS & USART_TXCIF_bm))
+ continue;
+}
+
+
+void usart_rx_flush() {
+ rx_buf_init();
+}
+
+
+int16_t usart_rx_space() {
+ return rx_buf_space();
+}
+
+
+int16_t usart_rx_fill() {
+ return rx_buf_fill();
+}
+
+
+int16_t usart_tx_space() {
+ return tx_buf_space();
+}
+
+
+int16_t usart_tx_fill() {
+ return tx_buf_fill();
+}
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2016 Buildbotics LLC
+ All rights reserved.
+
+ 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/>.
+
+ The software is distributed in the hope that it will be useful, but
+ WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public
+ License along with the software. If not, see
+ <http://www.gnu.org/licenses/>.
+
+ For information regarding this software email:
+ "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#pragma once
+
+
+#include <stdint.h>
+#include <stdbool.h>
+
+#define USART_TX_RING_BUF_SIZE 256
+#define USART_RX_RING_BUF_SIZE 256
+
+enum {
+ USART_BAUD_9600,
+ USART_BAUD_19200,
+ USART_BAUD_38400,
+ USART_BAUD_57600,
+ USART_BAUD_115200,
+ USART_BAUD_230400,
+ USART_BAUD_460800,
+ USART_BAUD_921600,
+ USART_BAUD_500000,
+ USART_BAUD_1000000
+};
+
+enum {
+ USART_CRLF = 1 << 0,
+ USART_ECHO = 1 << 1,
+ USART_XOFF = 1 << 2,
+ USART_FLUSH = 1 << 3,
+};
+
+void usart_init();
+void usart_set_baud(int baud);
+void usart_set(int flag, bool enable);
+bool usart_is_set(int flags);
+void usart_putc(char c);
+void usart_puts(const char *s);
+int8_t usart_getc();
+char *usart_readline();
+int16_t usart_peek();
+void usart_flush();
+
+void usart_rx_flush();
+int16_t usart_rx_fill();
+int16_t usart_rx_space();
+inline bool usart_rx_empty() {return !usart_rx_fill();}
+inline bool usart_rx_full() {return !usart_rx_space();}
+
+int16_t usart_tx_fill();
+int16_t usart_tx_space();
+inline bool usart_tx_empty() {return !usart_tx_fill();}
+inline bool usart_tx_full() {return !usart_tx_space();}
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2016 Buildbotics LLC
+ Copyright (c) 2010 - 2015 Alden S. Hart, Jr.
+ All rights reserved.
+
+ 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/>.
+
+ The software is distributed in the hope that it will be useful, but
+ WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public
+ License along with the software. If not, see
+ <http://www.gnu.org/licenses/>.
+
+ For information regarding this software email:
+ "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#include "util.h"
+
+
+
+
+/// Fast inverse square root originally from Quake III Arena code. Original
+/// comments left intact.
+/// See: https://en.wikipedia.org/wiki/Fast_inverse_square_root
+float invsqrt(float number) {
+ const float threehalfs = 1.5F;
+
+ float x2 = number * 0.5;
+ float y = number;
+ long i = *(long *)&y; // evil floating point bit level hacking
+ i = 0x5f3759df - (i >> 1); // what the fuck?
+ y = *(float *)&i;
+ y = y * (threehalfs - x2 * y * y); // 1st iteration
+ y = y * (threehalfs - x2 * y * y); // 2nd iteration, this can be removed
+
+ return y; //isnan(y) ? 1.0 / sqrt(number) : y;
+}
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2016 Buildbotics LLC
+ Copyright (c) 2010 - 2014 Alden S. Hart, Jr.
+ All rights reserved.
+
+ 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/>.
+
+ The software is distributed in the hope that it will be useful, but
+ WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public
+ License along with the software. If not, see
+ <http://www.gnu.org/licenses/>.
+
+ For information regarding this software email:
+ "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#pragma once
+
+
+#include "config.h"
+
+#include <stdint.h>
+#include <math.h>
+#include <string.h>
+#include <stdbool.h>
+
+
+// Vector utilities
+#define clear_vector(a) memset(a, 0, sizeof(a))
+#define copy_vector(d, s) memcpy(d, s, sizeof(d))
+
+// Math utilities
+inline float min(float a, float b) {return a < b ? a : b;}
+inline float max(float a, float b) {return a < b ? b : a;}
+inline float min3(float a, float b, float c) {return min(min(a, b), c);}
+inline float max3(float a, float b, float c) {return max(max(a, b), c);}
+inline float min4(float a, float b, float c, float d)
+{return min(min(a, b), min(c, d));}
+inline float max4(float a, float b, float c, float d)
+{return max(max(a, b), max(c, d));}
+
+float invsqrt(float number);
+
+// Floating-point utilities
+#define EPSILON 0.00001 // allowable rounding error for floats
+inline bool fp_EQ(float a, float b) {return fabs(a - b) < EPSILON;}
+inline bool fp_NE(float a, float b) {return fabs(a - b) > EPSILON;}
+inline bool fp_ZERO(float a) {return fabs(a) < EPSILON;}
+inline bool fp_NOT_ZERO(float a) {return !fp_ZERO(a);}
+inline bool fp_FALSE(float a) {return fp_ZERO(a);}
+inline bool fp_TRUE(float a) {return !fp_ZERO(a);}
+
+// Constants
+#define MM_PER_INCH 25.4
+#define INCHES_PER_MM (1 / 25.4)
+#define MICROSECONDS_PER_MINUTE 60000000.0
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2016 Buildbotics LLC
+ Copyright (c) 2010 - 2015 Alden S. Hart, Jr.
+ All rights reserved.
+
+ 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/>.
+
+ The software is distributed in the hope that it will be useful, but
+ WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public
+ License along with the software. If not, see
+ <http://www.gnu.org/licenses/>.
+
+ For information regarding this software email:
+ "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#include "usart.h"
+#include "machine.h"
+#include "spindle.h"
+#include "coolant.h"
+#include "plan/runtime.h"
+#include "plan/state.h"
+
+// Axis
+float get_position(int index) {return mp_runtime_get_axis_position(index);}
+
+// GCode
+int32_t get_line() {return mp_runtime_get_line();}
+PGM_P get_unit() {return gs_get_units_pgmstr(mach_get_units());}
+float get_speed() {return spindle_get_speed();}
+float get_feed() {return mach_get_feed_rate();} // TODO get runtime value
+uint8_t get_tool() {return mp_runtime_get_tool();}
+
+
+PGM_P get_feed_mode() {
+ return gs_get_feed_mode_pgmstr(mach_get_feed_mode());
+}
+
+
+PGM_P get_plane() {return gs_get_plane_pgmstr(mach_get_plane());}
+
+
+PGM_P get_coord_system() {
+ return gs_get_coord_system_pgmstr(mach_get_coord_system());
+}
+
+
+bool get_abs_override() {return mach_get_absolute_mode();}
+PGM_P get_path_mode() {return gs_get_path_mode_pgmstr(mach_get_path_mode());}
+
+
+PGM_P get_distance_mode() {
+ return gs_get_distance_mode_pgmstr(mach_get_distance_mode());
+}
+
+
+PGM_P get_arc_dist_mode() {
+ return gs_get_distance_mode_pgmstr(mach_get_arc_distance_mode());
+}
+
+
+float get_feed_override() {return mach_get_feed_override();}
+float get_speed_override() {return mach_get_spindle_override();}
+bool get_mist_coolant() {return coolant_get_mist();}
+bool get_flood_coolant() {return coolant_get_flood();}
+
+// System
+float get_velocity() {return mp_runtime_get_velocity();}
+bool get_echo() {return usart_is_set(USART_ECHO);}
+void set_echo(bool value) {return usart_set(USART_ECHO, value);}
+PGM_P get_state() {return mp_get_state_pgmstr(mp_get_state());}
+PGM_P get_cycle() {return mp_get_cycle_pgmstr(mp_get_cycle());}
+
+
+PGM_P get_hold_reason() {
+ return mp_get_hold_reason_pgmstr(mp_get_hold_reason());
+}
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2016 Buildbotics LLC
+ All rights reserved.
+
+ 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/>.
+
+ The software is distributed in the hope that it will be useful, but
+ WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public
+ License along with the software. If not, see
+ <http://www.gnu.org/licenses/>.
+
+ For information regarding this software email:
+ "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#include "vars.h"
+
+#include "cpp_magic.h"
+#include "status.h"
+#include "hardware.h"
+#include "config.h"
+
+#include <stdint.h>
+#include <stdbool.h>
+#include <stdio.h>
+#include <string.h>
+#include <stdlib.h>
+#include <ctype.h>
+#include <math.h>
+
+#include <avr/pgmspace.h>
+#include <avr/eeprom.h>
+
+
+typedef uint8_t flags_t;
+typedef const char *string;
+typedef PGM_P pstring;
+
+
+// Format strings
+static const char code_fmt[] PROGMEM = "\"%s\":";
+static const char indexed_code_fmt[] PROGMEM = "\"%c%s\":";
+
+
+// Type names
+static const char bool_name [] PROGMEM = "<bool>";
+#define TYPE_NAME(TYPE) static const char TYPE##_name [] PROGMEM = "<" #TYPE ">"
+MAP(TYPE_NAME, SEMI, flags_t, string, pstring, float, uint8_t, uint16_t,
+ int32_t, char);
+
+
+// String
+static void var_print_string(string s) {
+ printf_P(PSTR("\"%s\""), s);
+}
+
+// Program string
+static void var_print_pstring(pstring s) {
+ printf_P(PSTR("\"%S\""), s);
+}
+
+
+// Flags
+extern void print_status_flags(uint8_t x);
+
+static void var_print_flags_t(uint8_t x) {
+ print_status_flags(x);
+}
+
+
+// Float
+static void var_print_float(float x) {
+ char buf[20];
+
+ int len = sprintf_P(buf, PSTR("%.6f"), x);
+
+ // Remove trailing zeros
+ for (int i = len; 0 < i; i--) {
+ if (buf[i - 1] == '.') buf[i - 1] = 0;
+ else if (buf[i - 1] == '0') {
+ buf[i - 1] = 0;
+ continue;
+ }
+
+ break;
+ }
+
+ printf(buf);
+}
+
+
+static float var_parse_float(const char *value) {
+ return strtod(value, 0);
+}
+
+
+// Bool
+static void var_print_bool(bool x) {
+ printf_P(x ? PSTR("true") : PSTR("false"));
+}
+
+
+static bool var_parse_bool(const char *value) {
+ return !strcasecmp(value, "true") || var_parse_float(value);
+}
+
+
+static uint8_t eeprom_read_bool(bool *addr) {
+ return eeprom_read_byte((uint8_t *)addr);
+}
+
+
+static void eeprom_update_bool(bool *addr, bool value) {
+ eeprom_update_byte((uint8_t *)addr, value);
+}
+
+
+// Char
+static void var_print_char(char x) {putchar('"'); putchar(x); putchar('"');}
+static char var_parse_char(const char *value) {return value[0];}
+
+
+static uint8_t eeprom_read_char(char *addr) {
+ return eeprom_read_byte((uint8_t *)addr);
+}
+
+
+static void eeprom_update_char(char *addr, char value) {
+ eeprom_update_byte((uint8_t *)addr, value);
+}
+
+
+// int8
+#if 0
+static void var_print_int8_t(int8_t x) {
+ printf_P(PSTR("%"PRIi8), x);
+}
+
+
+static int8_t var_parse_int8_t(const char *value) {
+ return strtol(value, 0, 0);
+}
+
+
+static int8_t eeprom_read_int8_t(int8_t *addr) {
+ return eeprom_read_byte((uint8_t *)addr);
+}
+
+
+static void eeprom_update_int8_t(int8_t *addr, int8_t value) {
+ eeprom_update_byte((uint8_t *)addr, value);
+}
+#endif
+
+// uint8
+static void var_print_uint8_t(uint8_t x) {
+ printf_P(PSTR("%"PRIu8), x);
+}
+
+
+static uint8_t var_parse_uint8_t(const char *value) {
+ return strtol(value, 0, 0);
+}
+
+
+static uint8_t eeprom_read_uint8_t(uint8_t *addr) {
+ return eeprom_read_byte(addr);
+}
+
+
+static void eeprom_update_uint8_t(uint8_t *addr, uint8_t value) {
+ eeprom_update_byte(addr, value);
+}
+
+
+// unit16
+static void var_print_uint16_t(uint16_t x) {
+ printf_P(PSTR("%"PRIu16), x);
+}
+
+
+static uint16_t var_parse_uint16_t(const char *value) {
+ return strtoul(value, 0, 0);
+}
+
+
+static uint16_t eeprom_read_uint16_t(uint16_t *addr) {
+ return eeprom_read_word(addr);
+}
+
+
+static void eeprom_update_uint16_t(uint16_t *addr, uint16_t value) {
+ eeprom_update_word(addr, value);
+}
+
+
+// int32
+static void var_print_int32_t(uint32_t x) {
+ printf_P(PSTR("%"PRIi32), x);
+}
+
+
+// Var forward declarations
+#define VAR(NAME, CODE, TYPE, INDEX, SET, ...) \
+ TYPE get_##NAME(IF(INDEX)(int index)); \
+ IF(SET) \
+ (void set_##NAME(IF(INDEX)(int index,) TYPE value);)
+
+#include "vars.def"
+#undef VAR
+
+// Var names, count & help
+#define VAR(NAME, CODE, TYPE, INDEX, SET, SAVE, HELP) \
+ static const char NAME##_name[] PROGMEM = #NAME; \
+ static const char NAME##_help[] PROGMEM = HELP;
+
+#include "vars.def"
+#undef VAR
+
+// EEPROM storage
+#define VAR(NAME, CODE, TYPE, INDEX, SET, SAVE, HELP) \
+ IF(SAVE) \
+ (static TYPE NAME##_eeprom IF(INDEX)([INDEX]) EEMEM;)
+
+#include "vars.def"
+#undef VAR
+
+static uint16_t eeprom_crc EEMEM;
+
+// Last value
+#define VAR(NAME, CODE, TYPE, INDEX, ...) \
+ static TYPE NAME##_state IF(INDEX)([INDEX]);
+
+#include "vars.def"
+#undef VAR
+
+
+
+void vars_init() {
+ vars_restore();
+
+ // Initialize var state
+#define VAR(NAME, CODE, TYPE, INDEX, SET, SAVE, ...) \
+ IF(INDEX)(for (int i = 0; i < INDEX; i++)) \
+ (NAME##_state)IF(INDEX)([i]) = get_##NAME(IF(INDEX)(i));
+
+#include "vars.def"
+#undef VAR
+}
+
+
+void vars_report(bool full) {
+ // Save and disable watchdog
+ uint8_t wd_state = hw_disable_watchdog();
+
+ bool reported = false;
+
+#define VAR(NAME, CODE, TYPE, INDEX, ...) \
+ IF(INDEX)(for (int i = 0; i < (INDEX ? INDEX : 1); i++)) { \
+ TYPE value = get_##NAME(IF(INDEX)(i)); \
+ TYPE last = (NAME##_state)IF(INDEX)([i]); \
+ \
+ if (value != last || full) { \
+ (NAME##_state)IF(INDEX)([i]) = value; \
+ \
+ if (!reported) { \
+ reported = true; \
+ putchar('{'); \
+ } else putchar(','); \
+ \
+ printf_P \
+ (IF_ELSE(INDEX)(indexed_code_fmt, code_fmt), \
+ IF(INDEX)(INDEX##_LABEL[i],) #CODE); \
+ \
+ var_print_##TYPE(value); \
+ } \
+ }
+
+#include "vars.def"
+#undef VAR
+
+ if (reported) printf("}\n");
+
+ // Restore watchdog
+ hw_restore_watchdog(wd_state);
+}
+
+
+int vars_find(const char *name) {
+ uint8_t i = 0;
+ uint8_t n = 0;
+ unsigned len = strlen(name);
+
+ if (!len) return -1;
+
+#define VAR(NAME, CODE, TYPE, INDEX, ...) \
+ if (!strcmp(IF_ELSE(INDEX)(name + 1, name), #CODE)) { \
+ IF(INDEX) \
+ (i = strchr(INDEX##_LABEL, name[0]) - INDEX##_LABEL; \
+ if (INDEX <= i) return -1); \
+ return n; \
+ } \
+ n++;
+
+#include "vars.def"
+#undef VAR
+
+ return -1;
+}
+
+
+bool vars_print(const char *name) {
+ uint8_t i;
+ unsigned len = strlen(name);
+
+ if (!len) return false;
+
+#define VAR(NAME, CODE, TYPE, INDEX, ...) \
+ if (!strcmp(IF_ELSE(INDEX)(name + 1, name), #CODE)) { \
+ IF(INDEX) \
+ (i = strchr(INDEX##_LABEL, name[0]) - INDEX##_LABEL; \
+ if (INDEX <= i) return false); \
+ \
+ putchar('{'); \
+ printf_P \
+ (IF_ELSE(INDEX)(indexed_code_fmt, code_fmt), \
+ IF(INDEX)(INDEX##_LABEL[i],) #CODE); \
+ var_print_##TYPE(get_##NAME(IF(INDEX)(i))); \
+ putchar('}'); \
+ \
+ return true; \
+ }
+
+#include "vars.def"
+#undef VAR
+
+ return false;
+}
+
+
+bool vars_set(const char *name, const char *value) {
+ uint8_t i;
+ unsigned len = strlen(name);
+
+ if (!len) return false;
+
+#define VAR(NAME, CODE, TYPE, INDEX, SET, ...) \
+ IF(SET) \
+ (if (!strcmp(IF_ELSE(INDEX)(name + 1, name), #CODE)) { \
+ IF(INDEX) \
+ (i = strchr(INDEX##_LABEL, name[0]) - INDEX##_LABEL; \
+ if (INDEX <= i) return false); \
+ \
+ TYPE x = var_parse_##TYPE(value); \
+ set_##NAME(IF(INDEX)(i,) x); \
+ \
+ return true; \
+ }) \
+
+#include "vars.def"
+#undef VAR
+
+ return false;
+}
+
+
+int vars_parser(char *vars) {
+ if (!*vars || !*vars++ == '{') return STAT_OK;
+
+ while (*vars) {
+ while (isspace(*vars)) vars++;
+ if (*vars == '}') return STAT_OK;
+ if (*vars != '"') return STAT_COMMAND_NOT_ACCEPTED;
+
+ // Parse name
+ vars++; // Skip "
+ const char *name = vars;
+ while (*vars && *vars != '"') vars++;
+ *vars++ = 0;
+
+ while (isspace(*vars)) vars++;
+ if (*vars != ':') return STAT_COMMAND_NOT_ACCEPTED;
+ vars++;
+ while (isspace(*vars)) vars++;
+
+ // Parse value
+ const char *value = vars;
+ while (*vars && *vars != ',' && *vars != '}') vars++;
+ if (*vars) {
+ char c = *vars;
+ *vars++ = 0;
+ vars_set(name, value);
+ if (c == '}') break;
+ }
+ }
+
+ return STAT_OK;
+}
+
+
+void vars_print_help() {
+ static const char fmt[] PROGMEM = " $%-5s %-20S %-16S %S\n";
+
+ // Save and disable watchdog
+ uint8_t wd_state = hw_disable_watchdog();
+
+#define VAR(NAME, CODE, TYPE, ...) \
+ printf_P(fmt, #CODE, NAME##_name, TYPE##_name, NAME##_help);
+#include "vars.def"
+#undef VAR
+
+ // Restore watchdog
+ hw_restore_watchdog(wd_state);
+}
+
+
+uint16_t vars_crc() {
+ // Save and disable watchdog
+ uint8_t wd_state = hw_disable_watchdog();
+
+ CRC.CTRL = CRC_RESET_RESET0_gc;
+ CRC.CTRL = CRC_SOURCE_IO_gc; // Must be after reset
+
+#define VAR(NAME, CODE, TYPE, INDEX, SET, SAVE, ...) \
+ IF(SAVE) \
+ ({ \
+ for (int j = 0; ; j++) { \
+ char c = pgm_read_byte(&NAME##_name[j]); \
+ if (!c) break; \
+ CRC.DATAIN = c; \
+ } \
+ \
+ CRC.DATAIN = INDEX; \
+ })
+
+#include "vars.def"
+#undef VAR
+
+ // Restore watchdog
+ hw_restore_watchdog(wd_state);
+
+ CRC.STATUS = CRC_BUSY_bm; // Done
+ return CRC.CHECKSUM1 << 8 | CRC.CHECKSUM0;
+}
+
+
+void vars_save() {
+ // Save and disable watchdog
+ uint8_t wd_state = hw_disable_watchdog();
+
+#define VAR(NAME, CODE, TYPE, INDEX, SET, SAVE, ...) \
+ IF(SAVE) \
+ (IF(INDEX)(for (int i = 0; i < (INDEX ? INDEX : 1); i++)) { \
+ TYPE value = get_##NAME(IF(INDEX)(i)); \
+ eeprom_update_##TYPE(&NAME##_eeprom IF(INDEX)([i]), value); \
+ }) \
+
+#include "vars.def"
+#undef VAR
+
+ // Restore watchdog
+ hw_restore_watchdog(wd_state);
+
+ // Save CRC
+ eeprom_update_word(&eeprom_crc, vars_crc());
+}
+
+
+
+bool vars_valid() {
+ return eeprom_read_word(&eeprom_crc) == vars_crc();
+}
+
+
+stat_t vars_restore() {
+ if (!vars_valid()) return STAT_EEPROM_DATA_INVALID;
+
+ // Save and disable watchdog
+ uint8_t wd_state = hw_disable_watchdog();
+
+#define VAR(NAME, CODE, TYPE, INDEX, SET, SAVE, ...) \
+ IF(SAVE) \
+ (IF(INDEX)(for (int i = 0; i < (INDEX ? INDEX : 1); i++)) { \
+ TYPE value = eeprom_read_##TYPE(&NAME##_eeprom IF(INDEX)([i])); \
+ set_##NAME(IF(INDEX)(i,) value); \
+ NAME##_state IF(INDEX)([i]) = value; \
+ }) \
+
+#include "vars.def"
+#undef VAR
+
+ // Restore watchdog
+ hw_restore_watchdog(wd_state);
+
+ return STAT_OK;
+}
+
+
+void vars_clear() {
+ eeprom_update_word(&eeprom_crc, -1);
+}
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2016 Buildbotics LLC
+ All rights reserved.
+
+ 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/>.
+
+ The software is distributed in the hope that it will be useful, but
+ WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public
+ License along with the software. If not, see
+ <http://www.gnu.org/licenses/>.
+
+ For information regarding this software email:
+ "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#define AXES_LABEL "xyzabcuvw"
+#define MOTORS_LABEL "0123"
+#define SWITCHES_LABEL "0123456789"
+
+// VAR(name, code, type, index, settable, save, help)
+
+// Motor
+VAR(axis_name, an, char, MOTORS, 1, 1, "Maps motor to axis name")
+VAR(step_angle, sa, float, MOTORS, 1, 1, "In degrees per full step")
+VAR(travel, tr, float, MOTORS, 1, 1, "Travel in mm per revolution")
+VAR(microstep, mi, uint16_t, MOTORS, 1, 1, "Microsteps per full step")
+VAR(reverse, rv, uint8_t, MOTORS, 1, 1, "Reverse motor polarity")
+
+VAR(power_mode, pm, uint8_t, MOTORS, 1, 1, "Motor power mode")
+VAR(drive_power, dp, float, MOTORS, 1, 1, "Motor drive power")
+VAR(idle_power, ip, float, MOTORS, 1, 1, "Motor idle power")
+VAR(current_power, cp, float, MOTORS, 0, 0, "Motor power now")
+VAR(status_flags, mf, uint8_t, MOTORS, 0, 0, "Motor status flags")
+VAR(status_strings, ms, flags_t, MOTORS, 0, 0, "Motor status strings")
+
+VAR(motor_fault, mf, bool, 0, 0, 0, "Motor fault status")
+
+VAR(velocity_max, vm, float, MOTORS, 1, 1, "Maxium velocity in mm/min")
+VAR(feedrate_max, fr, float, MOTORS, 1, 1, "Maxium feedrate in mm/min")
+VAR(jerk_max, jm, float, MOTORS, 1, 1, "Maxium jerk in mm/min^3")
+VAR(junction_dev, jd, float, MOTORS, 1, 1, "Junction deviation")
+VAR(radius, ra, float, MOTORS, 1, 1, "Axis radius or zero")
+
+// Homing
+VAR(homing_mode, hm, uint8_t, MOTORS, 1, 1, "Homing type")
+VAR(travel_min, tn, float, MOTORS, 1, 1, "Minimum soft limit")
+VAR(travel_max, tm, float, MOTORS, 1, 1, "Maximum soft limit")
+VAR(search_velocity,sv, float, MOTORS, 1, 1, "Homing search velocity")
+VAR(latch_velocity, lv, float, MOTORS, 1, 1, "Homing latch velocity")
+VAR(latch_backoff, lb, float, MOTORS, 1, 1, "Homing latch backof")
+VAR(zero_backoff, zb, float, MOTORS, 1, 1, "Homing zero backof")
+
+// Axis
+VAR(position, p, float, AXES, 0, 0, "Current axis position")
+
+// Spindle
+VAR(spindle_type, st, uint8_t, 0, 1, 1, "PWM=0 or HUANYANG=1")
+VAR(spin_reverse, sr, bool, 0, 1, 1, "Reverse spin")
+VAR(max_spin, sx, float, 0, 1, 1, "Maximum spindle speed")
+VAR(min_spin, sm, float, 0, 1, 1, "Minimum spindle speed")
+VAR(spin_min_pulse, np, float, 0, 1, 1, "Minimum pulse width")
+VAR(spin_max_pulse, mp, float, 0, 1, 1, "Maximum pulse width")
+VAR(spin_up, su, float, 0, 1, 1, "Spin up velocity")
+VAR(spin_down, sd, float, 0, 1, 1, "Spin down velocity")
+
+// Huanyang spindle
+VAR(huanyang_id, hi, uint8_t, 0, 1, 1, "Huanyang ID")
+VAR(huanyang_freq, hz, float, 0, 0, 0, "Huanyang actual freq")
+VAR(huanyang_current, hc, float, 0, 0, 0, "Huanyang actual current")
+VAR(huanyang_rpm, hr, uint16_t, 0, 0, 0, "Huanyang actual RPM")
+//VAR(huanyang_dcv, hd, uint16_t, 0, 0, 0, "Huanyang DC voltage")
+//VAR(huanyang_acv, ha, uint16_t, 0, 0, 0, "Huanyang AC voltage")
+VAR(huanyang_temp, ht, uint16_t, 0, 0, 0, "Huanyang temperature")
+VAR(huanyang_max_freq, hx, float, 0, 0, 0, "Huanyang max freq")
+VAR(huanyang_min_freq, hm, float, 0, 0, 0, "Huanyang min freq")
+VAR(huanyang_rated_rpm, hq, uint16_t, 0, 0, 0, "Huanyang rated RPM")
+VAR(huanyang_status, hs, uint8_t, 0, 0, 0, "Huanyang status flags")
+VAR(huanyang_debug, hb, bool, 0, 1, 0, "Huanyang debugging")
+VAR(huanyang_connected, he, bool, 0, 0, 0, "Huanyang connected")
+
+// Switches
+VAR(switch_type, sw, uint8_t, SWITCHES, 1, 1, "Normally open or closed")
+
+// GCode
+VAR(line, ln, int32_t, 0, 0, 0, "Last GCode line executed")
+VAR(unit, u, pstring, 0, 0, 0, "Current unit of measure")
+VAR(speed, s, float, 0, 0, 0, "Current spindle speed")
+VAR(feed, f, float, 0, 0, 0, "Current feed rate")
+VAR(tool, t, uint8_t, 0, 0, 0, "Current tool")
+VAR(feed_mode, fm, pstring, 0, 0, 0, "Current feed rate mode")
+VAR(plane, pa, pstring, 0, 0, 0, "Current plane")
+VAR(coord_system, cs, pstring, 0, 0, 0, "Current coordinate system")
+VAR(abs_override, ao, bool, 0, 0, 0, "Absolute override enabled")
+VAR(path_mode, pc, pstring, 0, 0, 0, "Current path control mode")
+VAR(distance_mode, dm, pstring, 0, 0, 0, "Current distance mode")
+VAR(arc_dist_mode, ad, pstring, 0, 0, 0, "Current arc distance mode")
+VAR(mist_coolant, mc, bool, 0, 0, 0, "Mist coolant enabled")
+VAR(flood_coolant, fc, bool, 0, 0, 0, "Flood coolant enabled")
+VAR(feed_override, fo, float, 0, 0, 0, "Feed rate override")
+VAR(speed_override, so, float, 0, 0, 0, "Spindle speed override")
+
+// System
+VAR(velocity, v, float, 0, 0, 0, "Current velocity")
+VAR(hw_id, id, string, 0, 0, 0, "Hardware ID")
+VAR(echo, ec, bool, 0, 1, 0, "Enable or disable echo")
+VAR(estop, es, bool, 0, 1, 0, "Emergency stop")
+VAR(estop_reason, er, pstring, 0, 0, 0, "Emergency stop reason")
+VAR(state, x, pstring, 0, 0, 0, "Machine state")
+VAR(cycle, c, pstring, 0, 0, 0, "Machine cycle")
+VAR(hold_reason, pr, pstring, 0, 0, 0, "Machine pause reason")
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2016 Buildbotics LLC
+ All rights reserved.
+
+ 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/>.
+
+ The software is distributed in the hope that it will be useful, but
+ WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public
+ License along with the software. If not, see
+ <http://www.gnu.org/licenses/>.
+
+ For information regarding this software email:
+ "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#pragma once
+
+#include "status.h"
+
+#include <stdbool.h>
+
+
+void vars_init();
+
+void vars_report(bool full);
+int vars_find(const char *name);
+bool vars_print(const char *name);
+bool vars_set(const char *name, const char *value);
+int vars_parser(char *vars);
+void vars_print_help();
+
+void vars_save();
+bool vars_valid();
+stat_t vars_restore();
+void vars_clear();
--- /dev/null
+/************************************************************************/
+/* XMEGA EEPROM Driver */
+/* */
+/* eeprom.c */
+/* */
+/* Alex Forencich <alex@alexforencich.com> */
+/* */
+/* Copyright (c) 2011 Alex Forencich */
+/* */
+/* Permission is hereby granted, free of charge, to any person */
+/* obtaining a copy of this software and associated documentation */
+/* files(the "Software"), to deal in the Software without restriction, */
+/* including without limitation the rights to use, copy, modify, merge, */
+/* publish, distribute, sublicense, and/or sell copies of the Software, */
+/* and to permit persons to whom the Software is furnished to do so, */
+/* subject to the following conditions: */
+/* */
+/* The above copyright notice and this permission notice shall be */
+/* included in all copies or substantial portions of the Software. */
+/* */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT 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 "eeprom_driver.h"
+
+
+static inline void NVM_EXEC(void) {
+ void *z = (void *)&NVM_CTRLA;
+
+ __asm__ volatile("out %[ccp], %[ioreg]" "\n\t"
+ "st z, %[cmdex]"
+ :
+ : [ccp] "I" (_SFR_IO_ADDR(CCP)),
+ [ioreg] "d" (CCP_IOREG_gc),
+ [cmdex] "r" (NVM_CMDEX_bm),
+ [z] "z" (z)
+ );
+}
+
+
+void wait_for_nvm() {
+ while (NVM.STATUS & NVM_NVMBUSY_bm) continue;
+}
+
+
+void EEPROM_erase_all() {
+ wait_for_nvm();
+ NVM.CMD = NVM_CMD_ERASE_EEPROM_gc;
+ NVM_EXEC();
+}
--- /dev/null
+/************************************************************************/
+/* XMEGA EEPROM Driver */
+/* */
+/* eeprom.h */
+/* */
+/* Alex Forencich <alex@alexforencich.com> */
+/* */
+/* Copyright (c) 2011 Alex Forencich */
+/* */
+/* Permission is hereby granted, free of charge, to any person */
+/* obtaining a copy of this software and associated documentation */
+/* files(the "Software"), to deal in the Software without restriction, */
+/* including without limitation the rights to use, copy, modify, merge, */
+/* publish, distribute, sublicense, and/or sell copies of the Software, */
+/* and to permit persons to whom the Software is furnished to do so, */
+/* subject to the following conditions: */
+/* */
+/* The above copyright notice and this permission notice shall be */
+/* included in all copies or substantial portions of the Software. */
+/* */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT 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. */
+/* */
+/************************************************************************/
+
+#pragma once
+
+#include <avr/io.h>
+#include <avr/interrupt.h>
+#include <avr/eeprom.h>
+
+#include "xboot.h"
+
+#ifndef EEPROM_PAGE_SIZE
+#define EEPROM_PAGE_SIZE E2PAGESIZE
+#endif
+
+#define EEPROM_read_byte(addr) \
+ eeprom_read_byte((const uint8_t *)((uint16_t)(addr)))
+
+#define EEPROM_write_byte(addr, value) \
+ eeprom_write_byte((uint8_t *)((uint16_t)(addr)), (value))
+
+#define EEPROM_read_block(addr, dest, len) \
+ eeprom_read_block((dest), (void *)((uint16_t)(addr)), (len))
+
+#define EEPROM_write_block(addr, src, len) \
+ eeprom_write_block((src), (void *)((uint16_t)(addr)), (len))
+
+void EEPROM_erase_all();
--- /dev/null
+/************************************************************************/
+/* XBoot Extensible AVR Bootloader */
+/* */
+/* XBoot Protocol Definition */
+/* */
+/* protocol.h */
+/* */
+/* Alex Forencich <alex@alexforencich.com> */
+/* */
+/* Copyright (c) 2010 Alex Forencich */
+/* */
+/* Permission is hereby granted, free of charge, to any person */
+/* obtaining a copy of this software and associated documentation */
+/* files(the "Software"), to deal in the Software without restriction, */
+/* including without limitation the rights to use, copy, modify, merge, */
+/* publish, distribute, sublicense, and/or sell copies of the Software, */
+/* and to permit persons to whom the Software is furnished to do so, */
+/* subject to the following conditions: */
+/* */
+/* The above copyright notice and this permission notice shall be */
+/* included in all copies or substantial portions of the Software. */
+/* */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT 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. */
+/* */
+/************************************************************************/
+
+#pragma once
+
+#include "xboot.h"
+
+// General Commands
+#define CMD_SYNC '\x1b'
+
+// Informational Commands
+#define CMD_CHECK_AUTOINCREMENT 'a'
+#define CMD_CHECK_BLOCK_SUPPORT 'b'
+#define CMD_PROGRAMMER_TYPE 'p'
+#define CMD_DEVICE_CODE 't'
+#define CMD_PROGRAM_ID 'S'
+#define CMD_VERSION 'V'
+#define CMD_READ_SIGNATURE 's'
+
+// Addressing
+#define CMD_SET_ADDRESS 'A'
+#define CMD_SET_EXT_ADDRESS 'H'
+
+// Erase
+#define CMD_CHIP_ERASE 'e'
+
+// Block Access
+#define CMD_BLOCK_LOAD 'B'
+#define CMD_BLOCK_READ 'g'
+
+// Byte Access
+#define CMD_READ_BYTE 'R'
+#define CMD_WRITE_LOW_BYTE 'c'
+#define CMD_WRITE_HIGH_BYTE 'C'
+#define CMD_WRITE_PAGE 'm'
+#define CMD_WRITE_EEPROM_BYTE 'D'
+#define CMD_READ_EEPROM_BYTE 'd'
+
+// Lock and Fuse Bits
+#define CMD_WRITE_LOCK_BITS 'l'
+#define CMD_READ_LOCK_BITS 'r'
+#define CMD_READ_LOW_FUSE_BITS 'F'
+#define CMD_READ_HIGH_FUSE_BITS 'N'
+#define CMD_READ_EXT_FUSE_BITS 'Q'
+
+// Bootloader Commands
+#define CMD_ENTER_PROG_MODE 'P'
+#define CMD_LEAVE_PROG_MODE 'L'
+#define CMD_EXIT_BOOTLOADER 'E'
+#define CMD_SET_LED 'x'
+#define CMD_CLEAR_LED 'y'
+#define CMD_SET_TYPE 'T'
+
+#define CMD_CRC 'h'
+
+// Memory types for block access
+#define MEM_EEPROM 'E'
+#define MEM_FLASH 'F'
+#define MEM_USERSIG 'U'
+#define MEM_PRODSIG 'P'
+
+// Sections for CRC checks
+#define SECTION_FLASH 'F'
+#define SECTION_APPLICATION 'A'
+#define SECTION_BOOT 'B'
+#define SECTION_APP 'a'
+#define SECTION_APP_TEMP 't'
+
+// Command Responses
+#define REPLY_ACK '\r'
+#define REPLY_YES 'Y'
+#define REPLY_ERROR '?'
--- /dev/null
+;******************************************************************************
+;*
+;* XMEGA Self-programming driver assembly source file.
+;*
+;* This file contains the low-level implementations for the
+;* XMEGA Self-programming driver. It is written for the GCC Assembler.
+;*
+;* If any SPM instructions are used, the linker file must define
+;* a segment named bootloader which must be located in the device Boot section.
+;* This can be done by passing "-Wl,--section-start=.BOOT=0x020000" to the
+;* linker with the correct address for the boot section.
+;*
+;* None of these routines clean up the NVM Command Register after use.
+;* It is therefore important to write NVM_CMD_NO_OPERATION_gc (0x00) to this
+;* register when you are finished using any of the functions in this driver.
+;*
+;* For all routines, it is important that any interrupt handlers do not
+;* perform any NVM operations. The user must implement a scheme for mutually
+;* exclusive access to the NVM. However, the 4-cycle timeout will work fine,
+;* since writing to the Configuration Change Protection register (CCP)
+;* automatically disables interrupts for 4 instruction cycles.
+;*
+;* Note on IAR calling convention:
+;* Scratch registers: R18-R27, R30-R31
+;* Preserved registers: R2-R17, R28-R29
+;* Parameter registers: R8-R25 (2-,4-, or 8- byte alignment)
+;* Return registers: R18-R25 (up to 64-bit)
+;*
+;* Application note:
+;* AVR1316: XMEGA Self-programming
+;*
+;* Documentation
+;* For comprehensive code documentation, supported compilers, compiler
+;* settings and supported devices see readme.html
+;*
+;* Atmel Corporation: http:;www.atmel.com \n
+;* Support email: avr@atmel.com
+;*
+;* $Revision: 1153 $
+;* $Date: 2007-12-18 09:48:23 +0100 (ti, 18 des 2007) $
+;*
+;* Copyright (c) 2007, Atmel Corporation All rights reserved.
+;*
+;* Redistribution and use in source and binary forms, with or without
+;* modification, are permitted provided that the following conditions are met:
+;*
+;* 1. Redistributions of source code must retain the above copyright notice,
+;* this list of conditions and the following disclaimer.
+;*
+;* 2. Redistributions in binary form must reproduce the above copyright notice,
+;* this list of conditions and the following disclaimer in the documentation
+;* and/or other materials provided with the distribution.
+;*
+;* 3. The name of ATMEL may not be used to endorse or promote products derived
+;* from this software without specific prior written permission.
+;*
+;* THIS SOFTWARE IS PROVIDED BY ATMEL "AS IS" AND ANY EXPRESS OR IMPLIED
+;* WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
+;* MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE EXPRESSLY AND
+;* SPECIFICALLY DISCLAIMED. IN NO EVENT SHALL ATMEL BE LIABLE FOR ANY DIRECT,
+;* INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+;* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+;* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+;* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+;* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
+;* THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+;******************************************************************************
+
+#include <avr/io.h>
+;#include "Flash_Defines.h"
+
+/* Define the size of the flash page if not defined in the header files. */
+#ifndef APP_SECTION_PAGE_SIZE
+ #error APP_SECTION_PAGE_SIZE must be defined if not defined in header files.
+ //#define APP_SECTION_PAGE_SIZE 512
+#endif /*APP_SECTION_PAGE_SIZE*/
+
+/* Defines not yet included in header file. */
+#define NVM_CMD_NO_OPERATION_gc (0x00<<0) // Noop/Ordinary LPM
+#define NVM_CMD_READ_USER_SIG_ROW_gc (0x01<<0) // Read user signature row
+#define NVM_CMD_READ_CALIB_ROW_gc (0x02<<0) // Read calibration row
+#define NVM_CMD_READ_EEPROM_gc (0x06<<0) // Read EEPROM
+#define NVM_CMD_READ_FUSES_gc (0x07<<0) // Read fuse byte
+#define NVM_CMD_WRITE_LOCK_BITS_gc (0x08<<0) // Write lock bits
+#define NVM_CMD_ERASE_USER_SIG_ROW_gc (0x18<<0) // Erase user signature row
+#define NVM_CMD_WRITE_USER_SIG_ROW_gc (0x1A<<0) // Write user signature row
+#define NVM_CMD_ERASE_APP_gc (0x20<<0) // Erase Application Section
+#define NVM_CMD_ERASE_APP_PAGE_gc (0x22<<0) // Erase Application Section page
+#define NVM_CMD_LOAD_FLASH_BUFFER_gc (0x23<<0) // Load Flash page buffer
+#define NVM_CMD_WRITE_APP_PAGE_gc (0x24<<0) // Write Application Section page
+#define NVM_CMD_ERASE_WRITE_APP_PAGE_gc (0x25<<0) // Erase-and-write Application Section page
+#define NVM_CMD_ERASE_FLASH_BUFFER_gc (0x26<<0) // Erase/flush Flash page buffer
+#define NVM_CMD_ERASE_BOOT_PAGE_gc (0x2A<<0) // Erase Boot Section page
+#define NVM_CMD_WRITE_BOOT_PAGE_gc (0x2C<<0) // Write Boot Section page
+#define NVM_CMD_ERASE_WRITE_BOOT_PAGE_gc (0x2D<<0) // Erase-and-write Boot Section page
+#define NVM_CMD_ERASE_EEPROM_gc (0x30<<0) // Erase EEPROM
+#define NVM_CMD_ERASE_EEPROM_PAGE_gc (0x32<<0) // Erase EEPROM page
+#define NVM_CMD_LOAD_EEPROM_BUFFER_gc (0x33<<0) // Load EEPROM page buffer
+#define NVM_CMD_WRITE_EEPROM_PAGE_gc (0x34<<0) // Write EEPROM page
+#define NVM_CMD_ERASE_WRITE_EEPROM_PAGE_gc (0x35<<0) // Erase-and-write EEPROM page
+#define NVM_CMD_ERASE_EEPROM_BUFFER_gc (0x36<<0) // Erase/flush EEPROM page buffer
+#define NVM_CMD_APP_CRC_gc (0x38<<0) // Generate Application section CRC
+#define NVM_CMD_BOOT_CRC_gc (0x39<<0) // Generate Boot Section CRC
+#define NVM_CMD_FLASH_RANGE_CRC_gc (0x3A<<0) // Generate Flash Range CRC
+#define CCP_SPM_gc (0x9D<<0) // SPM Instruction Protection
+#define CCP_IOREG_gc (0xD8<<0) // IO Register Protection
+
+
+
+; ---
+; This routine reads a byte from flash given by the address in
+; R25:R24:R23:R22.
+;
+; Input:
+; R25:R24:R23:R22.
+;
+; Returns:
+; R24 - Read byte.
+; ---
+
+.section .text
+.global SP_ReadByte
+
+SP_ReadByte:
+ in r19, RAMPZ ; Save RAMPZ.
+ out RAMPZ, r24 ; Load RAMPZ with the MSB of the address.
+ movw ZL, r22 ; Move the low bytes to the Z pointer
+ elpm r24, Z ; Extended load byte from address pointed to by Z.
+ out RAMPZ, r19 ; Restore RAMPZ register.
+ ret
+
+
+
+; ---
+; This routine reads a word from flash given by the address in
+; R25:R24:R23:R22.
+;
+; Input:
+; R25:R24:R23:R22.
+;
+; Returns:
+; R25:R24 - Read word.
+; ---
+
+.section .text
+.global SP_ReadWord
+
+SP_ReadWord:
+ in r19, RAMPZ ; Save RAMPZ.
+ out RAMPZ, r24 ; Load RAMPZ with the MSB of the address.
+ movw ZL, r22 ; Move the low bytes to the Z pointer
+ elpm r24, Z+ ; Extended load byte from address pointed to by Z.
+ elpm r25, Z ; Extended load byte from address pointed to by Z.
+ out RAMPZ, r19 ; Restore RAMPZ register.
+ ret
+
+
+
+; ---
+; This routine reads the calibration byte given by the index in R24.
+;
+; Input:
+; R24 - Byte index.
+;
+; Returns:
+; R24 - Calibration byte.
+; ---
+
+.section .text
+.global SP_ReadCalibrationByte
+
+SP_ReadCalibrationByte:
+ ldi r20, NVM_CMD_READ_CALIB_ROW_gc ; Prepare NVM command in R20.
+ rjmp SP_CommonLPM ; Jump to common LPM code.
+
+
+
+; ---
+; This routine reads the user signature byte given by the index in R25:R24.
+;
+; Input:
+; R25:R24 - Byte index.
+;
+; Returns:
+; R24 - Signature byte.
+; ---
+
+.section .text
+.global SP_ReadUserSignatureByte
+
+SP_ReadUserSignatureByte:
+ ldi r20, NVM_CMD_READ_USER_SIG_ROW_gc ; Prepare NVM command in R20.
+ rjmp SP_CommonLPM ; Jump to common LPM code.
+
+
+
+; ---
+; This routine reads the fuse byte given by the index in R24.
+;
+; Input:
+; R24 - Byte index.
+;
+; Returns:
+; R24 - Fuse byte.
+; ---
+
+.section .text
+.global SP_ReadFuseByte
+
+SP_ReadFuseByte:
+ sts NVM_ADDR0, r24 ; Load fuse byte index into NVM Address Register 0.
+ clr r24 ; Prepare a zero.
+ sts NVM_ADDR1, r24 ; Load zero into NVM Address Register 1.
+ sts NVM_ADDR2, r24 ; Load zero into NVM Address Register 2.
+ ldi r20, NVM_CMD_READ_FUSES_gc ; Prepare NVM command in R20.
+ rcall SP_CommonCMD ; Jump to common NVM Action code.
+ movw r24, r22 ; Move low byte to 1 byte return address.
+ ret
+
+
+
+; ---
+; This routine sets the lock bits from R24. Note that unlocking is only
+; possible by doing a full chip erase, not available from software.
+;
+; Input:
+; R24 - Lock bits.
+;
+; Returns:
+; Nothing.
+; ---
+
+.section .text
+.global SP_WriteLockBits
+
+SP_WriteLockBits:
+ sts NVM_DATA0, r24 ; Load lock bits into NVM Data Register 0.
+ ldi r20, NVM_CMD_WRITE_LOCK_BITS_gc ; Prepare NVM command in R20.
+ rjmp SP_CommonCMD ; Jump to common NVM Action code.
+
+
+
+; ---
+; This routine reads the lock bits.
+;
+; Input:
+; Nothing.
+;
+; Returns:
+; R24 - Lock bits.
+; ---
+
+.section .text
+.global SP_ReadLockBits
+
+SP_ReadLockBits:
+ lds r24, NVM_LOCKBITS ; Read IO-mapped lock bits.
+ ret
+
+
+
+; ---
+; This routine erases the user signature row.
+;
+; Input:
+; Nothing.
+;
+; Returns:
+; Nothing.
+; ---
+
+.section .text
+.global SP_EraseUserSignatureRow
+
+SP_EraseUserSignatureRow:
+ in r19, RAMPZ ; Save RAMPZ, which is restored in SP_CommonSPM.
+ ldi r20, NVM_CMD_ERASE_USER_SIG_ROW_gc ; Prepare NVM command in R20.
+ jmp SP_CommonSPM ; Jump to common SPM code.
+
+
+
+; ---
+; This routine writes the flash buffer to the user signature row.
+;
+; Input:
+; Nothing.
+;
+; Returns:
+; Nothing.
+; ---
+
+.section .text
+.global SP_WriteUserSignatureRow
+
+SP_WriteUserSignatureRow:
+ in r19, RAMPZ ; Save RAMPZ, which is restored in SP_CommonSPM.
+ ldi r20, NVM_CMD_WRITE_USER_SIG_ROW_gc ; Prepare NVM command in R20.
+ jmp SP_CommonSPM ; Jump to common SPM code.
+
+
+
+; ---
+; This routine erases the entire application section.
+;
+; Input:
+; Nothing.
+;
+; Returns:
+; Nothing.
+; ---
+
+.section .text
+.global SP_EraseApplicationSection
+
+SP_EraseApplicationSection:
+ in r19, RAMPZ ; Save RAMPZ, which is restored in SP_CommonSPM.
+ clr r24 ; Prepare a zero.
+ clr r25
+ out RAMPZ, r24 ; Point into Application area.
+ ldi r20, NVM_CMD_ERASE_APP_gc ; Prepare NVM command in R20.
+ jmp SP_CommonSPM ; Jump to common SPM code.
+
+
+
+; ---
+; This routine erases the page at address R25:R24:R23:R22 in the application
+; section. The address can point anywhere inside the page.
+;
+; Input:
+; R25:R24:R23:R22 - Byte address into Flash page.
+;
+; Returns:
+; Nothing.
+; ---
+
+.section .text
+.global SP_EraseApplicationPage
+
+SP_EraseApplicationPage:
+ in r19, RAMPZ ; Save RAMPZ, which is restored in SP_CommonSPM.
+ out RAMPZ, r24 ; Load RAMPZ with the MSB of the address.
+ movw r24, r22 ; Move low bytes for ZH:ZL to R25:R24
+ ldi r20, NVM_CMD_ERASE_APP_PAGE_gc ; Prepare NVM command in R20.
+ jmp SP_CommonSPM ; Jump to common SPM code.
+
+
+
+; ---
+; This routine writes the word from R23:R22 into the Flash page buffer at
+; address R25:R24.
+;
+; Input:
+; R25:R24 - Byte address into Flash page.
+; R23:R22 - Word to write.
+;
+; Returns:
+; Nothing.
+; ---
+
+.section .text
+.global SP_LoadFlashWord
+
+SP_LoadFlashWord:
+ in r19, RAMPZ ; Save RAMPZ, which is restored in SP_CommonSPM.
+ movw r0, r22 ; Prepare flash word in R1:R0.
+ ldi r20, NVM_CMD_LOAD_FLASH_BUFFER_gc ; Prepare NVM command in R20.
+ jmp SP_CommonSPM ; Jump to common SPM code.
+
+
+
+; ---
+; This routine writes an entire page from the SRAM buffer at
+; address R25:R24 into the Flash page buffer.
+;
+; Note that you must define "-Wl,--section-start=.BOOT=0x020000" for the
+; linker to place this function in the boot section with the correct address.
+;
+; Input:
+; R25:R24 - 16-bit pointer to SRAM buffer.
+;
+; Returns:
+; Nothing.
+; ---
+
+.section .text
+.global SP_LoadFlashPage
+
+SP_LoadFlashPage:
+ clr ZL ; Clear low byte of Z, to indicate start of page.
+ clr ZH ; Clear high byte of Z, to indicate start of page.
+
+ out RAMPX, r1 ; Clear RAMPX pointer.
+ movw XL, r24 ; Load X with data buffer address.
+
+ ldi r20, NVM_CMD_LOAD_FLASH_BUFFER_gc ; Prepare NVM command code in R20.
+ sts NVM_CMD, r20 ; Load it into NVM command register.
+
+#if APP_SECTION_PAGE_SIZE > 512
+ ldi r22, ((APP_SECTION_PAGE_SIZE/2) >> 8)
+#endif
+
+ ldi r21, ((APP_SECTION_PAGE_SIZE/2)&0xFF) ; Load R21 with page word count.
+ ldi r18, CCP_SPM_gc ; Prepare Protect SPM signature in R16.
+
+SP_LoadFlashPage_1:
+ ld r0, X+ ; Load low byte from buffer into R0.
+ ld r1, X+ ; Load high byte from buffer into R1.
+ sts CCP, r18 ; Enable SPM operation (this disables interrupts for 4 cycles).
+ spm ; Self-program.
+ adiw ZL, 2 ; Move Z to next Flash word.
+
+#if APP_SECTION_PAGE_SIZE > 512
+ subi r21, 1 ; Decrement word count.
+ sbci r22, 0
+#else
+ dec r21 ; Decrement word count.
+#endif
+
+ brne SP_LoadFlashPage_1 ; Repeat until word cont is zero.
+
+ clr r1 ; Clear R1 for GCC _zero_reg_ to function properly.
+ ret
+
+
+
+; ---
+; This routine reads an entire Flash page from address R23:R22:R21:R20 into the
+; SRAM buffer at address R25:R24.
+;
+;
+; Input:
+; R23:R22:R21:R20 - Flash byte address.
+; R25:R24 - 16-bit pointer to SRAM buffer.
+;
+; Returns:
+; Nothing.
+; ---
+
+.section .text
+.global SP_ReadFlashPage
+
+SP_ReadFlashPage:
+
+ in r19, RAMPZ ; Save RAMPZ during assembly.
+ out RAMPZ, r22 ; Load RAMPZ with MSB of address
+ movw ZL, r20 ; Load Z with Flash address.
+
+ out RAMPX, r1 ; Load RAMPX with data pointer
+ movw XL, r24 ; Load X with data buffer address.
+
+ ldi r20, NVM_CMD_NO_OPERATION_gc ; Prepare NVM command code in R20.
+ sts NVM_CMD, r20 ; Set NVM command to No Operation so that LPM reads Flash.
+
+#if APP_SECTION_PAGE_SIZE > 512
+ ldi r22, ((APP_SECTION_PAGE_SIZE/2) >> 8) ; Load R22 with byte cont high if flash page is large.
+#endif
+
+ ldi r21, ((APP_SECTION_PAGE_SIZE)&0xFF) ; Load R21 with byte count.
+
+SP_ReadFlashPage_1:
+ elpm r24, Z+ ; Load Flash bytes into R18:r19
+ elpm r25, Z+
+ st X+, r24 ; Write bytes to buffer.
+ st X+, r25
+
+#if APP_SECTION_PAGE_SIZE > 512
+ subi r21, 1 ; Decrement word count.
+ sbci r22, 0
+#else
+ dec r21 ; Decrement word count.
+#endif
+
+ brne SP_ReadFlashPage_1 ; Repeat until byte count is zero.
+
+ out RAMPZ, r19
+ ret
+
+
+
+; ---
+; This routine writes the page buffer to the Flash page at address R25:R24:R23:R22
+; in the application section. The address can point anywhere inside the page.
+;
+; Input:
+; R25:R24:R23:R22 - Byte address into Flash page.
+;
+; Returns:
+; Nothing.
+; ---
+
+.section .text
+.global SP_WriteApplicationPage
+
+SP_WriteApplicationPage:
+ in r19, RAMPZ ; Save RAMPZ, which is restored in SP_CommonSPM.
+ out RAMPZ, r24 ; Load RAMPZ with the MSB of the address.
+ movw r24, r22 ; Move low bytes of address to ZH:ZL from R23:R22
+ ldi r20, NVM_CMD_WRITE_APP_PAGE_gc ; Prepare NVM command in R20.
+ jmp SP_CommonSPM ; Jump to common SPM code.
+
+
+
+; ---
+; This routine erases first and then writes the page buffer to the
+; Flash page at address R25:R24:R23:R22 in the application section. The address
+; can point anywhere inside the page.
+;
+; Input:
+; R25:R24:R23:R22 - Byte address into Flash page.
+;
+; Returns:
+; Nothing.
+; ---
+
+.section .text
+.global SP_EraseWriteApplicationPage
+
+SP_EraseWriteApplicationPage:
+ in r19, RAMPZ ; Save RAMPZ, which is restored in SP_CommonSPM.
+ out RAMPZ, r24 ; Load RAMPZ with the MSB of the address.
+ movw r24, r22 ; Move low bytes of address to ZH:ZL from R23:R22
+ ldi r20, NVM_CMD_ERASE_WRITE_APP_PAGE_gc ; Prepare NVM command in R20.
+ jmp SP_CommonSPM ; Jump to common SPM code.
+
+
+
+; ---
+; This routine flushes the Flash page buffer.
+;
+; Input:
+; Nothing.
+;
+; Returns:
+; Nothing.
+; ---
+
+.section .text
+.global SP_EraseFlashBuffer
+
+SP_EraseFlashBuffer:
+ in r19, RAMPZ ; Save RAMPZ, which is restored in SP_CommonSPM.
+ ldi r20, NVM_CMD_ERASE_FLASH_BUFFER_gc ; Prepare NVM command in R20.
+ jmp SP_CommonSPM ; Jump to common SPM code.
+
+
+
+; ---
+; This routine erases the page at address R25:R24:R23:R22 in the Boot section. The
+; address can point anywhere inside the page.
+;
+; Input:
+; R25:R24:R23:R22 - Byte address into Flash page.
+;
+; Returns:
+; Nothing.
+; ---
+
+.section .text
+.global SP_EraseBootPage
+
+SP_EraseBootPage:
+ in r19, RAMPZ ; Save RAMPZ, which is restored in SP_CommonSPM.
+ out RAMPZ, r24 ; Load RAMPZ with the MSB of the address.
+ movw r24, r22 ; Move low bytes of address to ZH:ZL from R23:R22
+ ldi r20, NVM_CMD_ERASE_BOOT_PAGE_gc ; Prepare NVM command in R20.
+ jmp SP_CommonSPM ; Jump to common SPM code.
+
+
+
+; ---
+; This routine writes the page buffer to the Flash page at address R25:R24:R23:R22
+; in the BOOT section. The address can point anywhere inside the page.
+;
+; Input:
+; R25:R24:R23:R22 - Byte address into Flash page.
+;
+; Returns:
+; Nothing.
+; ---
+
+.section .text
+.global SP_WriteBootPage
+
+SP_WriteBootPage:
+ in r19, RAMPZ ; Save RAMPZ, which is restored in SP_CommonSPM.
+ out RAMPZ, r24 ; Load RAMPZ with the MSB of the address.
+ movw r24, r22 ; Move low bytes of address to ZH:ZL from R23:R22
+ ldi r20, NVM_CMD_WRITE_BOOT_PAGE_gc ; Prepare NVM command in R20.
+ jmp SP_CommonSPM ; Jump to common SPM code.
+
+
+
+; ---
+; This routine erases first and then writes the page buffer to the
+; Flash page at address R25:R24:R23:R22 in the Boot section. The address
+; can point anywhere inside the page.
+;
+; Input:
+; R25:R24:R23:R22 - Byte address into Flash page.
+;
+; Returns:
+; Nothing.
+; ---
+
+.section .text
+.global SP_EraseWriteBootPage
+
+SP_EraseWriteBootPage:
+ in r19, RAMPZ ; Save RAMPZ, which is restored in SP_CommonSPM.
+ out RAMPZ, r24 ; Load RAMPZ with the MSB of the address.
+ movw r24, r22 ; Move low bytes of address to ZH:ZL from R23:R22
+ ldi r20, NVM_CMD_ERASE_WRITE_BOOT_PAGE_gc ; Prepare NVM command in R20.
+ jmp SP_CommonSPM ; Jump to common SPM code.
+
+
+
+; ---
+; This routine calculates a CRC for the application section.
+;
+; Input:
+; Nothing.
+;
+; Returns:
+; R25:R24:R23:R22 - 32-bit CRC result (actually only 24-bit used).
+; ---
+
+.section .text
+.global SP_ApplicationCRC
+
+SP_ApplicationCRC:
+ ldi r20, NVM_CMD_APP_CRC_gc ; Prepare NVM command in R20.
+ rjmp SP_CommonCMD ; Jump to common NVM Action code.
+
+
+
+; ---
+; This routine calculates a CRC for the Boot section.
+;
+; Input:
+; Nothing.
+;
+; Returns:
+; R25:R24:R23:R22 - 32-bit CRC result (actually only 24-bit used).
+; ---
+
+.section .text
+.global SP_BootCRC
+
+SP_BootCRC:
+ ldi r20, NVM_CMD_BOOT_CRC_gc ; Prepare NVM command in R20.
+ rjmp SP_CommonCMD ; Jump to common NVM Action code.
+
+
+
+; ---
+; This routine locks all further access to SPM operations until next reset.
+;
+; Input:
+; Nothing.
+;
+; Returns:
+; Nothing.
+; ---
+
+.section .text
+.global SP_LockSPM
+
+SP_LockSPM:
+ ldi r18, CCP_IOREG_gc ; Prepare Protect IO-register signature in R18.
+ sts CCP, r18 ; Enable IO-register operation (this disables interrupts for 4 cycles).
+ ldi r18, NVM_SPMLOCK_bm ; Prepare bitmask for locking SPM into R18.
+ sts NVM_CTRLB, r18 ; Load bitmask into NVM Control Register B, which locks SPM.
+ ret
+
+
+
+; ---
+; This routine wait for the SPM to finish and clears the command register.
+;
+; Note that this routine is blocking, and will halt any execution until the SPM
+; is finished.
+;
+; Input:
+; Nothing.
+;
+; Returns:
+; Nothing.
+; ---
+
+.section .text
+.global SP_WaitForSPM
+
+SP_WaitForSPM:
+ lds r18, NVM_STATUS ; Load the NVM Status register.
+ sbrc r18, NVM_NVMBUSY_bp ; Check if bit is cleared.
+ rjmp SP_WaitForSPM ; Repeat check if bit is not cleared.
+ clr r18
+ sts NVM_CMD, r18 ; Clear up command register to NO_OPERATION.
+ ret
+
+
+
+; ---
+; This routine is called by several other routines, and contains common code
+; for executing an NVM command, including the return statement itself.
+;
+; If the operation (NVM command) requires the NVM Address registers to be
+; prepared, this must be done before jumping to this routine.
+;
+; Note that R25:R24:R23:R22 is used for returning results, even if the
+; C-domain calling function only expects a single byte or even void.
+;
+; Input:
+; R20 - NVM Command code.
+;
+; Returns:
+; R25:R24:R23:R22 - 32-bit result from NVM operation.
+; ---
+
+.section .text
+
+SP_CommonCMD:
+ sts NVM_CMD, r20 ; Load command into NVM Command register.
+ ldi r18, CCP_IOREG_gc ; Prepare Protect IO-register signature in R18.
+ ldi r19, NVM_CMDEX_bm ; Prepare bitmask for setting NVM Command Execute bit into R19.
+ sts CCP, r18 ; Enable IO-register operation (this disables interrupts for 4 cycles).
+ sts NVM_CTRLA, r19 ; Load bitmask into NVM Control Register A, which executes the command.
+ lds r22, NVM_DATA0 ; Load NVM Data Register 0 into R22.
+ lds r23, NVM_DATA1 ; Load NVM Data Register 1 into R23.
+ lds r24, NVM_DATA2 ; Load NVM Data Register 2 into R24.
+ clr r25 ; Clear R25 in order to return a clean 32-bit value.
+ ret
+
+
+
+; ---
+; This routine is called by several other routines, and contains common code
+; for executing an LPM command, including the return statement itself.
+;
+; Note that R24 is used for returning results, even if the
+; C-domain calling function expects a void.
+;
+; Input:
+; R25:R24 - Low bytes of Z pointer.
+; R20 - NVM Command code.
+;
+; Returns:
+; R24 - Result from LPM operation.
+; ---
+
+.section .text
+
+SP_CommonLPM:
+ movw ZL, r24 ; Load index into Z.
+ sts NVM_CMD, r20 ; Load prepared command into NVM Command register.
+ lpm r24,Z
+ ret
+
+
+
+; ---
+; This routine is called by several other routines, and contains common code
+; for executing an SPM command, including the return statement itself.
+;
+; If the operation (SPM command) requires the R1:R0 registers to be
+; prepared, this must be done before jumping to this routine.
+;
+; Note that you must define "-Wl,--section-start=.BOOT=0x020000" for the
+; linker to place this function in the boot section with the correct address.
+;
+; Input:
+; R1:R0 - Optional input to SPM command.
+; R25:R24 - Low bytes of Z pointer.
+; R20 - NVM Command code.
+;
+; Returns:
+; Nothing.
+; ---
+
+.section .text
+
+SP_CommonSPM:
+ movw ZL, r24 ; Load R25:R24 into Z.
+ sts NVM_CMD, r20 ; Load prepared command into NVM Command register.
+ ldi r18, CCP_SPM_gc ; Prepare Protect SPM signature in R18
+ sts CCP, r18 ; Enable SPM operation (this disables interrupts for 4 cycles).
+ spm ; Self-program.
+ clr r1 ; Clear R1 for GCC _zero_reg_ to function properly.
+ out RAMPZ, r19 ; Restore RAMPZ register.
+ ret
+
+
+; END OF FILE
+
--- /dev/null
+/* This file has been prepared for Doxygen automatic documentation generation.*/
+/*! \file *********************************************************************
+ *
+ * \brief XMEGA Self-programming driver header file.
+ *
+ * This file contains the function prototypes for the
+ * XMEGA Self-programming driver.
+ * If any SPM instructions are used, the linker file must define
+ * a segment named BOOT which must be located in the device boot section.
+ *
+ *
+ * None of these functions clean up the NVM Command Register after use.
+ * It is therefore important to write NVMCMD_NO_OPERATION (0x00) to this
+ * register when you are finished using any of the functions in this
+ * driver.
+ *
+ * For all functions, it is important that no interrupt handlers perform
+ * any NVM operations. The user must implement a scheme for mutually
+ * exclusive access to the NVM. However, the 4-cycle timeout will work
+ * fine, since writing to the Configuration Change Protection register
+ * (CCP) automatically disables interrupts for 4 instruction cycles.
+ *
+ * \par Application note:
+ * AVR1316: XMEGA Self-programming
+ *
+ * \par Documentation
+ * For comprehensive code documentation, supported compilers, compiler
+ * settings and supported devices see readme.html
+ *
+ * \author
+ * Atmel Corporation: http://www.atmel.com \n
+ * Support email: avr@atmel.com
+ *
+ * $Revision: 1691 $
+ * $Date: 2008-07-29 13:25:40 +0200 (ti, 29 jul 2008) $ \n
+ *
+ * Copyright (c) 2008, Atmel Corporation All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * 3. The name of ATMEL may not be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY ATMEL "AS IS" AND ANY EXPRESS OR IMPLIED
+ * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE EXPRESSLY AND
+ * SPECIFICALLY DISCLAIMED. IN NO EVENT SHALL ATMEL BE LIABLE FOR ANY DIRECT,
+ * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
+ * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *****************************************************************************/
+#ifndef SP_DRIVER_H
+#define SP_DRIVER_H
+
+//#include "avr_compiler.h"
+//#include "Flash_Defines.h"
+
+
+
+/* Define the size of the flash page if not defined in the header files. */
+#ifndef APP_SECTION_PAGE_SIZE
+#error APP_SECTION_PAGE_SIZE must be defined if not defined in header files.
+//#define APP_SECTION_PAGE_SIZE 512
+#endif /*FLASH_PAGE_SIZE*/
+
+// Define the Start of the application table if not defined in the header files.
+#ifndef APPTABLE_SECTION_START
+#error APPTABLE_SECTION_START must be defined if not defined in header files.
+//#define APPTABLE_SECTION_START 0x01E000 //APPTABLE address for ATxmega128A1
+#endif /*APPTABLE_SECTION_START*/
+
+/*! \brief Read a byte from flash.
+ *
+ * This function reads one byte from the flash.
+ *
+ * \note Both IAR and GCC have functions to do this, but
+ * we include the fucntions for easier use.
+ *
+ * \param address Address to the location of the byte to read.
+ *
+ * \retval Byte read from flash.
+ */
+uint8_t SP_ReadByte(uint32_t address);
+
+/*! \brief Read a word from flash.
+ *
+ * This function reads one word from the flash.
+ *
+ * \note Both IAR and GCC have functions to do this automatically, but
+ * we include the fucntions for easier use.
+ *
+ * \param address Address to the location of the word to read.
+ *
+ * \retval word read from flash.
+ */
+uint16_t SP_ReadWord(uint32_t address);
+
+/*! \brief Read calibration byte at given index.
+ *
+ * This function reads one calibration byte from the Calibration signature row.
+ *
+ * \param index Index of the byte in the calibration signature row.
+ *
+ * \retval Calibration byte
+ */
+uint8_t SP_ReadCalibrationByte(uint8_t index);
+
+/*! \brief Read fuse byte from given index.
+ *
+ * This function reads the fuse byte at the given index.
+ *
+ * \param index Index of the fuse byte.
+ *
+ * \retval Fuse byte
+ */
+uint8_t SP_ReadFuseByte(uint8_t index);
+
+/*! \brief Write lock bits.
+ *
+ * This function changes the lock bits.
+ *
+ * \note It is only possible to change the lock bits to a higher security l
+ * evel.
+ *
+ * \param data The new value of the lock bits.
+ */
+void SP_WriteLockBits(uint8_t data);
+
+/*! \brief Read lock bits.
+ *
+ * This function reads the lock bits.
+ *
+ * \retval Lock bits
+ */
+uint8_t SP_ReadLockBits(void);
+
+/*! \brief Read user signature at given index.
+ *
+ * This function reads one byte from the user signature row.
+ *
+ * \param index Index of the byte in the user signature row.
+ *
+ * \retval User signature byte
+ */
+uint8_t SP_ReadUserSignatureByte(uint16_t index);
+
+/*! \brief Erase user signature row.
+ *
+ * This function erase the entire user signature row.
+ */
+void SP_EraseUserSignatureRow(void);
+
+/*! \brief Write user signature row.
+ *
+ * This function write the flash buffer in the user signature row.
+ */
+void SP_WriteUserSignatureRow(void);
+
+/*! \brief Erase entire application section.
+ *
+ * This function erases the entire application and application table section
+ *
+ * \note If the lock bits is set to not allow spm in the application or
+ * application table section the erase is not done.
+ */
+void SP_EraseApplicationSection(void);
+
+/*! \brief Erase page at byte address in application or application table
+ * section.
+ *
+ * This function erase one page given by the byte address.
+ *
+ * \param address Byte address for flash page.
+ */
+void SP_EraseApplicationPage(uint32_t address);
+
+/*! \brief Erase and write page buffer to application or application table
+ * section at byte address.
+ *
+ * This function does a combined erase and write to a flash page in the
+ * application or application table section.
+ *
+ * \param address Byte address for flash page.
+ */
+void SP_EraseWriteApplicationPage(uint32_t address);
+
+/*! \brief Write page buffer to application or application table section at
+ * byte address.
+ *
+ * This function writes the Flash page buffer to a page in the application or
+ * application table section given by the byte address.
+ *
+ * \note The page that is written to must be erased before it is written to.
+ *
+ * \param address Byte address for flash page.
+ */
+void SP_WriteApplicationPage(uint32_t address);
+
+/*! \brief Load one word into Flash page buffer.
+ *
+ * This function Loads one word into the Flash page buffer.
+ *
+ * \param address Position in inside the flash page buffer.
+ * \param data Value to be put into the buffer.
+ */
+void SP_LoadFlashWord(uint16_t address, uint16_t data);
+
+/*! \brief Load entire page from SRAM buffer into Flash page buffer.
+ *
+ * This function load an entire page from SRAM.
+ *
+ * \param data Pointer to the data to put in buffer.
+ *
+ * \note The __near keyword limits the pointer to two bytes which means that
+ * only data up to 64K (internal SRAM) can be used.
+ */
+void SP_LoadFlashPage(const uint8_t * data);
+
+/*! \brief Read entire Flash page into SRAM buffer.
+ *
+ * This function reads an entire flash page and puts it to SRAM.
+ *
+ * \param data Pointer to where to store the data.
+ * \param address Address to page to read from flash.
+ */
+void SP_ReadFlashPage(const uint8_t * data, uint32_t address);
+
+/*! \brief Flush Flash page buffer.
+ *
+ * This function flush the Flash page buffer.
+ */
+void SP_EraseFlashBuffer(void);
+
+/*! \brief Erase page at byte address in boot section.
+ *
+ * This function erase one page given by the byte address.
+ *
+ * \param address Byte address for flash page.
+ */
+void SP_EraseBootPage(uint32_t address);
+
+/*! \brief Erase and write page buffer to boot section at byte address.
+ *
+ * This function does a combined erase and write to a flash page in the boot
+ * section.
+ *
+ * \param address Byte address for flash page.
+ */
+void SP_EraseWriteBootPage(uint32_t address);
+
+/*! \brief Write page buffer to boot section at byte address.
+ *
+ * This function writes the Flash page buffer to a page in the boot section
+ * given by the byte address.
+ *
+ * \note The page that is written to must be erased before it is written to.
+ *
+ * \param address Byte address for flash page.
+ */
+void SP_WriteBootPage(uint32_t address);
+
+/*! \brief Generate CRC from application section.
+ *
+ * \retval 24-bit CRC value
+ */
+uint32_t SP_ApplicationCRC(void);
+
+/*! \brief Generate CRC from boot section.
+ *
+ * \retval 24-bit CRC value
+ */
+uint32_t SP_BootCRC(void);
+
+/*! \brief Lock SPM instruction.
+ *
+ * This function locks the SPM instruction, and will disable the use of
+ * SPM until the next reset occurs.
+ */
+void SP_LockSPM(void);
+
+/*! \brief Wait for SPM to finish.
+ *
+ * This routine waits for the SPM to finish and clears the command register.
+ */
+void SP_WaitForSPM(void);
+
+#endif /* SP_DRIVER_H */
--- /dev/null
+/************************************************************************/
+/* XBoot Extensible AVR Bootloader */
+/* */
+/* UART Module */
+/* */
+/* uart.c */
+/* */
+/* Alex Forencich <alex@alexforencich.com> */
+/* */
+/* Copyright (c) 2010 Alex Forencich */
+/* */
+/* Permission is hereby granted, free of charge, to any person */
+/* obtaining a copy of this software and associated documentation */
+/* files(the "Software"), to deal in the Software without restriction, */
+/* including without limitation the rights to use, copy, modify, merge, */
+/* publish, distribute, sublicense, and/or sell copies of the Software, */
+/* and to permit persons to whom the Software is furnished to do so, */
+/* subject to the following conditions: */
+/* */
+/* The above copyright notice and this permission notice shall be */
+/* included in all copies or substantial portions of the Software. */
+/* */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT 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 "uart.h"
+#include "xboot.h"
+
+
+bool uart_has_char() {return UART_DEVICE.STATUS & USART_RXCIF_bm;}
+uint8_t uart_recv_char() {return UART_DEVICE.DATA;}
+
+
+void uart_send_char_blocking(uint8_t c) {
+ UART_DEVICE.DATA = c;
+ while (!(UART_DEVICE.STATUS & USART_TXCIF_bm)) continue;
+ UART_DEVICE.STATUS |= USART_TXCIF_bm;
+}
+
+
+void uart_init() {
+ UART_PORT.DIRSET = 1 << UART_TX_PIN;
+ UART_DEVICE.BAUDCTRLA = UART_BSEL_VALUE & USART_BSEL_gm;
+ UART_DEVICE.BAUDCTRLB =
+ ((UART_BSCALE_VALUE << USART_BSCALE_gp) & USART_BSCALE_gm) |
+ ((UART_BSEL_VALUE >> 8) & ~USART_BSCALE_gm);
+
+#if UART_CLK2X
+ UART_DEVICE.CTRLB = USART_RXEN_bm | USART_CLK2X_bm | USART_TXEN_bm;
+#else
+ UART_DEVICE.CTRLB = USART_RXEN_bm | USART_TXEN_bm;
+#endif // UART_CLK2X
+
+ PORTC.OUTCLR = 1 << 4; // CTS Lo (enable)
+ PORTC.DIRSET = 1 << 4; // CTS Output
+}
+
+
+void uart_deinit() {
+ UART_DEVICE.CTRLB = 0;
+ UART_DEVICE.BAUDCTRLA = 0;
+ UART_DEVICE.BAUDCTRLB = 0;
+ UART_PORT.DIRCLR = 1 << UART_TX_PIN;
+}
--- /dev/null
+/************************************************************************/
+/* XBoot Extensible AVR Bootloader */
+/* */
+/* UART Module */
+/* */
+/* uart.h */
+/* */
+/* Alex Forencich <alex@alexforencich.com> */
+/* */
+/* Copyright (c) 2010 Alex Forencich */
+/* */
+/* Permission is hereby granted, free of charge, to any person */
+/* obtaining a copy of this software and associated documentation */
+/* files(the "Software"), to deal in the Software without restriction, */
+/* including without limitation the rights to use, copy, modify, merge, */
+/* publish, distribute, sublicense, and/or sell copies of the Software, */
+/* and to permit persons to whom the Software is furnished to do so, */
+/* subject to the following conditions: */
+/* */
+/* The above copyright notice and this permission notice shall be */
+/* included in all copies or substantial portions of the Software. */
+/* */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT 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. */
+/* */
+/************************************************************************/
+
+#pragma once
+
+#include <stdbool.h>
+#include <stdint.h>
+
+bool uart_has_char();
+uint8_t uart_recv_char();
+void uart_send_char_blocking(uint8_t c);
+void uart_init(void);
+void uart_deinit(void);
--- /dev/null
+/************************************************************************/
+/* XBoot Extensible AVR Bootloader */
+/* */
+/* Watchdog Module */
+/* */
+/* watchdog.c */
+/* */
+/* Alex Forencich <alex@alexforencich.com> */
+/* */
+/* Copyright (c) 2010 Alex Forencich */
+/* */
+/* Permission is hereby granted, free of charge, to any person */
+/* obtaining a copy of this software and associated documentation */
+/* files(the "Software"), to deal in the Software without restriction, */
+/* including without limitation the rights to use, copy, modify, merge, */
+/* publish, distribute, sublicense, and/or sell copies of the Software, */
+/* and to permit persons to whom the Software is furnished to do so, */
+/* subject to the following conditions: */
+/* */
+/* The above copyright notice and this permission notice shall be */
+/* included in all copies or substantial portions of the Software. */
+/* */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT 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 "watchdog.h"
+
+
+void WDT_EnableAndSetTimeout() {
+ uint8_t temp = WDT_ENABLE_bm | WDT_CEN_bm | WATCHDOG_TIMEOUT;
+ CCP = CCP_IOREG_gc;
+ WDT.CTRL = temp;
+
+ // Wait for WD to synchronize with new settings.
+ while (WDT_IsSyncBusy()) continue;
+}
+
+
+void WDT_Disable() {
+ uint8_t temp = (WDT.CTRL & ~WDT_ENABLE_bm) | WDT_CEN_bm;
+ CCP = CCP_IOREG_gc;
+ WDT.CTRL = temp;
+}
--- /dev/null
+/************************************************************************/
+/* XBoot Extensible AVR Bootloader */
+/* */
+/* Watchdog Module */
+/* */
+/* watchdog.c */
+/* */
+/* Alex Forencich <alex@alexforencich.com> */
+/* */
+/* Copyright (c) 2010 Alex Forencich */
+/* */
+/* Permission is hereby granted, free of charge, to any person */
+/* obtaining a copy of this software and associated documentation */
+/* files(the "Software"), to deal in the Software without restriction, */
+/* including without limitation the rights to use, copy, modify, merge, */
+/* publish, distribute, sublicense, and/or sell copies of the Software, */
+/* and to permit persons to whom the Software is furnished to do so, */
+/* subject to the following conditions: */
+/* */
+/* The above copyright notice and this permission notice shall be */
+/* included in all copies or substantial portions of the Software. */
+/* */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT 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. */
+/* */
+/************************************************************************/
+
+#pragma once
+
+#include "xboot.h"
+
+#define WDT_IsSyncBusy() (WDT.STATUS & WDT_SYNCBUSY_bm)
+#define WDT_Reset() asm("wdr")
+
+void WDT_EnableAndSetTimeout();
+void WDT_Disable();
--- /dev/null
+/************************************************************************/
+/* XBoot Extensible AVR Bootloader */
+/* */
+/* xboot.c */
+/* */
+/* Alex Forencich <alex@alexforencich.com> */
+/* */
+/* Copyright (c) 2010 Alex Forencich */
+/* */
+/* Permission is hereby granted, free of charge, to any person */
+/* obtaining a copy of this software and associated documentation */
+/* files(the "Software"), to deal in the Software without restriction, */
+/* including without limitation the rights to use, copy, modify, merge, */
+/* publish, distribute, sublicense, and/or sell copies of the Software, */
+/* and to permit persons to whom the Software is furnished to do so, */
+/* subject to the following conditions: */
+/* */
+/* The above copyright notice and this permission notice shall be */
+/* included in all copies or substantial portions of the Software. */
+/* */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT 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 "xboot.h"
+
+#include <util/delay.h>
+
+#include <stdbool.h>
+
+
+uint8_t buffer[SPM_PAGESIZE];
+
+
+void NVM_Wait() {
+ while (NVM_STATUS & NVM_NVMBUSY_bp)
+ // reset watchdog while waiting for erase completion
+ WDT_Reset();
+}
+
+
+int main() {
+ // Initialization section
+ // Entry point and communication methods are initialized here
+ // --------------------------------------------------
+#ifdef USE_32MHZ_RC
+#if (F_CPU != 32000000L)
+#error F_CPU must match oscillator setting!
+#endif // F_CPU
+ OSC.CTRL |= OSC_RC32MEN_bm; // turn on 32 MHz oscillator
+ while (!(OSC.STATUS & OSC_RC32MRDY_bm)) continue; // wait for it to start
+ CCP = CCP_IOREG_gc;
+ CLK.CTRL = CLK_SCLKSEL_RC32M_gc;
+#ifdef USE_DFLL
+ OSC.CTRL |= OSC_RC32KEN_bm;
+ while(!(OSC.STATUS & OSC_RC32KRDY_bm));
+ DFLLRC32M.CTRL = DFLL_ENABLE_bm;
+#endif // USE_DFLL
+#else // USE_32MHZ_RC
+#if (F_CPU != 2000000L)
+#error F_CPU must match oscillator setting!
+#endif // F_CPU
+#ifdef USE_DFLL
+ OSC.CTRL |= OSC_RC32KEN_bm;
+ while(!(OSC.STATUS & OSC_RC32KRDY_bm));
+ DFLLRC2M.CTRL = DFLL_ENABLE_bm;
+#endif // USE_DFLL
+#endif // USE_32MHZ_RC
+
+ // Initialize UART
+ uart_init();
+
+ // End initialization section
+
+ uint32_t address = 0;
+ uint16_t i = 0;
+
+ bool in_bootloader = false;
+ uint16_t j = ENTER_WAIT;
+ while (!in_bootloader && 0 < j--) {
+ // Check for trigger
+ if (uart_has_char()) in_bootloader = uart_recv_char() == CMD_SYNC;
+
+ _delay_ms(1);
+ }
+
+ WDT_EnableAndSetTimeout();
+
+ // Main bootloader
+ while (in_bootloader) {
+ uint8_t val = get_char();
+ WDT_Reset();
+
+ // Main bootloader parser
+ switch (val) {
+ case CMD_CHECK_AUTOINCREMENT: send_char(REPLY_YES); break;
+
+ case CMD_SET_ADDRESS:
+ address = get_word();
+ send_char(REPLY_ACK);
+ break;
+
+ case CMD_SET_EXT_ADDRESS:
+ address = ((uint32_t)get_char() << 16) | get_word();
+ send_char(REPLY_ACK);
+ break;
+
+ case CMD_CHIP_ERASE:
+ // Erase the application section
+ SP_EraseApplicationSection();
+
+ // Wait for completion
+ NVM_Wait();
+
+ // Erase EEPROM
+ EEPROM_erase_all();
+
+ send_char(REPLY_ACK);
+ break;
+
+ case CMD_CHECK_BLOCK_SUPPORT:
+ send_char(REPLY_YES);
+ // Send block size (page size)
+ send_char((SPM_PAGESIZE >> 8) & 0xFF);
+ send_char(SPM_PAGESIZE & 0xFF);
+ break;
+
+ case CMD_BLOCK_LOAD:
+ i = get_word(); // Block size
+ val = get_char(); // Memory type
+ send_char(BlockLoad(i, val, &address)); // Load it
+ break;
+
+ case CMD_BLOCK_READ:
+ i = get_word(); // Block size
+ val = get_char(); // Memory type
+ BlockRead(i, val, &address); // Read it
+ break;
+
+ case CMD_READ_BYTE: {
+ unsigned w = SP_ReadWord((address << 1));
+
+ send_char(w >> 8);
+ send_char(w);
+
+ address++;
+ break;
+ }
+
+ case CMD_WRITE_LOW_BYTE:
+ i = get_char(); // get low byte
+ send_char(REPLY_ACK);
+ break;
+
+ case CMD_WRITE_HIGH_BYTE:
+ i |= (get_char() << 8); // get high byte; combine
+ SP_LoadFlashWord((address << 1), i);
+ address++;
+ send_char(REPLY_ACK);
+ break;
+
+ case CMD_WRITE_PAGE:
+ if (address >= (APP_SECTION_SIZE >> 1))
+ send_char(REPLY_ERROR); // don't allow bootloader overwrite
+
+ else {
+ SP_WriteApplicationPage(address << 1);
+ send_char(REPLY_ACK);
+ }
+ break;
+
+ case CMD_WRITE_EEPROM_BYTE:
+ EEPROM_write_byte(address, get_char());
+ address++;
+ send_char(REPLY_ACK);
+ break;
+
+ case CMD_READ_EEPROM_BYTE:
+ send_char(EEPROM_read_byte(address));
+ address++;
+ break;
+
+ case CMD_READ_LOW_FUSE_BITS: send_char(SP_ReadFuseByte(0)); break;
+ case CMD_READ_HIGH_FUSE_BITS: send_char(SP_ReadFuseByte(1)); break;
+ case CMD_READ_EXT_FUSE_BITS: send_char(SP_ReadFuseByte(2)); break;
+
+ case CMD_ENTER_PROG_MODE: case CMD_LEAVE_PROG_MODE:
+ send_char(REPLY_ACK);
+ break;
+
+ case CMD_EXIT_BOOTLOADER:
+ in_bootloader = false;
+ send_char(REPLY_ACK);
+ break;
+
+ case CMD_PROGRAMMER_TYPE: send_char('S'); break; // serial
+
+ case CMD_DEVICE_CODE:
+ send_char(123); // send only this device
+ send_char(0); // terminator
+ break;
+
+ case CMD_SET_LED: case CMD_CLEAR_LED: case CMD_SET_TYPE:
+ get_char(); // discard parameter
+ send_char(REPLY_ACK);
+ break;
+
+ case CMD_PROGRAM_ID:
+ send_char('X');
+ send_char('B');
+ send_char('o');
+ send_char('o');
+ send_char('t');
+ send_char('+');
+ send_char('+');
+ break;
+
+ case CMD_VERSION:
+ send_char('0' + XBOOT_VERSION_MAJOR);
+ send_char('0' + XBOOT_VERSION_MINOR);
+ break;
+
+ case CMD_READ_SIGNATURE:
+ send_char(SIGNATURE_2);
+ send_char(SIGNATURE_1);
+ send_char(SIGNATURE_0);
+ break;
+
+ case CMD_CRC: {
+ uint32_t start = 0;
+ uint32_t length = 0;
+ uint16_t crc;
+
+ val = get_char();
+
+ switch (val) {
+ case SECTION_FLASH: length = PROGMEM_SIZE; break;
+ case SECTION_APPLICATION: length = APP_SECTION_SIZE; break;
+ case SECTION_BOOT:
+ start = BOOT_SECTION_START;
+ length = BOOT_SECTION_SIZE;
+ break;
+
+ default:
+ send_char(REPLY_ERROR);
+ continue;
+ }
+
+ crc = crc16_block(start, length);
+
+ send_char((crc >> 8) & 0xff);
+ send_char(crc & 0xff);
+ break;
+ }
+
+ case CMD_SYNC: break; // ESC (0x1b) to sync
+
+ default: // otherwise, error
+ send_char(REPLY_ERROR);
+ break;
+ }
+
+ // Wait for any lingering SPM instructions to finish
+ NVM_Wait();
+ }
+
+ // Bootloader exit
+ // Code here runs after the bootloader has exited,
+ // but before the application code has started
+
+ // Shut down UART
+ uart_deinit();
+
+ WDT_Disable();
+
+ // Jump into main code
+ asm("jmp 0");
+}
+
+
+uint8_t get_char() {
+ while (!uart_has_char()) continue;
+ return uart_recv_char();
+}
+
+
+void send_char(uint8_t c) {uart_send_char_blocking(c);}
+uint16_t get_word() {return (get_char() << 8) | get_char();}
+
+
+uint8_t BlockLoad(unsigned size, uint8_t mem, uint32_t *address) {
+ WDT_Reset();
+
+ // fill up buffer
+ for (int i = 0; i < SPM_PAGESIZE; i++) {
+ char c = 0xff;
+ if (i < size) c = get_char();
+ buffer[i] = c;
+ }
+
+ switch (mem) {
+ case MEM_EEPROM:
+ EEPROM_write_block(*address, buffer, size);
+ *address += size;
+ break;
+
+ case MEM_FLASH:
+ // NOTE: Flash programming, address is given in words.
+ SP_LoadFlashPage(buffer);
+ SP_EraseWriteApplicationPage(*address << 1);
+ *address += size >> 1;
+ NVM_Wait();
+ break;
+
+ case MEM_USERSIG:
+ SP_LoadFlashPage(buffer);
+ SP_EraseUserSignatureRow();
+ NVM_Wait();
+ SP_WriteUserSignatureRow();
+ NVM_Wait();
+ break;
+
+ default: return REPLY_ERROR;
+ }
+
+ return REPLY_ACK;
+}
+
+
+
+void BlockRead(unsigned size, uint8_t mem, uint32_t *address) {
+ int offset = 0;
+ int size2 = size;
+
+ switch (mem) {
+ case MEM_EEPROM:
+ EEPROM_read_block(*address, buffer, size);
+ (*address) += size;
+ break;
+
+ case MEM_FLASH: case MEM_USERSIG: case MEM_PRODSIG: {
+ *address <<= 1; // Convert address to bytes temporarily
+
+ do {
+ switch (mem) {
+ case MEM_FLASH: buffer[offset++] = SP_ReadByte(*address); break;
+
+ case MEM_USERSIG:
+ buffer[offset++] = SP_ReadUserSignatureByte(*address);
+ break;
+
+ case MEM_PRODSIG:
+ buffer[offset++] = SP_ReadCalibrationByte(*address);
+ break;
+ }
+
+ NVM_Wait();
+
+ (*address)++; // Select next word in memory.
+ size--; // Subtract two bytes from number of bytes to read
+ } while (size); // Repeat until all block has been read
+
+ *address >>= 1; // Convert address back to Flash words again.
+ break;
+ }
+
+ default: send_char(REPLY_ERROR); break;
+ }
+
+ // send bytes
+ for (int i = 0; i < size2; i++)
+ send_char(buffer[i]);
+}
+
+
+uint16_t crc16_block(uint32_t start, uint32_t length) {
+ uint16_t crc = 0;
+
+ int bc = SPM_PAGESIZE;
+
+ for (; length > 0; length--) {
+ if (bc == SPM_PAGESIZE) {
+ SP_ReadFlashPage(buffer, start);
+ start += SPM_PAGESIZE;
+ bc = 0;
+ }
+
+ crc = _crc16_update(crc, buffer[bc]);
+
+ bc++;
+ }
+
+ return crc;
+}
--- /dev/null
+/************************************************************************/
+/* XBoot Extensible AVR Bootloader */
+/* */
+/* xboot.h */
+/* */
+/* Alex Forencich <alex@alexforencich.com> */
+/* */
+/* Copyright (c) 2010 Alex Forencich */
+/* */
+/* Permission is hereby granted, free of charge, to any person */
+/* obtaining a copy of this software and associated documentation */
+/* files(the "Software"), to deal in the Software without restriction, */
+/* including without limitation the rights to use, copy, modify, merge, */
+/* publish, distribute, sublicense, and/or sell copies of the Software, */
+/* and to permit persons to whom the Software is furnished to do so, */
+/* subject to the following conditions: */
+/* */
+/* The above copyright notice and this permission notice shall be */
+/* included in all copies or substantial portions of the Software. */
+/* */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT 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. */
+/* */
+/************************************************************************/
+
+#pragma once
+
+#include <avr/io.h>
+#include <avr/interrupt.h>
+#include <util/crc16.h>
+
+// token pasting
+#define CAT2_int(x, y) x ## y
+#define CAT2(x, y) CAT2_int(x, y)
+#define CAT3_int(x, y, z) x ## y ## z
+#define CAT3(x, y, z) CAT3_int(x, y, z)
+
+// Version
+#define XBOOT_VERSION_MAJOR 1
+#define XBOOT_VERSION_MINOR 7
+
+// Configuration
+#define ENTER_WAIT 1000 // In ms
+#define UART_BAUD_RATE 115200
+#define UART_NUMBER 0
+#define UART_PORT_NAME C
+#define USE_AVR1008_EEPROM
+#define USE_DFLL
+#define WATCHDOG_TIMEOUT WDT_PER_1KCLK_gc
+
+// clock config
+// use 32MHz osc if makefile calls for it
+#if (F_CPU == 32000000L)
+// defaults to 2MHz RC oscillator
+// define USE_32MHZ_RC to override
+#define USE_32MHZ_RC
+#endif // F_CPU
+
+// UART RS485 Enable Output
+#define UART_EN_PORT CAT2(PORT, UART_EN_PORT_NAME)
+
+#if (UART_NUMBER == 0)
+#define UART_RX_PIN 2
+#define UART_TX_PIN 3
+#else
+#define UART_RX_PIN 6
+#define UART_TX_PIN 7
+#endif
+#define UART_PORT CAT2(PORT, UART_PORT_NAME)
+#define UART_DEVICE_PORT CAT2(UART_PORT_NAME, UART_NUMBER)
+#define UART_DEVICE CAT2(USART, UART_DEVICE_PORT)
+#define UART_DEVICE_RXC_ISR CAT3(USART, UART_DEVICE_PORT, _RXC_vect)
+#define UART_DEVICE_DRE_ISR CAT3(USART, UART_DEVICE_PORT, _DRE_vect)
+#define UART_DEVICE_TXC_ISR CAT3(USART, UART_DEVICE_PORT, _TXC_vect)
+#define UART_RX_PIN_CTRL CAT3(UART_PORT.PIN, UART_RX_PIN, CTRL)
+#define UART_TX_PIN_CTRL CAT3(UART_PORT.PIN, UART_TX_PIN, CTRL)
+
+// BAUD Rate Values
+// Known good at 2MHz
+#if (F_CPU == 2000000L) && (UART_BAUD_RATE == 19200)
+#define UART_BSEL_VALUE 12
+#define UART_BSCALE_VALUE 0
+#define UART_CLK2X 1
+#elif (F_CPU == 2000000L) && (UART_BAUD_RATE == 38400)
+#define UART_BSEL_VALUE 22
+#define UART_BSCALE_VALUE -2
+#define UART_CLK2X 1
+#elif (F_CPU == 2000000L) && (UART_BAUD_RATE == 57600)
+#define UART_BSEL_VALUE 26
+#define UART_BSCALE_VALUE -3
+#define UART_CLK2X 1
+#elif (F_CPU == 2000000L) && (UART_BAUD_RATE == 115200)
+#define UART_BSEL_VALUE 19
+#define UART_BSCALE_VALUE -4
+#define UART_CLK2X 1
+
+// Known good at 32MHz
+#elif (F_CPU == 32000000L) && (UART_BAUD_RATE == 19200)
+#define UART_BSEL_VALUE 103
+#define UART_BSCALE_VALUE 0
+#define UART_CLK2X 0
+#elif (F_CPU == 32000000L) && (UART_BAUD_RATE == 38400)
+#define UART_BSEL_VALUE 51
+#define UART_BSCALE_VALUE 0
+#define UART_CLK2X 0
+#elif (F_CPU == 32000000L) && (UART_BAUD_RATE == 57600)
+#define UART_BSEL_VALUE 34
+#define UART_BSCALE_VALUE 0
+#define UART_CLK2X 0
+#elif (F_CPU == 32000000L) && (UART_BAUD_RATE == 115200)
+#define UART_BSEL_VALUE 1047
+#define UART_BSCALE_VALUE -6
+#define UART_CLK2X 0
+
+#else // None of the above, so calculate something
+#warning Not using predefined BAUD rate, possible BAUD rate error!
+#if (F_CPU == 2000000L)
+#define UART_BSEL_VALUE ((F_CPU) / ((uint32_t)UART_BAUD_RATE * 8) - 1)
+#define UART_BSCALE_VALUE 0
+#define UART_CLK2X 1
+
+#else
+#define UART_BSEL_VALUE ((F_CPU) / ((uint32_t)UART_BAUD_RATE * 16) - 1)
+#define UART_BSCALE_VALUE 0
+#define UART_CLK2X 0
+#endif
+#endif
+
+#ifndef EEPROM_PAGE_SIZE
+#define EEPROM_PAGE_SIZE E2PAGESIZE
+#endif
+
+#ifndef EEPROM_BYTE_ADDRESS_MASK
+#if EEPROM_PAGE_SIZE == 32
+#define EEPROM_BYTE_ADDRESS_MASK 0x1f
+#elif EEPROM_PAGE_SIZE == 16
+#define EEPROM_BYTE_ADDRESS_MASK = 0x0f
+#elif EEPROM_PAGE_SIZE == 8
+#define EEPROM_BYTE_ADDRESS_MASK = 0x07
+#elif EEPROM_PAGE_SIZE == 4
+#define EEPROM_BYTE_ADDRESS_MASK = 0x03
+#else
+#error Unknown EEPROM page size! Please add new byte address value!
+#endif
+#endif
+
+// Includes
+#include "protocol.h"
+#include "sp_driver.h"
+#include "eeprom_driver.h"
+#include "uart.h"
+#include "watchdog.h"
+
+// Functions
+uint8_t get_char(void);
+void send_char(uint8_t c);
+uint16_t get_word(void);
+
+uint8_t BlockLoad(unsigned size, uint8_t mem, uint32_t *address);
+void BlockRead(unsigned size, uint8_t mem, uint32_t *address);
+
+uint16_t crc16_block(uint32_t start, uint32_t length);
--- /dev/null
+#!/usr/bin/env python3
+
+import sys
+import csv
+import json
+
+
+rdsel = 1
+
+
+def twos_comp(val, bits):
+ if val & (1 << (bits - 1)): return val - (1 << bits)
+ return val
+
+
+def tmc2660_decode_response(x, rdsel = 1):
+ x >>= 4 # Shift right 4 bits
+ d = {'_hex': '0x%05x' % x}
+
+ if rdsel == 0:
+ d['mstep'] = (x >> 10) & 0x3ff
+
+ elif rdsel == 1:
+ d['sg'] = (x >> 10) & 0x3ff
+
+ elif rdsel == 2:
+ d['sg'] = x >> 15 & 0x1f
+ d['se'] = (x >> 10) & 0x1f
+
+ flags = []
+ if x & (1 << 7): flags += ['stand']
+ if x & (1 << 6): flags += ['open B']
+ if x & (1 << 5): flags += ['open A']
+ if x & (1 << 4): flags += ['short B']
+ if x & (1 << 3): flags += ['short A']
+ if x & (1 << 2): flags += ['temp warn']
+ if x & (1 << 1): flags += ['overtemp']
+ if x & (1 << 0): flags += ['stall']
+
+ d['flags'] = flags
+
+ return d
+
+
+def tmc2660_decode_cmd(x, r):
+ global rdsel
+
+ addr = x >> 17
+
+ d = {'_hex': '0x%05x' % x}
+
+ if addr == 0 or addr == 1:
+ cmd = 'DRVCTRL'
+ d['mstep'] = (256, 128, 64, 32, 16, 8, 4, 2, 1)[x & 0xf]
+ d['dedge'] = bool(x & (1 << 8))
+ d['intpol'] = bool(x & (1 << 9))
+
+ elif addr == 4:
+ chm = bool(x & (1 << 14))
+
+ cmd = 'CHOPCONF'
+ d['blank'] = (16, 24, 36, 54)[(x >> 15) & 3]
+ d['chm'] = 'standard' if chm else 'constant'
+ d['random toff'] = bool(x & (1 << 13))
+
+ if chm: d['fast decay mode'] = ('current', 'timer')[x >> 12]
+ else: d['hdec'] = (16, 32, 48, 64)[(x >> 11) & 3]
+
+ if chm: d['SWO'] = ((x >> 7) & 0xf) - 3
+ else: d['HEND'] = ((x >> 7) & 0xf) - 3
+
+ if chm: d['fast decay'] = (((x >> 4) & 7) + ((x >> 7) & 8)) * 32
+ else: d['HSTART'] = ((x >> 4) & 7) + 1
+
+ # TODO this isn't quite right
+ toff = x & 0xf
+ if toff == 0: d['TOFF'] = 'disabled'
+ else: d['TOFF'] = 12 + (32 * toff)
+
+ elif addr == 5:
+ cmd = 'SMARTEN'
+ d['SEIMIN'] = '1/4 CS' if x & (1 << 15) else '1/2 CS'
+ d['SEDN'] = (32, 8, 2, 1)[(x >> 13) & 3]
+ d['SEMAX'] = (x >> 8) & 0xf
+ d['SEUP'] = (1, 2, 4, 8)[(x >> 5) & 3]
+ semin = x & 0xf
+ d['SEMIN'] = semin if semin else 'disabled'
+
+ elif addr == 6:
+ cmd = 'SGCSCONF'
+ d['SFILT'] = bool(x & (1 << 16))
+ d['SGT'] = twos_comp((x >> 8) & 0x7f, 7)
+ d['CS'] = ((x & 0x1f) + 1) / 32.0
+
+ elif addr == 7:
+ cmd = 'DRVCONF'
+ d['TST'] = bool(x & (1 << 16))
+ d['SLPH'] = ('min', 'min temp', 'med temp', 'max')[(x >> 14) & 3]
+ d['SLPL'] = ('min', 'min', 'med', 'max')[(x >> 12) & 3]
+ d['DISS2G'] = bool(x & (1 << 10))
+ d['TS2G'] = (3.2, 1.6, 1.2, 0.8)[(x >> 8) & 3]
+ d['SDOFF'] = 'SPI' if x & (1 << 7) else 'step'
+ d['VSENSE'] = '165mV' if x & (1 << 6) else '305mV'
+ rdsel = (x >> 4) & 3
+ d['RDSEL'] = ('mstep', 'SG', 'SG & CS', 'Invalid')[rdsel]
+
+ else: cmd = 'INVALID'
+
+ return {cmd: d, 'response': tmc2660_decode_response(r, rdsel)}
+
+
+def tmc2660_decode(path):
+ with open(path, newline = '') as f:
+ reader = csv.reader(f)
+
+ first = True
+ for time, packet, mosi, miso in reader:
+ if first:
+ first = False
+ continue
+
+ cmd = tmc2660_decode_cmd(int(mosi, 0), int(miso, 0))
+ cmd['id'] = int(packet)
+ cmd['ts'] = float(time)
+
+ print(json.dumps(cmd, sort_keys = True))
+
+
+if __name__ == "__main__":
+ for path in sys.argv[1:]:
+ tmc2660_decode(path)
+++ /dev/null
-#!/usr/bin/env python3
-
-import os
-import re
-import shlex
-import subprocess
-
-
-lineRE = r'%(addr)s '
-command = 'avr-objdump -j .bss -t buildbotics.elf'
-
-proc = subprocess.Popen(shlex.split(command), stdout = subprocess.PIPE)
-
-out, err = proc.communicate()
-
-if proc.returncode:
- print(out)
- raise Exception('command failed')
-
-def get_sizes(data):
- for line in data.decode().split('\n'):
- if not re.match(r'^[0-9a-f]{8} .*', line): continue
-
- size, name = int(line[22:30], 16), line[31:]
- if not size: continue
-
- yield (size, name)
-
-sizes = sorted(get_sizes(out))
-total = sum(x[0] for x in sizes)
-
-for size, name in sizes:
- print('% 6d %5.2f%% %s' % (size, size / total * 100, name))
-
-print('-' * 40)
-print('% 6d Total' % total)
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2016 Buildbotics LLC
- All rights reserved.
-
- 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/>.
-
- The software is distributed in the hope that it will be useful, but
- WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
- Lesser General Public License for more details.
-
- You should have received a copy of the GNU Lesser General Public
- License along with the software. If not, see
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-
-#include "axis.h"
-#include "motor.h"
-#include "plan/planner.h"
-
-#include <math.h>
-#include <string.h>
-
-
-int motor_map[AXES] = {-1, -1, -1, -1, -1, -1};
-
-
-typedef struct {
- float velocity_max; // max velocity in mm/min or deg/min
- float feedrate_max; // max velocity in mm/min or deg/min
- 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 recip_jerk; // reciprocal of current jerk value - with million
- float junction_dev; // aka cornering delta
- float radius; // radius in mm for rotary axes
- float search_velocity; // homing search velocity
- float latch_velocity; // homing latch velocity
- float latch_backoff; // backoff from switches prior to homing latch movement
- float zero_backoff; // backoff from switches for machine zero
- homing_mode_t homing_mode;
- bool homed;
-} axis_t;
-
-
-axis_t axes[MOTORS] = {
- {
- .velocity_max = X_VELOCITY_MAX,
- .feedrate_max = X_FEEDRATE_MAX,
- .travel_min = X_TRAVEL_MIN,
- .travel_max = X_TRAVEL_MAX,
- .jerk_max = X_JERK_MAX,
- .junction_dev = X_JUNCTION_DEVIATION,
- .search_velocity = X_SEARCH_VELOCITY,
- .latch_velocity = X_LATCH_VELOCITY,
- .latch_backoff = X_LATCH_BACKOFF,
- .zero_backoff = X_ZERO_BACKOFF,
- .homing_mode = X_HOMING_MODE,
- }, {
- .velocity_max = Y_VELOCITY_MAX,
- .feedrate_max = Y_FEEDRATE_MAX,
- .travel_min = Y_TRAVEL_MIN,
- .travel_max = Y_TRAVEL_MAX,
- .jerk_max = Y_JERK_MAX,
- .junction_dev = Y_JUNCTION_DEVIATION,
- .search_velocity = Y_SEARCH_VELOCITY,
- .latch_velocity = Y_LATCH_VELOCITY,
- .latch_backoff = Y_LATCH_BACKOFF,
- .zero_backoff = Y_ZERO_BACKOFF,
- .homing_mode = Y_HOMING_MODE,
- }, {
- .velocity_max = Z_VELOCITY_MAX,
- .feedrate_max = Z_FEEDRATE_MAX,
- .travel_min = Z_TRAVEL_MIN,
- .travel_max = Z_TRAVEL_MAX,
- .jerk_max = Z_JERK_MAX,
- .junction_dev = Z_JUNCTION_DEVIATION,
- .search_velocity = Z_SEARCH_VELOCITY,
- .latch_velocity = Z_LATCH_VELOCITY,
- .latch_backoff = Z_LATCH_BACKOFF,
- .zero_backoff = Z_ZERO_BACKOFF,
- .homing_mode = Z_HOMING_MODE,
- }, {
- .velocity_max = A_VELOCITY_MAX,
- .feedrate_max = A_FEEDRATE_MAX,
- .travel_min = A_TRAVEL_MIN,
- .travel_max = A_TRAVEL_MAX,
- .jerk_max = A_JERK_MAX,
- .junction_dev = A_JUNCTION_DEVIATION,
- .radius = A_RADIUS,
- .search_velocity = A_SEARCH_VELOCITY,
- .latch_velocity = A_LATCH_VELOCITY,
- .latch_backoff = A_LATCH_BACKOFF,
- .zero_backoff = A_ZERO_BACKOFF,
- .homing_mode = A_HOMING_MODE,
- }
-};
-
-
-bool axis_is_enabled(int axis) {
- int motor = axis_get_motor(axis);
- return motor != -1 && motor_is_enabled(motor);
-}
-
-
-char axis_get_char(int axis) {
- return (axis < 0 || AXES <= axis) ? '?' : "XYZABCUVW"[axis];
-}
-
-
-int axis_get_id(char axis) {
- const char *axes = "XYZABCUVW";
- char *ptr = strchr(axes, axis);
- return ptr == 0 ? -1 : (ptr - axes);
-}
-
-
-int axis_get_motor(int axis) {return motor_map[axis];}
-
-
-void axis_set_motor(int axis, int motor) {
- motor_map[axis] = motor;
- axis_set_jerk_max(axis, axes[motor].jerk_max); // Init 1/jerk
-}
-
-
-float axis_get_vector_length(const float a[], const float b[]) {
- return sqrt(square(a[AXIS_X] - b[AXIS_X]) + square(a[AXIS_Y] - b[AXIS_Y]) +
- square(a[AXIS_Z] - b[AXIS_Z]) + square(a[AXIS_A] - b[AXIS_A]) +
- square(a[AXIS_B] - b[AXIS_B]) + square(a[AXIS_C] - b[AXIS_C]));
-}
-
-
-#define AXIS_VAR_GET(NAME, TYPE) \
- TYPE get_##NAME(int axis) {return axes[axis].NAME;}
-
-
-#define AXIS_VAR_SET(NAME, TYPE) \
- void set_##NAME(int axis, TYPE value) {axes[axis].NAME = value;}
-
-
-#define AXIS_GET(NAME, TYPE, DEFAULT) \
- TYPE axis_get_##NAME(int axis) { \
- int motor = axis_get_motor(axis); \
- return motor == -1 ? DEFAULT : axes[motor].NAME; \
- } \
- AXIS_VAR_GET(NAME, TYPE)
-
-
-#define AXIS_SET(NAME, TYPE) \
- void axis_set_##NAME(int axis, TYPE value) { \
- int motor = axis_get_motor(axis); \
- if (motor != -1) axes[motor].NAME = value; \
- } \
- AXIS_VAR_SET(NAME, TYPE)
-
-
-AXIS_GET(velocity_max, float, 0)
-AXIS_GET(feedrate_max, float, 0)
-AXIS_GET(homed, bool, false)
-AXIS_SET(homed, bool)
-AXIS_GET(homing_mode, homing_mode_t, HOMING_DISABLED)
-AXIS_SET(homing_mode, homing_mode_t)
-AXIS_GET(radius, float, 0)
-AXIS_GET(travel_min, float, DISABLE_SOFT_LIMIT)
-AXIS_GET(travel_max, float, DISABLE_SOFT_LIMIT)
-AXIS_GET(search_velocity, float, 0)
-AXIS_GET(latch_velocity, float, 0)
-AXIS_GET(zero_backoff, float, 0)
-AXIS_GET(latch_backoff, float, 0)
-AXIS_GET(junction_dev, float, 0)
-AXIS_GET(recip_jerk, float, 0)
-
-
-/* Note on jerk functions
- *
- * Jerk values can be rather large. Jerk values are stored in the system in
- * truncated format; values are divided by 1,000,000 then multiplied before use.
- *
- * The axis_jerk() functions expect the jerk in divided by 1,000,000 form.
- */
-AXIS_GET(jerk_max, float, 0)
-
-
-/// Sets jerk and its reciprocal for axis
-void axis_set_jerk_max(int axis, float jerk) {
- axes[axis].jerk_max = jerk;
- axes[axis].recip_jerk = 1 / (jerk * JERK_MULTIPLIER);
-}
-
-
-AXIS_VAR_SET(velocity_max, float)
-AXIS_VAR_SET(feedrate_max, float)
-AXIS_VAR_SET(radius, float)
-AXIS_VAR_SET(travel_min, float)
-AXIS_VAR_SET(travel_max, float)
-AXIS_VAR_SET(search_velocity, float)
-AXIS_VAR_SET(latch_velocity, float)
-AXIS_VAR_SET(zero_backoff, float)
-AXIS_VAR_SET(latch_backoff, float)
-AXIS_VAR_SET(junction_dev, float)
-AXIS_VAR_SET(jerk_max, float)
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2016 Buildbotics LLC
- Copyright (c) 2010 - 2015 Alden S. Hart, Jr.
- All rights reserved.
-
- 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/>.
-
- The software is distributed in the hope that it will be useful, but
- WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
- Lesser General Public License for more details.
-
- You should have received a copy of the GNU Lesser General Public
- License along with the software. If not, see
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-#pragma once
-
-#include "config.h"
-
-#include <stdbool.h>
-
-
-enum {
- AXIS_X, AXIS_Y, AXIS_Z,
- AXIS_A, AXIS_B, AXIS_C,
- AXIS_U, AXIS_V, AXIS_W // reserved
-};
-
-
-typedef enum {
- HOMING_DISABLED,
- HOMING_STALL_MIN,
- HOMING_STALL_MAX,
- HOMING_SWITCH_MIN,
- HOMING_SWITCH_MAX,
-} homing_mode_t;
-
-
-bool axis_is_enabled(int axis);
-char axis_get_char(int axis);
-int axis_get_id(char axis);
-int axis_get_motor(int axis);
-void axis_set_motor(int axis, int motor);
-float axis_get_vector_length(const float a[], const float b[]);
-
-float axis_get_velocity_max(int axis);
-float axis_get_feedrate_max(int axis);
-float axis_get_jerk_max(int axis);
-void axis_set_jerk_max(int axis, float jerk);
-bool axis_get_homed(int axis);
-void axis_set_homed(int axis, bool homed);
-homing_mode_t axis_get_homing_mode(int axis);
-void axis_set_homing_mode(int axis, homing_mode_t mode);
-float axis_get_radius(int axis);
-float axis_get_travel_min(int axis);
-float axis_get_travel_max(int axis);
-float axis_get_search_velocity(int axis);
-float axis_get_latch_velocity(int axis);
-float axis_get_zero_backoff(int axis);
-float axis_get_latch_backoff(int axis);
-float axis_get_junction_dev(int axis);
-float axis_get_recip_jerk(int axis);
-float axis_get_jerk_max(int axis);
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2016 Buildbotics LLC
- All rights reserved.
-
- 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/>.
-
- The software is distributed in the hope that it will be useful, but
- WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
- Lesser General Public License for more details.
-
- You should have received a copy of the GNU Lesser General Public
- License along with the software. If not, see
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-#include "command.h"
-
-#include "gcode_parser.h"
-#include "usart.h"
-#include "hardware.h"
-#include "report.h"
-#include "vars.h"
-#include "estop.h"
-#include "homing.h"
-#include "probing.h"
-#include "i2c.h"
-#include "plan/jog.h"
-#include "plan/calibrate.h"
-#include "plan/buffer.h"
-#include "plan/arc.h"
-#include "plan/state.h"
-#include "config.h"
-
-#include <avr/pgmspace.h>
-#include <avr/wdt.h>
-
-#include <stdio.h>
-#include <string.h>
-#include <ctype.h>
-#include <stdlib.h>
-
-
-static char *_cmd = 0;
-
-
-static void _reboot() {hw_request_hard_reset();}
-
-
-static unsigned _parse_axis(uint8_t axis) {
- switch (axis) {
- case 'x': return 0; case 'y': return 1; case 'z': return 2;
- case 'a': return 3; case 'b': return 4; case 'c': return 5;
- case 'X': return 0; case 'Y': return 1; case 'Z': return 2;
- case 'A': return 3; case 'B': return 4; case 'C': return 5;
- default: return axis;
- }
-}
-
-
-static void command_i2c_cb(i2c_cmd_t cmd, uint8_t *data, uint8_t length) {
- switch (cmd) {
- case I2C_NULL: break;
- case I2C_ESTOP: estop_trigger(STAT_ESTOP_USER); break;
- case I2C_CLEAR: estop_clear(); break;
- case I2C_PAUSE: mp_request_hold(); break;
- case I2C_OPTIONAL_PAUSE: mp_request_optional_pause(); break;
- case I2C_RUN: mp_request_start(); break;
- case I2C_STEP: mp_request_step(); break;
- case I2C_FLUSH: mp_request_flush(); break;
- case I2C_REPORT: report_request_full(); break;
- case I2C_HOME: break; // TODO
- case I2C_REBOOT: _reboot(); break;
- case I2C_ZERO:
- if (length == 0) mach_zero_all();
- else if (length == 1) mach_zero_axis(_parse_axis(*data));
- break;
- }
-}
-
-
-void command_init() {
- i2c_set_read_callback(command_i2c_cb);
-}
-
-
-// Command forward declarations
-// (Don't be afraid, X-Macros rock!)
-#define CMD(NAME, ...) \
- uint8_t command_##NAME(int, char *[]);
-
-#include "command.def"
-#undef CMD
-
-// Command names & help
-#define CMD(NAME, MINARGS, MAXARGS, HELP) \
- static const char pstr_##NAME[] PROGMEM = #NAME; \
- static const char NAME##_help[] PROGMEM = HELP;
-
-#include "command.def"
-#undef CMD
-
-// Command table
-#define CMD(NAME, MINARGS, MAXARGS, HELP) \
- {pstr_##NAME, command_##NAME, MINARGS, MAXARGS, NAME##_help},
-
-static const command_t commands[] PROGMEM = {
-#include "command.def"
-#undef CMD
- {}, // Sentinel
-};
-
-
-int command_find(const char *match) {
- for (int i = 0; ; i++) {
- const char *name = pgm_read_word(&commands[i].name);
- if (!name) break;
-
- if (strcmp_P(match, name) == 0) return i;
- }
-
- return -1;
-}
-
-
-int command_exec(int argc, char *argv[]) {
- putchar('\n');
-
- int i = command_find(argv[0]);
- if (i != -1) {
- uint8_t min_args = pgm_read_byte(&commands[i].min_args);
- uint8_t max_args = pgm_read_byte(&commands[i].max_args);
-
- if (argc <= min_args) return STAT_TOO_FEW_ARGUMENTS;
- else if (max_args < argc - 1) return STAT_TOO_MANY_ARGUMENTS;
- else {
- command_cb_t cb = pgm_read_word(&commands[i].cb);
- return cb(argc, argv);
- }
-
- } else if (argc != 1)
- return STAT_INVALID_OR_MALFORMED_COMMAND;
-
- // Get or set variable
- char *value = strchr(argv[0], '=');
- if (value) {
- *value++ = 0;
- if (vars_set(argv[0], value)) return STAT_OK;
-
- } else if (vars_print(argv[0])) {
- putchar('\n');
- return STAT_OK;
- }
-
- STATUS_ERROR(STAT_UNRECOGNIZED_NAME, "'%s'", argv[0]);
- return STAT_UNRECOGNIZED_NAME;
-}
-
-
-int command_parser(char *cmd) {
- // Parse line
- char *p = cmd + 1; // Skip `$`
- int argc = 0;
- char *argv[MAX_ARGS] = {0};
-
- if (cmd[1] == '$' && !cmd[2]) {
- report_request_full(); // Full report
- return STAT_OK;
- }
-
- while (argc < MAX_ARGS && *p) {
- // Skip space
- while (*p && isspace(*p)) *p++ = 0;
-
- // Start of token
- if (*p) argv[argc++] = p;
-
- // Find end
- while (*p && !isspace(*p)) p++;
- }
-
- // Exec command
- if (argc) return command_exec(argc, argv);
-
- return STAT_OK;
-}
-
-
-static char *_command_next() {
- if (_cmd) return _cmd;
-
- // Get next command
- _cmd = usart_readline();
- if (!_cmd) return 0;
-
- // Remove leading whitespace
- while (*_cmd && isspace(*_cmd)) _cmd++;
-
- // Remove trailing whitespace
- for (size_t len = strlen(_cmd); len && isspace(_cmd[len - 1]); len--)
- _cmd[len - 1] = 0;
-
- return _cmd;
-}
-
-
-void command_callback() {
- if (!_command_next()) return;
-
- stat_t status = STAT_OK;
-
- switch (*_cmd) {
- case 0: break; // Empty line
- case '{': status = vars_parser(_cmd); break;
- case '$': status = command_parser(_cmd); break;
- case '%': break; // GCode program separator, ignore it
-
- default:
- if (estop_triggered()) {status = STAT_MACHINE_ALARMED; break;}
- else if (mp_is_flushing()) break; // Flush GCode command
- else if (!mp_queue_get_room() ||
- mp_is_resuming() ||
- mach_arc_active() ||
- mach_is_homing() ||
- mach_is_probing() ||
- calibrate_busy() ||
- mp_jog_busy()) return; // Wait
-
- // Parse and execute GCode command
- status = gc_gcode_parser(_cmd);
- }
-
- _cmd = 0; // Command consumed
- report_request();
-
- if (status) status_error(status);
-}
-
-
-// Command functions
-void static print_command_help(int i) {
- static const char fmt[] PROGMEM = " $%-12S %S\n";
- const char *name = pgm_read_word(&commands[i].name);
- const char *help = pgm_read_word(&commands[i].help);
-
- printf_P(fmt, name, help);
-}
-
-
-uint8_t command_help(int argc, char *argv[]) {
- if (argc == 2) {
- int i = command_find(argv[1]);
-
- if (i == -1) return STAT_UNRECOGNIZED_NAME;
- else print_command_help(i);
-
- return STAT_OK;
- }
-
- puts_P(PSTR("\nLine editing:\n"
- " ENTER Submit current command line.\n"
- " BS Backspace, delete last character.\n"
- " CTRL-X Cancel current line entry."));
-
- puts_P(PSTR("\nCommands:"));
- for (int i = 0; ; i++) {
- const char *name = pgm_read_word(&commands[i].name);
- if (!name) break;
- print_command_help(i);
- wdt_reset();
- }
-
- puts_P(PSTR("\nVariables:"));
- vars_print_help();
-
- return STAT_OK;
-}
-
-
-uint8_t command_reboot(int argc, char *argv[]) {
- _reboot();
- return 0;
-}
-
-
-uint8_t command_bootloader(int argc, char *argv[]) {
- hw_request_bootloader();
- return 0;
-}
-
-
-uint8_t command_save(int argc, char *argv[]) {
- vars_save();
- return 0;
-}
-
-
-uint8_t command_valid(int argc, char *argv[]) {
- printf_P(vars_valid() ? PSTR("true\n") : PSTR("false\n"));
- return 0;
-}
-
-
-uint8_t command_restore(int argc, char *argv[]) {
- return vars_restore();
-}
-
-
-uint8_t command_clear(int argc, char *argv[]) {
- vars_clear();
- return 0;
-}
-
-
-uint8_t command_messages(int argc, char *argv[]) {
- status_help();
- return 0;
-}
-
-
-uint8_t command_resume(int argc, char *argv[]) {
- mp_request_resume();
- return 0;
-}
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2016 Buildbotics LLC
- All rights reserved.
-
- 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/>.
-
- The software is distributed in the hope that it will be useful, but
- WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
- Lesser General Public License for more details.
-
- You should have received a copy of the GNU Lesser General Public
- License along with the software. If not, see
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-// Name Min, Max args, Help
-CMD(help, 0, 1, "Print this help screen")
-CMD(reboot, 0, 0, "Reboot the controller")
-CMD(bootloader, 0, 0, "Load bootloader")
-CMD(save, 0, 0, "Save settings")
-CMD(valid, 0, 0, "Print 'true' if saved settings are valid")
-CMD(restore, 0, 0, "Restore settings")
-CMD(clear, 0, 0, "Clear saved settings")
-CMD(jog, 1, 4, "Jog")
-CMD(mreset, 0, 1, "Reset motor")
-CMD(calibrate, 0, 0, "Calibrate motors")
-CMD(messages, 0, 0, "Dump all possible status messages")
-CMD(resume, 0, 0, "Resume processing after a flush")
-CMD(home, 0, 0, "Home all axes")
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2016 Buildbotics LLC
- All rights reserved.
-
- 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/>.
-
- The software is distributed in the hope that it will be useful, but
- WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
- Lesser General Public License for more details.
-
- You should have received a copy of the GNU Lesser General Public
- License along with the software. If not, see
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-#pragma once
-
-
-#include <stdint.h>
-
-
-#define MAX_ARGS 16
-
-typedef uint8_t (*command_cb_t)(int argc, char *argv[]);
-
-typedef struct {
- const char *name;
- command_cb_t cb;
- uint8_t min_args;
- uint8_t max_args;
- const char *help;
-} command_t;
-
-
-void command_init();
-int command_find(const char *name);
-int command_exec(int argc, char *argv[]);
-void command_callback();
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2016 Buildbotics LLC
- All rights reserved.
-
- 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/>.
-
- The software is distributed in the hope that it will be useful, but
- WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
- Lesser General Public License for more details.
-
- You should have received a copy of the GNU Lesser General Public
- License along with the software. If not, see
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-#pragma once
-
-#include "pins.h"
-
-#include <avr/interrupt.h>
-
-
-// Pins
-enum {
- STALL_X_PIN = PORT_A << 3,
- STALL_Y_PIN,
- STALL_Z_PIN,
- STALL_A_PIN,
- SPIN_DIR_PIN,
- SPIN_ENABLE_PIN,
- ANALOG_PIN,
- PROBE_PIN,
-
- MIN_X_PIN = PORT_B << 3,
- MAX_X_PIN,
- MIN_A_PIN,
- MAX_A_PIN,
- MIN_Y_PIN,
- MAX_Y_PIN,
- MIN_Z_PIN,
- MAX_Z_PIN,
-
- SDA_PIN = PORT_C << 3,
- SCL_PIN,
- SERIAL_RX_PIN,
- SERIAL_TX_PIN,
- SERIAL_CTS_PIN,
- SPI_CLK_PIN,
- SPI_MISO_PIN,
- SPI_MOSI_PIN,
-
- STEP_X_PIN = PORT_D << 3,
- SPI_CS_X_PIN,
- SPI_CS_A_PIN,
- SPI_CS_Z_PIN,
- SPIN_PWM_PIN,
- SWITCH_1_PIN,
- RS485_RO_PIN,
- RS485_DI_PIN,
-
- STEP_Y_PIN = PORT_E << 3,
- SPI_CS_Y_PIN,
- DIR_X_PIN,
- DIR_Y_PIN,
- STEP_A_PIN,
- SWITCH_2_PIN,
- DIR_Z_PIN,
- DIR_A_PIN,
-
- STEP_Z_PIN = PORT_F << 3,
- RS485_RW_PIN,
- FAULT_PIN,
- ESTOP_PIN,
- MOTOR_FAULT_PIN,
- MOTOR_ENABLE_PIN,
- NC_0_PIN,
- NC_1_PIN,
-};
-
-#define SPI_SS_PIN SERIAL_CTS_PIN // Needed for SPI configuration
-
-
-// Compile-time settings
-#define __CLOCK_EXTERNAL_16MHZ // uses PLL to provide 32 MHz system clock
-//#define __CLOCK_INTERNAL_32MHZ
-
-
-#define AXES 6 // number of axes
-#define MOTORS 4 // number of motors on the board
-#define COORDS 6 // number of supported coordinate systems
-#define SWITCHES 9 // number of supported limit switches
-#define PWMS 2 // number of supported PWM channels
-
-#define DISABLE_SOFT_LIMIT -1000000
-
-
-// Motor settings. See motor.c
-#define MOTOR_MAX_CURRENT 1.0 // 1.0 is full power
-#define MOTOR_MIN_CURRENT 0.25 // 1.0 is full power
-#define MOTOR_IDLE_CURRENT 0.05 // 1.0 is full power
-#define MOTOR_STALL_THRESHOLD 0 // 0 -> 1 is least -> most sensitive
-#define MOTOR_MICROSTEPS 32
-#define MOTOR_POWER_MODE MOTOR_POWERED_ONLY_WHEN_MOVING
-#define MOTOR_IDLE_TIMEOUT 0.25 // secs, motor off after this time
-
-#define M1_AXIS AXIS_X
-#define M1_STEP_ANGLE 1.8
-#define M1_TRAVEL_PER_REV 6.35
-#define M1_MICROSTEPS MOTOR_MICROSTEPS
-#define M1_REVERSE false
-#define M1_POWER_MODE MOTOR_POWER_MODE
-
-#define M2_AXIS AXIS_Y
-#define M2_STEP_ANGLE 1.8
-#define M2_TRAVEL_PER_REV 6.35
-#define M2_MICROSTEPS MOTOR_MICROSTEPS
-#define M2_REVERSE false
-#define M2_POWER_MODE MOTOR_POWER_MODE
-
-#define M3_AXIS AXIS_Z
-#define M3_STEP_ANGLE 1.8
-#define M3_TRAVEL_PER_REV (25.4 / 6.0)
-#define M3_MICROSTEPS MOTOR_MICROSTEPS
-#define M3_REVERSE false
-#define M3_POWER_MODE MOTOR_POWER_MODE
-
-#define M4_AXIS AXIS_A
-#define M4_STEP_ANGLE 1.8
-#define M4_TRAVEL_PER_REV 360 // degrees per motor rev
-#define M4_MICROSTEPS MOTOR_MICROSTEPS
-#define M4_REVERSE false
-#define M4_POWER_MODE MOTOR_POWER_MODE
-
-
-// Switch settings. See switch.c
-#define SWITCH_INTLVL PORT_INT0LVL_MED_gc
-#define SW_LOCKOUT_TICKS 250 // ms
-#define SW_DEGLITCH_TICKS 30 // ms
-
-
-// Machine settings
-//#define STEP_CORRECTION // Enable step correction
-#define MAX_STEP_CORRECTION 4 // In steps per segment
-#define CHORDAL_TOLERANCE 0.01 // chordal accuracy for arcs
-#define JERK_MAX 5 // yes, that's km/min^3
-#define JUNCTION_DEVIATION 0.05 // default value, in mm
-#define JUNCTION_ACCELERATION 100000 // centripetal corner accel
-#define JOG_JERK_MULT 4 // Jogging jerk multipler
-#define JOG_MIN_VELOCITY 10 // mm/min
-#define CAL_ACCELERATION 500000 // mm/min^2
-
-// Axis settings
-#define VELOCITY_MAX 13000 // mm/min
-#define FEEDRATE_MAX VELOCITY_MAX
-
-#define X_VELOCITY_MAX VELOCITY_MAX // G0 max velocity in mm/min
-#define X_FEEDRATE_MAX FEEDRATE_MAX // G1 max feed rate in mm/min
-#define X_TRAVEL_MIN 0 // minimum travel for soft limits
-#define X_TRAVEL_MAX 350 // between switches or crashes
-#define X_JERK_MAX JERK_MAX
-#define X_JUNCTION_DEVIATION JUNCTION_DEVIATION
-#define X_SEARCH_VELOCITY 2400 // move in negative direction
-#define X_LATCH_VELOCITY 100 // mm/min
-#define X_LATCH_BACKOFF 5 // mm
-#define X_ZERO_BACKOFF 1 // mm
-#define X_HOMING_MODE HOMING_STALL_MAX
-
-#define Y_VELOCITY_MAX VELOCITY_MAX
-#define Y_FEEDRATE_MAX FEEDRATE_MAX
-#define Y_TRAVEL_MIN 0
-#define Y_TRAVEL_MAX 350
-#define Y_JERK_MAX JERK_MAX
-#define Y_JUNCTION_DEVIATION JUNCTION_DEVIATION
-#define Y_SEARCH_VELOCITY 3000
-#define Y_LATCH_VELOCITY 100
-#define Y_LATCH_BACKOFF 5
-#define Y_ZERO_BACKOFF 1
-#define Y_HOMING_MODE HOMING_STALL_MAX
-
-#define Z_VELOCITY_MAX 2000 // VELOCITY_MAX
-#define Z_FEEDRATE_MAX FEEDRATE_MAX
-#define Z_TRAVEL_MIN 0
-#define Z_TRAVEL_MAX 75
-#define Z_JERK_MAX JERK_MAX
-#define Z_JUNCTION_DEVIATION JUNCTION_DEVIATION
-#define Z_SEARCH_VELOCITY 400
-#define Z_LATCH_VELOCITY 100
-#define Z_LATCH_BACKOFF 5
-#define Z_ZERO_BACKOFF 1
-#define Z_HOMING_MODE HOMING_STALL_MAX
-
-// A values are chosen to make the A motor react the same as X for testing
-// set to the same speed as X axis
-#define A_VELOCITY_MAX (X_VELOCITY_MAX / M1_TRAVEL_PER_REV * 360)
-#define A_FEEDRATE_MAX A_VELOCITY_MAX
-#define A_TRAVEL_MIN -1
-#define A_TRAVEL_MAX -1 // same value means infinite
-#define A_JERK_MAX (X_JERK_MAX * 360 / M1_TRAVEL_PER_REV)
-#define A_JUNCTION_DEVIATION JUNCTION_DEVIATION
-#define A_RADIUS (M1_TRAVEL_PER_REV / 2 / M_PI)
-#define A_SEARCH_VELOCITY 600
-#define A_LATCH_VELOCITY 100
-#define A_LATCH_BACKOFF 5
-#define A_ZERO_BACKOFF 2
-#define A_HOMING_MODE HOMING_DISABLED
-
-
-// Spindle settings
-#define SPINDLE_TYPE SPINDLE_TYPE_HUANYANG
-#define SPINDLE_PWM_FREQUENCY 100 // in Hz
-#define SPINDLE_MIN_RPM 1000
-#define SPINDLE_MAX_RPM 24000
-#define SPINDLE_MIN_DUTY 0.05
-#define SPINDLE_MAX_DUTY 0.99
-#define SPINDLE_REVERSE false
-
-
-// Gcode defaults
-#define GCODE_DEFAULT_UNITS MILLIMETERS // MILLIMETERS or INCHES
-#define GCODE_DEFAULT_PLANE PLANE_XY // See machine.h
-#define GCODE_DEFAULT_COORD_SYSTEM G54 // G54, G55, G56, G57, G58, G59
-#define GCODE_DEFAULT_PATH_CONTROL PATH_CONTINUOUS
-#define GCODE_DEFAULT_DISTANCE_MODE ABSOLUTE_MODE
-#define GCODE_DEFAULT_ARC_DISTANCE_MODE INCREMENTAL_MODE
-
-
-// Motor ISRs
-#define STALL_ISR_vect PORTA_INT1_vect
-#define FAULT_ISR_vect PORTF_INT1_vect
-
-
-/* Interrupt usage:
- *
- * HI Stepper timers (set in stepper.h)
- * LO Segment execution SW interrupt (set in stepper.h)
- * MED GPIO1 switch port (set in gpio.h)
- * MED Serial RX (set in usart.c)
- * MED Serial TX (set in usart.c) (* see note)
- * LO Real time clock interrupt (set in xmega_rtc.h)
- *
- * (*) The TX cannot run at LO level or exception reports and other prints
- * called from a LO interrupt (as in prep_line()) will kill the system
- * in a permanent loop call in usart_putc() (usart.c).
- */
-
-// Timer assignments - see specific modules for details
-// TCC1 free
-#define TIMER_STEP TCC0 // Step timer (see stepper.h)
-#define TIMER_PWM TCD1 // PWM timer (see pwm_spindle.c)
-
-#define M1_TIMER TCD0
-#define M2_TIMER TCE0
-#define M3_TIMER TCF0
-#define M4_TIMER TCE1
-
-#define M1_DMA_CH DMA.CH0
-#define M2_DMA_CH DMA.CH1
-#define M3_DMA_CH DMA.CH2
-#define M4_DMA_CH DMA.CH3
-
-#define M1_DMA_TRIGGER DMA_CH_TRIGSRC_TCD0_CCA_gc
-#define M2_DMA_TRIGGER DMA_CH_TRIGSRC_TCE0_CCA_gc
-#define M3_DMA_TRIGGER DMA_CH_TRIGSRC_TCF0_CCA_gc
-#define M4_DMA_TRIGGER DMA_CH_TRIGSRC_TCE1_CCA_gc
-
-
-// Timer setup for stepper and dwells
-#define STEP_TIMER_DISABLE 0
-#define STEP_TIMER_ENABLE TC_CLKSEL_DIV4_gc
-#define STEP_TIMER_DIV 4
-#define STEP_TIMER_FREQ (F_CPU / STEP_TIMER_DIV)
-#define STEP_TIMER_POLL (STEP_TIMER_FREQ * 0.001)
-#define STEP_TIMER_WGMODE TC_WGMODE_NORMAL_gc // count to TOP & rollover
-#define STEP_TIMER_ISR TCC0_OVF_vect
-#define STEP_TIMER_INTLVL TC_OVFINTLVL_HI_gc
-
-#define MAX_SEGMENT_TIME ((float)0xffff / 60.0 / STEP_TIMER_FREQ)
-#define NOM_SEGMENT_USEC 5000.0 // nominal segment time
-#define MIN_SEGMENT_USEC 2500.0 // minimum segment time
-
-
-// Huanyang settings
-#define HUANYANG_PORT USARTD1
-#define HUANYANG_DRE_vect USARTD1_DRE_vect
-#define HUANYANG_TXC_vect USARTD1_TXC_vect
-#define HUANYANG_RXC_vect USARTD1_RXC_vect
-#define HUANYANG_TIMEOUT 50 // ms. response timeout
-#define HUANYANG_RETRIES 4 // Number of retries before failure
-#define HUANYANG_ID 1 // Default ID
-
-
-// Serial settings
-#define SERIAL_BAUD USART_BAUD_115200
-#define SERIAL_PORT USARTC0
-#define SERIAL_DRE_vect USARTC0_DRE_vect
-#define SERIAL_RXC_vect USARTC0_RXC_vect
-
-
-// Input
-#define INPUT_BUFFER_LEN 255 // text buffer size (255 max)
-
-
-// Arc
-#define ARC_RADIUS_ERROR_MAX 1.0 // max mm diff between start and end radius
-#define ARC_RADIUS_ERROR_MIN 0.005 // min mm where 1% rule applies
-#define ARC_RADIUS_TOLERANCE 0.001 // 0.1% radius variance test
-
-
-// Planner
-/// Should be at least the number of buffers required to support optimal
-/// planning in the case of very short lines or arc segments. Suggest no less
-/// than 12. Maximum is 255 with out also changing the type of mb.space. Must
-/// leave headroom for stack.
-#define PLANNER_BUFFER_POOL_SIZE 32
-
-/// Buffers to reserve in planner before processing new input line
-#define PLANNER_BUFFER_HEADROOM 4
-
-/// Minimum number of filled buffers before timeout until execution starts
-#define PLANNER_EXEC_MIN_FILL 4
-
-/// Delay before executing new buffers unless PLANNER_EXEC_MIN_FILL is met
-/// This gives the planner a chance to make a good plan before execution starts
-#define PLANNER_EXEC_DELAY 250 // In ms
-
-
-// I2C
-#define I2C_DEV TWIC
-#define I2C_ISR TWIC_TWIS_vect
-#define I2C_ADDR 0x2b
-#define I2C_MAX_DATA 8
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2016 Buildbotics LLC
- All rights reserved.
-
- 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/>.
-
- The software is distributed in the hope that it will be useful, but
- WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
- Lesser General Public License for more details.
-
- You should have received a copy of the GNU Lesser General Public
- License along with the software. If not, see
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-#include "coolant.h"
-#include "config.h"
-
-#include <avr/io.h>
-
-
-void coolant_init() {
- OUTSET_PIN(SWITCH_1_PIN); // High
- DIRSET_PIN(SWITCH_1_PIN); // Output
- OUTSET_PIN(SWITCH_2_PIN); // High
- DIRSET_PIN(SWITCH_2_PIN); // Output
-}
-
-
-void coolant_set_mist(bool x) {
- if (x) OUTCLR_PIN(SWITCH_1_PIN);
- else OUTSET_PIN(SWITCH_1_PIN);
-}
-
-
-void coolant_set_flood(bool x) {
- if (x) OUTCLR_PIN(SWITCH_2_PIN);
- else OUTSET_PIN(SWITCH_2_PIN);
-}
-
-
-bool coolant_get_mist() {return OUT_PIN(SWITCH_1_PIN);}
-bool coolant_get_flood() {return OUT_PIN(SWITCH_2_PIN);}
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2016 Buildbotics LLC
- All rights reserved.
-
- 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/>.
-
- The software is distributed in the hope that it will be useful, but
- WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
- Lesser General Public License for more details.
-
- You should have received a copy of the GNU Lesser General Public
- License along with the software. If not, see
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-#pragma once
-
-#include <stdbool.h>
-
-
-void coolant_init();
-void coolant_set_mist(bool x);
-void coolant_set_flood(bool x);
-bool coolant_get_mist();
-bool coolant_get_flood();
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the uSHET library.
- See https://github.com/18sg/uSHET
-
- Copyright (c) 2014 Thomas Nixon, Jonathan Heathcote
- All rights reserved.
-
- Permission is hereby granted, free of charge, to any person obtaining
- a copy of this software and associated documentation files (the
- "Software"), to deal in the Software without restriction, including
- without limitation the rights to use, copy, modify, merge, publish,
- distribute, sublicense, and/or sell copies of the Software, and to
- permit persons to whom the Software is furnished to do so, subject to
- the following conditions:
-
- The above copyright notice and this permission notice shall be
- included in all copies or substantial portions of the Software.
-
- THE SOFTWARE IS PROVIDED "AS IS", WITHOUT 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.
-
-\******************************************************************************/
-
-/* This header file contains a library of advanced C Pre-Processor (CPP) macros
- * which implement various useful functions, such as iteration, in the
- * pre-processor.
- *
- * Though the file name (quite validly) labels this as magic, there should be
- * enough documentation in the comments for a reader only casually familiar
- * with the CPP to be able to understand how everything works.
- *
- * The majority of the magic tricks used in this file are based on those
- * described by pfultz2 in his "Cloak" library:
- *
- * https://github.com/pfultz2/Cloak/wiki/C-Preprocessor-tricks,-tips,-and-idioms
- *
- * Major differences are a greater level of detailed explanation in this
- * implementation and also a refusal to include any macros which require a O(N)
- * macro definitions to handle O(N) arguments (with the exception of DEFERn).
- */
-
-#pragma once
-
-/**
- * Force the pre-processor to expand the macro a large number of times.
- * Usage:
- *
- * EVAL(expression)
- *
- * This is useful when you have a macro which evaluates to a valid
- * macro expression which is not subsequently expanded in the same
- * pass. A contrived, but easy to understand, example of such a macro
- * follows. Note that though this example is contrived, this behaviour
- * is abused to implement bounded recursion in macros such as FOR.
- *
- * #define A(x) x+1
- * #define EMPTY
- * #define NOT_QUITE_RIGHT(x) A EMPTY (x)
- * NOT_QUITE_RIGHT(999)
- *
- * Here's what happens inside the C preprocessor:
- *
- * 1. It sees a macro "NOT_QUITE_RIGHT" and performs a single macro
- * expansion pass on its arguments. Since the argument is "999" and
- * this isn't a macro, this is a boring step resulting in no
- * change.
- *
- * 2. The NOT_QUITE_RIGHT macro is substituted for its definition
- * giving "A EMPTY() (x)".
- *
- * 3. The expander moves from left-to-right trying to expand the
- * macro: The first token, A, cannot be expanded since there are no
- * brackets immediately following it. The second token EMPTY(),
- * however, can be expanded (recursively in this manner) and is
- * replaced with "".
- *
- * 4. Expansion continues from the start of the substituted test
- * (which in this case is just empty), and sees "(999)" but since
- * no macro name is present, nothing is done. This results in a
- * final expansion of "A (999)".
- *
- * Unfortunately, this doesn't quite meet expectations since you may
- * expect that "A (999)" would have been expanded into
- * "999+1". Unfortunately this requires a second expansion pass but
- * luckily we can force the macro processor to make more passes by
- * abusing the first step of macro expansion: the preprocessor expands
- * arguments in their own pass. If we define a macro which does
- * nothing except produce its arguments e.g.:
- *
- * #define PASS_THROUGH(...) __VA_ARGS__
- *
- * We can now do "PASS_THROUGH(NOT_QUITE_RIGHT(999))" causing
- * "NOT_QUITE_RIGHT" to be expanded to "A (999)", as described above,
- * when the arguments are expanded. Now when the body of PASS_THROUGH
- * is expanded, "A (999)" gets expanded to "999+1".
- *
- * The EVAL defined below is essentially equivalent to a large nesting
- * of "PASS_THROUGH(PASS_THROUGH(PASS_THROUGH(..." which results in
- * the preprocessor making a large number of expansion passes over the
- * given expression.
- */
-#define EVAL(...) EVAL1024(__VA_ARGS__)
-#define EVAL1024(...) EVAL512(EVAL512(__VA_ARGS__))
-#define EVAL512(...) EVAL256(EVAL256(__VA_ARGS__))
-#define EVAL256(...) EVAL128(EVAL128(__VA_ARGS__))
-#define EVAL128(...) EVAL64(EVAL64(__VA_ARGS__))
-#define EVAL64(...) EVAL32(EVAL32(__VA_ARGS__))
-#define EVAL32(...) EVAL16(EVAL16(__VA_ARGS__))
-#define EVAL16(...) EVAL8(EVAL8(__VA_ARGS__))
-#define EVAL8(...) EVAL4(EVAL4(__VA_ARGS__))
-#define EVAL4(...) EVAL2(EVAL2(__VA_ARGS__))
-#define EVAL2(...) EVAL1(EVAL1(__VA_ARGS__))
-#define EVAL1(...) __VA_ARGS__
-
-
-// Macros which expand to common values
-#define PASS(...) __VA_ARGS__
-#define EMPTY()
-#define COMMA() ,
-#define SEMI() ;
-#define PLUS() +
-#define ZERO() 0
-#define ONE() 1
-
-/**
- * Causes a function-style macro to require an additional pass to be expanded.
- *
- * This is useful, for example, when trying to implement recursion since the
- * recursive step must not be expanded in a single pass as the pre-processor
- * will catch it and prevent it.
- *
- * Usage:
- *
- * DEFER1(IN_NEXT_PASS)(args, to, the, macro)
- *
- * How it works:
- *
- * 1. When DEFER1 is expanded, first its arguments are expanded which are
- * simply IN_NEXT_PASS. Since this is a function-style macro and it has no
- * arguments, nothing will happen.
- * 2. The body of DEFER1 will now be expanded resulting in EMPTY() being
- * deleted. This results in "IN_NEXT_PASS (args, to, the macro)". Note that
- * since the macro expander has already passed IN_NEXT_PASS by the time it
- * expands EMPTY() and so it won't spot that the brackets which remain can be
- * applied to IN_NEXT_PASS.
- * 3. At this point the macro expansion completes. If one more pass is made,
- * IN_NEXT_PASS(args, to, the, macro) will be expanded as desired.
- */
-#define DEFER1(id) id EMPTY()
-
-/**
- * As with DEFER1 except here n additional passes are required for DEFERn.
- *
- * The mechanism is analogous.
- *
- * Note that there doesn't appear to be a way of combining DEFERn macros in
- * order to achieve exponentially increasing defers e.g. as is done by EVAL.
- */
-#define DEFER2(id) id EMPTY EMPTY()()
-#define DEFER3(id) id EMPTY EMPTY EMPTY()()()
-#define DEFER4(id) id EMPTY EMPTY EMPTY EMPTY()()()()
-#define DEFER5(id) id EMPTY EMPTY EMPTY EMPTY EMPTY()()()()()
-#define DEFER6(id) id EMPTY EMPTY EMPTY EMPTY EMPTY EMPTY()()()()()()
-#define DEFER7(id) id EMPTY EMPTY EMPTY EMPTY EMPTY EMPTY EMPTY()()()()()()()
-#define DEFER8(id) \
- id EMPTY EMPTY EMPTY EMPTY EMPTY EMPTY EMPTY EMPTY()()()()()()()()
-
-
-/**
- * Indirection around the standard ## concatenation operator. This simply
- * ensures that the arguments are expanded (once) before concatenation.
- */
-#define CAT(a, ...) a ## __VA_ARGS__
-#define CAT3(a, b, ...) a ## b ## __VA_ARGS__
-
-
-/**
- * Get the first argument and ignore the rest.
- */
-#define FIRST(a, ...) a
-
-/**
- * Get the second argument and ignore the rest.
- */
-#define SECOND(a, b, ...) b
-
-/**
- * Expects a single input (not containing commas). Returns 1 if the input is
- * PROBE() and otherwise returns 0.
- *
- * This can be useful as the basis of a NOT function.
- *
- * This macro abuses the fact that PROBE() contains a comma while other valid
- * inputs must not.
- */
-#define IS_PROBE(...) SECOND(__VA_ARGS__, 0)
-#define PROBE() ~, 1
-
-
-/**
- * Logical negation. 0 is defined as false and everything else as true.
- *
- * When 0, _NOT_0 will be found which evaluates to the PROBE. When 1
- * (or any other value) is given, an appropriately named macro won't
- * be found and the concatenated string will be produced. IS_PROBE
- * then simply checks to see if the PROBE was returned, cleanly
- * converting the argument into a 1 or 0.
- */
-#define NOT(x) IS_PROBE(CAT(_NOT_, x))
-#define _NOT_0 PROBE()
-
-/**
- * Macro version of C's famous "cast to bool" operator (i.e. !!) which takes
- * anything and casts it to 0 if it is 0 and 1 otherwise.
- */
-#define BOOL(x) NOT(NOT(x))
-
-/**
- * Logical OR. Simply performs a lookup.
- */
-#define OR(a,b) CAT3(_OR_, a, b)
-#define _OR_00 0
-#define _OR_01 1
-#define _OR_10 1
-#define _OR_11 1
-
-/**
- * Logical AND. Simply performs a lookup.
- */
-#define AND(a,b) CAT3(_AND_, a, b)
-#define _AND_00 0
-#define _AND_01 0
-#define _AND_10 0
-#define _AND_11 1
-
-
-/**
- * Macro if statement. Usage:
- *
- * IF(c)(expansion when true)
- *
- * Here's how:
- *
- * 1. The preprocessor expands the arguments to _IF casting the condition to '0'
- * or '1'.
- * 2. The casted condition is concatencated with _IF_ giving _IF_0 or _IF_1.
- * 3. The _IF_0 and _IF_1 macros either returns the argument or doesn't (e.g.
- * they implement the "choice selection" part of the macro).
- * 4. Note that the "true" clause is in the extra set of brackets; thus these
- * become the arguments to _IF_0 or _IF_1 and thus a selection is made!
- */
-#define IF(c) _IF(BOOL(c))
-#define _IF(c) CAT(_IF_,c)
-#define _IF_0(...)
-#define _IF_1(...) __VA_ARGS__
-
-/**
- * Macro if/else statement. Usage:
- *
- * IF_ELSE(c)( \
- * expansion when true, \
- * expansion when false \
- * )
- *
- * The mechanism is analogous to IF.
- */
-#define IF_ELSE(c) _IF_ELSE(BOOL(c))
-#define _IF_ELSE(c) CAT(_IF_ELSE_,c)
-#define _IF_ELSE_0(t,f) f
-#define _IF_ELSE_1(t,f) t
-
-
-/**
- * Macro which checks if it has any arguments. Returns '0' if there are no
- * arguments, '1' otherwise.
- *
- * Limitation: HAS_ARGS(,1,2,3) returns 0 -- this check essentially only checks
- * that the first argument exists.
- *
- * This macro works as follows:
- *
- * 1. _END_OF_ARGUMENTS_ is concatenated with the first argument.
- * 2. If the first argument is not present then only "_END_OF_ARGUMENTS_" will
- * remain, otherwise "_END_OF_ARGUMENTS something_here" will remain.
- * 3. In the former case, the _END_OF_ARGUMENTS_() macro expands to a
- * 0 when it is expanded. In the latter, a non-zero result remains.
- * 4. BOOL is used to force non-zero results into 1 giving the clean 0 or 1
- * output required.
- */
-#define HAS_ARGS(...) BOOL(FIRST(_END_OF_ARGUMENTS_ __VA_ARGS__)())
-#define _END_OF_ARGUMENTS_() 0
-
-
-/**
- * Macro map/list comprehension. Usage:
- *
- * MAP(op, sep, ...)
- *
- * Produces a 'sep()'-separated list of the result of op(arg) for each arg.
- *
- * Example Usage:
- *
- * #define MAKE_HAPPY(x) happy_##x
- * #define COMMA() ,
- * MAP(MAKE_HAPPY, COMMA, 1,2,3)
- *
- * Which expands to:
- *
- * happy_1 , happy_2 , happy_3
- *
- * How it works:
- *
- * 1. The MAP macro simply maps the inner MAP_INNER function in an EVAL which
- * forces it to be expanded a large number of times, thus enabling many steps
- * of iteration (see step 6).
- * 2. The MAP_INNER macro is substituted for its body.
- * 3. In the body, op(cur_val) is substituted giving the value for this
- * iteration.
- * 4. The IF macro expands according to whether further iterations are required.
- * This expansion either produces _IF_0 or _IF_1.
- * 5. Since the IF is followed by a set of brackets containing the "if true"
- * clause, these become the argument to _IF_0 or _IF_1. At this point, the
- * macro in the brackets will be expanded giving the separator followed by
- * _MAP_INNER EMPTY()()(op, sep, __VA_ARGS__).
- * 5. If the IF was not taken, the above will simply be discarded and everything
- * stops. If the IF is taken, The expression is then processed a second time
- * yielding "_MAP_INNER()(op, sep, __VA_ARGS__)". Note that this call looks
- * very similar to the essentially the same as the original call except the
- * first argument has been dropped.
- * 6. At this point expansion of MAP_INNER will terminate. However, since we can
- * force more rounds of expansion using EVAL1. In the argument-expansion pass
- * of the EVAL1, _MAP_INNER() is expanded to MAP_INNER which is then expanded
- * using the arguments which follow it as in step 2-5. This is followed by a
- * second expansion pass as the substitution of EVAL1() is expanded executing
- * 2-5 a second time. This results in up to two iterations occurring. Using
- * many nested EVAL1 macros, i.e. the very-deeply-nested EVAL macro, will in
- * this manner produce further iterations, hence the outer MAP macro doing
- * this for us.
- *
- * Important tricks used:
- *
- * * If we directly produce "MAP_INNER" in an expansion of MAP_INNER,
- * a special case in the preprocessor will prevent it being expanded
- * in the future, even if we EVAL. As a result, the MAP_INNER macro
- * carefully only expands to something containing "_MAP_INNER()"
- * which requires a further expansion step to invoke MAP_INNER and
- * thus implementing the recursion.
- *
- * * To prevent _MAP_INNER being expanded within the macro we must
- * first defer its expansion during its initial pass as an argument
- * to _IF_0 or _IF_1. We must then defer its expansion a second time
- * as part of the body of the _IF_0. As a result hence the DEFER2.
- *
- * * _MAP_INNER seemingly gets away with producing itself because it
- * actually only produces MAP_INNER. It just happens that when
- * _MAP_INNER() is expanded in this case it is followed by some
- * arguments which get consumed by MAP_INNER and produce a
- * _MAP_INNER. As such, the macro expander never marks _MAP_INNER
- * as expanding to itself and thus it will still be expanded in
- * future productions of itself.
- */
-#define MAP(...) \
- IF(HAS_ARGS(__VA_ARGS__))(EVAL(MAP_INNER(__VA_ARGS__)))
-#define MAP_INNER(op,sep,cur_val, ...) \
- op(cur_val) \
- IF(HAS_ARGS(__VA_ARGS__))( \
- sep() DEFER2(_MAP_INNER)()(op, sep, ##__VA_ARGS__) \
- )
-#define _MAP_INNER() MAP_INNER
-
-
-/**
- * This is a variant of the MAP macro which also includes as an argument to the
- * operation a valid C variable name which is different for each iteration.
- *
- * Usage:
- * MAP_WITH_ID(op, sep, ...)
- *
- * Where op is a macro op(val, id) which takes a list value and an ID. This ID
- * will simply be a unary number using the digit "I", that is, I, II, III, IIII,
- * and so on.
- *
- * Example:
- *
- * #define MAKE_STATIC_VAR(type, name) static type name;
- * MAP_WITH_ID(MAKE_STATIC_VAR, EMPTY, int, int, int, bool, char)
- *
- * Which expands to:
- *
- * static int I; static int II; static int III; static bool IIII;
- * static char IIIII;
- *
- * The mechanism is analogous to the MAP macro.
- */
-#define MAP_WITH_ID(op,sep,...) \
- IF(HAS_ARGS(__VA_ARGS__))(EVAL(MAP_WITH_ID_INNER(op,sep,I, ##__VA_ARGS__)))
-#define MAP_WITH_ID_INNER(op,sep,id,cur_val, ...) \
- op(cur_val,id) \
- IF(HAS_ARGS(__VA_ARGS__))( \
- sep() DEFER2(_MAP_WITH_ID_INNER)()(op, sep, CAT(id,I), ##__VA_ARGS__) \
- )
-#define _MAP_WITH_ID_INNER() MAP_WITH_ID_INNER
-
-
-/**
- * This is a variant of the MAP macro which iterates over pairs rather than
- * singletons.
- *
- * Usage:
- * MAP_PAIRS(op, sep, ...)
- *
- * Where op is a macro op(val_1, val_2) which takes two list values.
- *
- * Example:
- *
- * #define MAKE_STATIC_VAR(type, name) static type name;
- * MAP_PAIRS(MAKE_STATIC_VAR, EMPTY, char, my_char, int, my_int)
- *
- * Which expands to:
- *
- * static char my_char; static int my_int;
- *
- * The mechanism is analogous to the MAP macro.
- */
-#define MAP_PAIRS(op,sep,...) \
- IF(HAS_ARGS(__VA_ARGS__))(EVAL(MAP_PAIRS_INNER(op,sep,__VA_ARGS__)))
-#define MAP_PAIRS_INNER(op,sep,cur_val_1, cur_val_2, ...) \
- op(cur_val_1,cur_val_2) \
- IF(HAS_ARGS(__VA_ARGS__))( \
- sep() DEFER2(_MAP_PAIRS_INNER)()(op, sep, __VA_ARGS__) \
- )
-#define _MAP_PAIRS_INNER() MAP_PAIRS_INNER
-
-/**
- * This is a variant of the MAP macro which iterates over a two-element sliding
- * window.
- *
- * Usage:
- * MAP_SLIDE(op, last_op, sep, ...)
- *
- * Where op is a macro op(val_1, val_2) which takes the two list values
- * currently in the window. last_op is a macro taking a single value which is
- * called for the last argument.
- *
- * Example:
- *
- * #define SIMON_SAYS_OP(simon, next) IF(NOT(simon()))(next)
- * #define SIMON_SAYS_LAST_OP(val) last_but_not_least_##val
- * #define SIMON_SAYS() 0
- *
- * MAP_SLIDE(SIMON_SAYS_OP, SIMON_SAYS_LAST_OP, EMPTY, wiggle, SIMON_SAYS,
- * dance, move, SIMON_SAYS, boogie, stop)
- *
- * Which expands to:
- *
- * dance boogie last_but_not_least_stop
- *
- * The mechanism is analogous to the MAP macro.
- */
-#define MAP_SLIDE(op,last_op,sep,...) \
- IF(HAS_ARGS(__VA_ARGS__))(EVAL(MAP_SLIDE_INNER(op,last_op,sep,__VA_ARGS__)))
-#define MAP_SLIDE_INNER(op,last_op,sep,cur_val, ...) \
- IF(HAS_ARGS(__VA_ARGS__))(op(cur_val,FIRST(__VA_ARGS__))) \
- IF(NOT(HAS_ARGS(__VA_ARGS__)))(last_op(cur_val)) \
- IF(HAS_ARGS(__VA_ARGS__))( \
- sep() DEFER2(_MAP_SLIDE_INNER)()(op, last_op, sep, __VA_ARGS__) \
- )
-#define _MAP_SLIDE_INNER() MAP_SLIDE_INNER
-
-
-/**
- * Strip any excess commas from a set of arguments.
- */
-#define REMOVE_TRAILING_COMMAS(...) \
- MAP(PASS, COMMA, __VA_ARGS__)
-
-
-/**
- * Evaluates an array of macros passing 1 argument
- */
-#define EMAP1(...) \
- IF(HAS_ARGS(__VA_ARGS__))(EVAL(EMAP1_INNER(__VA_ARGS__)))
-
-#define EMAP1_INNER(ARG1, OP, ...) \
- _##OP(ARG1) \
- IF(HAS_ARGS(__VA_ARGS__)) \
- (DEFER2(_EMAP1_INNER)()(ARG1, ##__VA_ARGS__))
-
-#define _EMAP1_INNER() EMAP1_INNER
-
-
-/**
- * Evaluates an array of macros passing 2 arguments
- */
-#define EMAP2(...) \
- IF(HAS_ARGS(__VA_ARGS__))(EVAL(EMAP2_INNER(__VA_ARGS__)))
-
-#define EMAP2_INNER(ARG1, ARG2, OP, ...) \
- _##OP(ARG1, ARG2) \
- IF(HAS_ARGS(__VA_ARGS__)) \
- (DEFER2(_EMAP2_INNER)()(ARG1, ARG2, ##__VA_ARGS__))
-
-#define _EMAP2_INNER() EMAP2_INNER
-
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2016 Buildbotics LLC
- All rights reserved.
-
- 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/>.
-
- The software is distributed in the hope that it will be useful, but
- WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
- Lesser General Public License for more details.
-
- You should have received a copy of the GNU Lesser General Public
- License along with the software. If not, see
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-#include "drv8711.h"
-#include "status.h"
-#include "motor.h"
-
-#include <avr/interrupt.h>
-#include <util/delay.h>
-
-#include <string.h>
-#include <stdlib.h>
-#include <stdio.h>
-
-
-#define DRIVERS MOTORS
-#define COMMANDS 10
-
-
-#define DRV8711_WORD_BYTE_PTR(WORD, LOW) \
- (((uint8_t *)&(WORD)) + ((LOW) ? 0 : 1))
-
-
-bool motor_fault = false;
-
-
-typedef struct {
- uint8_t status;
-
- bool active;
- float idle_current;
- float drive_current;
- float stall_threshold;
- float power;
-
- uint8_t mode; // microstepping mode
- stall_callback_t stall_cb;
-
- uint8_t cs_pin;
- uint8_t stall_pin;
-} drv8711_driver_t;
-
-
-static drv8711_driver_t drivers[DRIVERS] = {
- {.cs_pin = SPI_CS_X_PIN, .stall_pin = STALL_X_PIN},
- {.cs_pin = SPI_CS_Y_PIN, .stall_pin = STALL_Y_PIN},
- {.cs_pin = SPI_CS_Z_PIN, .stall_pin = STALL_Z_PIN},
- {.cs_pin = SPI_CS_A_PIN, .stall_pin = STALL_A_PIN},
-};
-
-
-typedef struct {
- uint8_t *read;
- bool callback;
- uint8_t disable_cs_pin;
-
- uint8_t cmd;
- uint8_t driver;
- bool low_byte;
-
- uint8_t ncmds;
- uint16_t commands[DRIVERS][COMMANDS];
- uint16_t responses[DRIVERS];
-} spi_t;
-
-static spi_t spi = {0};
-
-
-static void _driver_check_status(int driver) {
- uint8_t status = drivers[driver].status;
- uint8_t mflags = 0;
-
- if (status & DRV8711_STATUS_OTS_bm) mflags |= MOTOR_FLAG_OVER_TEMP_bm;
- if (status & DRV8711_STATUS_AOCP_bm) mflags |= MOTOR_FLAG_OVER_CURRENT_bm;
- if (status & DRV8711_STATUS_BOCP_bm) mflags |= MOTOR_FLAG_OVER_CURRENT_bm;
- if (status & DRV8711_STATUS_APDF_bm) mflags |= MOTOR_FLAG_DRIVER_FAULT_bm;
- if (status & DRV8711_STATUS_BPDF_bm) mflags |= MOTOR_FLAG_DRIVER_FAULT_bm;
- if (status & DRV8711_STATUS_UVLO_bm) mflags |= MOTOR_FLAG_UNDER_VOLTAGE_bm;
- if (status & DRV8711_STATUS_STD_bm) mflags |= MOTOR_FLAG_STALLED_bm;
- if (status & DRV8711_STATUS_STDLAT_bm) mflags |= MOTOR_FLAG_STALLED_bm;
-
- //if (mflags) motor_error_callback(driver, mflags); TODO
-}
-
-
-static float _driver_get_current(int driver) {
- drv8711_driver_t *drv = &drivers[driver];
-
-#if 1
- if (!drv->active) return drv->idle_current;
- return
- MOTOR_MIN_CURRENT + (drv->drive_current - MOTOR_MIN_CURRENT) * drv->power;
-
-#else
- return drv->active ? drv->drive_current : drv->idle_current;
-#endif
-}
-
-
-static uint8_t _spi_next_command(uint8_t cmd) {
- // Process status responses
- for (int driver = 0; driver < DRIVERS; driver++) {
- uint16_t command = spi.commands[driver][cmd];
-
- if (DRV8711_CMD_IS_READ(command) &&
- DRV8711_CMD_ADDR(command) == DRV8711_STATUS_REG) {
- drivers[driver].status = spi.responses[driver];
- _driver_check_status(driver);
- }
- }
-
- // Next command
- if (++cmd == spi.ncmds) {
- cmd = 0; // Wrap around
-
- for (int driver = 0; driver < DRIVERS; driver++)
- motor_driver_callback(driver);
- }
-
- // Prep next command
- for (int driver = 0; driver < DRIVERS; driver++) {
- uint16_t *command = &spi.commands[driver][cmd];
-
- switch (DRV8711_CMD_ADDR(*command)) {
- case DRV8711_STATUS_REG:
- if (!DRV8711_CMD_IS_READ(*command))
- *command = (*command & 0xf000) | (0x0fff & ~(drivers[driver].status));
- break;
-
- case DRV8711_TORQUE_REG: // Update motor current setting
- *command = (*command & 0xff00) |
- (uint8_t)round(0xff * _driver_get_current(driver));
- break;
-
- case DRV8711_CTRL_REG: // Set microsteps
- *command = (*command & 0xff87) | (drivers[driver].mode << 3);
- break;
-
- default: break;
- }
- }
-
- return cmd;
-}
-
-
-static void _spi_send() {
- // Flush any status errors (TODO check for errors)
- uint8_t x = SPIC.STATUS;
- x = x;
-
- // Disable CS
- if (spi.disable_cs_pin) {
- OUTCLR_PIN(spi.disable_cs_pin); // Set low (inactive)
- _delay_us(1);
- spi.disable_cs_pin = 0;
- }
-
- // Schedule next CS disable or enable next CS now
- if (spi.low_byte) spi.disable_cs_pin = drivers[spi.driver].cs_pin;
- else {
- OUTSET_PIN(drivers[spi.driver].cs_pin); // Set high (active)
- _delay_us(1);
- }
-
- // Read
- if (spi.read) {
- *spi.read = SPIC.DATA;
- spi.read = 0;
- }
-
- // Callback, passing current command index, and get next command index
- if (spi.callback) {
- spi.cmd = _spi_next_command(spi.cmd);
- spi.callback = false;
- }
-
- // Write byte and prep next read
- SPIC.DATA =
- *DRV8711_WORD_BYTE_PTR(spi.commands[spi.driver][spi.cmd], spi.low_byte);
- spi.read = DRV8711_WORD_BYTE_PTR(spi.responses[spi.driver], spi.low_byte);
-
- // Check if WORD complete, go to next driver & check if command finished
- if (spi.low_byte && ++spi.driver == DRIVERS) {
- spi.driver = 0; // Wrap around
- spi.callback = true; // Call back after last byte is read
- }
-
- // Next byte
- spi.low_byte = !spi.low_byte;
-}
-
-
-static void _init_spi_commands() {
- // Setup SPI command sequence
- for (int driver = 0; driver < DRIVERS; driver++) {
- uint16_t *commands = spi.commands[driver];
- spi.ncmds = 0;
-
- // Set OFF
- commands[spi.ncmds++] = DRV8711_WRITE(DRV8711_OFF_REG, 12);
-
- // Set BLANK
- commands[spi.ncmds++] = DRV8711_WRITE(DRV8711_BLANK_REG, 0x80);
-
- // Set DECAY
- commands[spi.ncmds++] =
- DRV8711_WRITE(DRV8711_DECAY_REG, DRV8711_DECAY_DECMOD_AUTO_OPT | 6);
-
- // Set STALL
- commands[spi.ncmds++] =
- DRV8711_WRITE(DRV8711_STALL_REG,
- DRV8711_STALL_SDCNT_2 | DRV8711_STALL_VDIV_8 | 200);
-
- // Set DRIVE
- commands[spi.ncmds++] =
- DRV8711_WRITE(DRV8711_DRIVE_REG, DRV8711_DRIVE_IDRIVEP_50 |
- DRV8711_DRIVE_IDRIVEN_100 | DRV8711_DRIVE_TDRIVEP_500 |
- DRV8711_DRIVE_TDRIVEN_500 | DRV8711_DRIVE_OCPDEG_2 |
- DRV8711_DRIVE_OCPTH_500);
-
- // Set TORQUE
- commands[spi.ncmds++] =
- DRV8711_WRITE(DRV8711_TORQUE_REG, DRV8711_TORQUE_SMPLTH_50);
-
- // Set CTRL enable motor & set ISENSE gain
- commands[spi.ncmds++] =
- DRV8711_WRITE(DRV8711_CTRL_REG, DRV8711_CTRL_ENBL_bm |
- DRV8711_CTRL_ISGAIN_10 | DRV8711_CTRL_DTIME_850);
-
- // Read STATUS
- commands[spi.ncmds++] = DRV8711_READ(DRV8711_STATUS_REG);
-
- // Clear STATUS
- commands[spi.ncmds++] = DRV8711_WRITE(DRV8711_STATUS_REG, 0);
- }
-
- if (COMMANDS < spi.ncmds)
- STATUS_ERROR(STAT_INTERNAL_ERROR,
- "SPI command buffer overflow increase COMMANDS in %s",
- __FILE__);
-
- _spi_send(); // Kick it off
-}
-
-
-ISR(SPIC_INT_vect) {_spi_send();}
-
-
-ISR(STALL_ISR_vect) {
- for (int i = 0; i < DRIVERS; i++) {
- drv8711_driver_t *driver = &drivers[i];
- if (driver->stall_cb) driver->stall_cb(i);
- }
-}
-
-
-ISR(FAULT_ISR_vect) {motor_fault = !IN_PIN(MOTOR_FAULT_PIN);} // TODO
-
-
-void drv8711_init() {
- // Configure drivers
- for (int i = 0; i < DRIVERS; i++) {
- drivers[i].idle_current = MOTOR_IDLE_CURRENT;
- drivers[i].drive_current = MOTOR_MAX_CURRENT;
- drivers[i].stall_threshold = MOTOR_STALL_THRESHOLD;
-
- drv8711_disable(i);
- }
-
- // Setup pins
- // Must set the SS pin either in/high or any/output for master mode to work
- // Note, this pin is also used by the USART as the CTS line
- DIRSET_PIN(SPI_SS_PIN); // Output
- OUTSET_PIN(SPI_CLK_PIN); // High
- DIRSET_PIN(SPI_CLK_PIN); // Output
- DIRCLR_PIN(SPI_MISO_PIN); // Input
- OUTSET_PIN(SPI_MOSI_PIN); // High
- DIRSET_PIN(SPI_MOSI_PIN); // Output
-
- for (int i = 0; i < DRIVERS; i++) {
- uint8_t cs_pin = drivers[i].cs_pin;
- uint8_t stall_pin = drivers[i].stall_pin;
-
- OUTSET_PIN(cs_pin); // High
- DIRSET_PIN(cs_pin); // Output
- DIRCLR_PIN(stall_pin); // Input
-
- // Stall interrupt
- PINCTRL_PIN(stall_pin) = PORT_ISC_FALLING_gc;
- PORT(stall_pin)->INT1MASK |= BM(stall_pin);
- PORT(stall_pin)->INTCTRL |= PORT_INT1LVL_HI_gc;
- }
-
- // Fault interrupt
- DIRCLR_PIN(MOTOR_FAULT_PIN);
- PINCTRL_PIN(MOTOR_FAULT_PIN) = PORT_ISC_RISING_gc;
- PORT(MOTOR_FAULT_PIN)->INT1MASK |= BM(MOTOR_FAULT_PIN);
- PORT(MOTOR_FAULT_PIN)->INTCTRL |= PORT_INT1LVL_HI_gc;
-
- // Configure SPI
- PR.PRPC &= ~PR_SPI_bm; // Disable power reduction
- SPIC.CTRL = SPI_ENABLE_bm | SPI_MASTER_bm | SPI_MODE_0_gc |
- SPI_PRESCALER_DIV16_gc; // enable, big endian, master, mode, clock div
- PORT(SPI_CLK_PIN)->REMAP = PORT_SPI_bm; // Swap SCK and MOSI
- SPIC.INTCTRL = SPI_INTLVL_LO_gc; // interupt level
-
- _init_spi_commands();
-}
-
-
-void drv8711_enable(int driver) {
- if (driver < 0 || DRIVERS <= driver) return;
- drivers[driver].active = true;
-}
-
-
-void drv8711_disable(int driver) {
- if (driver < 0 || DRIVERS <= driver) return;
- drivers[driver].active = false;
-}
-
-
-void drv8711_set_power(int driver, float power) {
- if (driver < 0 || DRIVERS <= driver) return;
- drivers[driver].power = power < 0 ? 0 : (1 < power ? 1 : power);
-}
-
-
-void drv8711_set_microsteps(int driver, uint16_t msteps) {
- if (driver < 0 || DRIVERS <= driver) return;
- switch (msteps) {
- case 1: case 2: case 4: case 8: case 16: case 32: case 64: case 128: case 256:
- break;
- default: return; // Invalid
- }
-
- drivers[driver].mode = round(logf(msteps) / logf(2));
-}
-
-
-void drv8711_set_stall_callback(int driver, stall_callback_t cb) {
- drivers[driver].stall_cb = cb;
-}
-
-
-float get_drive_power(int driver) {
- if (driver < 0 || DRIVERS <= driver) return 0;
- return drivers[driver].drive_current;
-}
-
-
-void set_drive_power(int driver, float value) {
- if (driver < 0 || DRIVERS <= driver || value < 0 || 1 < value) return;
- drivers[driver].drive_current = value;
-}
-
-
-float get_idle_power(int driver) {
- if (driver < 0 || DRIVERS <= driver) return 0;
- return drivers[driver].idle_current;
-}
-
-
-void set_idle_power(int driver, float value) {
- if (driver < 0 || DRIVERS <= driver || value < 0 || 1 < value) return;
- drivers[driver].idle_current = value;
-}
-
-
-float get_current_power(int driver) {
- if (driver < 0 || DRIVERS <= driver) return 0;
- return _driver_get_current(driver);
-}
-
-
-bool get_motor_fault() {return motor_fault;}
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2016 Buildbotics LLC
- All rights reserved.
-
- 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/>.
-
- The software is distributed in the hope that it will be useful, but
- WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
- Lesser General Public License for more details.
-
- You should have received a copy of the GNU Lesser General Public
- License along with the software. If not, see
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-#pragma once
-
-
-#include "config.h"
-#include "status.h"
-#include "motor.h"
-
-#include <stdint.h>
-#include <stdbool.h>
-
-enum {
- DRV8711_CTRL_REG,
- DRV8711_TORQUE_REG,
- DRV8711_OFF_REG,
- DRV8711_BLANK_REG,
- DRV8711_DECAY_REG,
- DRV8711_STALL_REG,
- DRV8711_DRIVE_REG,
- DRV8711_STATUS_REG,
-};
-
-
-enum {
- DRV8711_CTRL_ENBL_bm = 1 << 0,
- DRV8711_CTRL_RDIR_bm = 1 << 1,
- DRV8711_CTRL_RSTEP_bm = 1 << 2,
- DRV8711_CTRL_MODE_1 = 0 << 3,
- DRV8711_CTRL_MODE_2 = 1 << 3,
- DRV8711_CTRL_MODE_4 = 2 << 3,
- DRV8711_CTRL_MODE_8 = 3 << 3,
- DRV8711_CTRL_MODE_16 = 4 << 3,
- DRV8711_CTRL_MODE_32 = 5 << 3,
- DRV8711_CTRL_MODE_64 = 6 << 3,
- DRV8711_CTRL_MODE_128 = 7 << 3,
- DRV8711_CTRL_MODE_256 = 8 << 3,
- DRV8711_CTRL_EXSTALL_bm = 1 << 7,
- DRV8711_CTRL_ISGAIN_5 = 0 << 8,
- DRV8711_CTRL_ISGAIN_10 = 1 << 8,
- DRV8711_CTRL_ISGAIN_20 = 2 << 8,
- DRV8711_CTRL_ISGAIN_40 = 3 << 8,
- DRV8711_CTRL_DTIME_400 = 0 << 10,
- DRV8711_CTRL_DTIME_450 = 1 << 10,
- DRV8711_CTRL_DTIME_650 = 2 << 10,
- DRV8711_CTRL_DTIME_850 = 3 << 10,
-};
-
-
-enum {
- DRV8711_TORQUE_SMPLTH_50 = 0 << 8,
- DRV8711_TORQUE_SMPLTH_100 = 1 << 8,
- DRV8711_TORQUE_SMPLTH_200 = 2 << 8,
- DRV8711_TORQUE_SMPLTH_300 = 3 << 8,
- DRV8711_TORQUE_SMPLTH_400 = 4 << 8,
- DRV8711_TORQUE_SMPLTH_600 = 5 << 8,
- DRV8711_TORQUE_SMPLTH_800 = 6 << 8,
- DRV8711_TORQUE_SMPLTH_1000 = 7 << 8,
-};
-
-
-enum {
- DRV8711_OFF_PWMMODE_bm = 1 << 8,
-};
-
-
-enum {
- DRV8711_BLANK_ABT_bm = 1 << 8,
-};
-
-
-enum {
- DRV8711_DECAY_DECMOD_SLOW = 0 << 8,
- DRV8711_DECAY_DECMOD_OPT = 1 << 8,
- DRV8711_DECAY_DECMOD_FAST = 2 << 8,
- DRV8711_DECAY_DECMOD_MIXED = 3 << 8,
- DRV8711_DECAY_DECMOD_AUTO_OPT = 4 << 8,
- DRV8711_DECAY_DECMOD_AUTO_MIXED = 5 << 8,
-};
-
-
-enum {
- DRV8711_STALL_SDCNT_1 = 0 << 8,
- DRV8711_STALL_SDCNT_2 = 1 << 8,
- DRV8711_STALL_SDCNT_4 = 2 << 8,
- DRV8711_STALL_SDCNT_8 = 3 << 8,
- DRV8711_STALL_VDIV_32 = 0 << 10,
- DRV8711_STALL_VDIV_16 = 1 << 10,
- DRV8711_STALL_VDIV_8 = 2 << 10,
- DRV8711_STALL_VDIV_4 = 3 << 10,
-};
-
-
-enum {
- DRV8711_DRIVE_OCPTH_250 = 0 << 0,
- DRV8711_DRIVE_OCPTH_500 = 1 << 0,
- DRV8711_DRIVE_OCPTH_750 = 2 << 0,
- DRV8711_DRIVE_OCPTH_1000 = 3 << 0,
- DRV8711_DRIVE_OCPDEG_1 = 0 << 2,
- DRV8711_DRIVE_OCPDEG_2 = 1 << 2,
- DRV8711_DRIVE_OCPDEG_4 = 2 << 2,
- DRV8711_DRIVE_OCPDEG_8 = 3 << 2,
- DRV8711_DRIVE_TDRIVEN_250 = 0 << 4,
- DRV8711_DRIVE_TDRIVEN_500 = 1 << 4,
- DRV8711_DRIVE_TDRIVEN_1000 = 2 << 4,
- DRV8711_DRIVE_TDRIVEN_2000 = 3 << 4,
- DRV8711_DRIVE_TDRIVEP_250 = 0 << 6,
- DRV8711_DRIVE_TDRIVEP_500 = 1 << 6,
- DRV8711_DRIVE_TDRIVEP_1000 = 2 << 6,
- DRV8711_DRIVE_TDRIVEP_2000 = 3 << 6,
- DRV8711_DRIVE_IDRIVEN_100 = 0 << 8,
- DRV8711_DRIVE_IDRIVEN_200 = 1 << 8,
- DRV8711_DRIVE_IDRIVEN_300 = 2 << 8,
- DRV8711_DRIVE_IDRIVEN_400 = 3 << 8,
- DRV8711_DRIVE_IDRIVEP_50 = 0 << 10,
- DRV8711_DRIVE_IDRIVEP_100 = 1 << 10,
- DRV8711_DRIVE_IDRIVEP_150 = 2 << 10,
- DRV8711_DRIVE_IDRIVEP_200 = 3 << 10,
-};
-
-enum {
- DRV8711_STATUS_OTS_bm = 1 << 0,
- DRV8711_STATUS_AOCP_bm = 1 << 1,
- DRV8711_STATUS_BOCP_bm = 1 << 2,
- DRV8711_STATUS_APDF_bm = 1 << 3,
- DRV8711_STATUS_BPDF_bm = 1 << 4,
- DRV8711_STATUS_UVLO_bm = 1 << 5,
- DRV8711_STATUS_STD_bm = 1 << 6,
- DRV8711_STATUS_STDLAT_bm = 1 << 7,
-};
-
-
-#define DRV8711_READ(ADDR) ((1 << 15) | ((ADDR) << 12))
-#define DRV8711_WRITE(ADDR, DATA) (((ADDR) << 12) | ((DATA) & 0xfff))
-#define DRV8711_CMD_ADDR(CMD) (((CMD) >> 12) & 7)
-#define DRV8711_CMD_IS_READ(CMD) ((1 << 15) & (CMD))
-
-
-void drv8711_init();
-void drv8711_enable(int driver);
-void drv8711_disable(int driver);
-void drv8711_set_power(int driver, float power);
-void drv8711_set_microsteps(int driver, uint16_t msteps);
-void drv8711_set_stall_callback(int driver, stall_callback_t cb);
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2016 Buildbotics LLC
- All rights reserved.
-
- 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/>.
-
- The software is distributed in the hope that it will be useful, but
- WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
- Lesser General Public License for more details.
-
- You should have received a copy of the GNU Lesser General Public
- License along with the software. If not, see
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-#include "estop.h"
-#include "motor.h"
-#include "stepper.h"
-#include "spindle.h"
-#include "switch.h"
-#include "report.h"
-#include "hardware.h"
-#include "homing.h"
-#include "config.h"
-
-#include "plan/planner.h"
-#include "plan/state.h"
-
-#include <avr/eeprom.h>
-
-
-typedef struct {
- bool triggered;
-} estop_t;
-
-
-static estop_t estop = {0};
-
-static uint16_t estop_reason_eeprom EEMEM;
-
-
-static void _set_reason(stat_t reason) {
- eeprom_update_word(&estop_reason_eeprom, reason);
-}
-
-
-static stat_t _get_reason() {
- return eeprom_read_word(&estop_reason_eeprom);
-}
-
-
-static void _switch_callback(switch_id_t id, bool active) {
- if (active) estop_trigger(STAT_ESTOP_SWITCH);
- else estop_clear();
-}
-
-
-void estop_init() {
- if (switch_is_active(SW_ESTOP)) _set_reason(STAT_ESTOP_SWITCH);
- if (STAT_MAX <= _get_reason()) _set_reason(STAT_OK);
- estop.triggered = _get_reason() != STAT_OK;
-
- switch_set_callback(SW_ESTOP, _switch_callback);
-
- if (estop.triggered) mp_state_estop();
-
- // Fault signal
- if (estop.triggered) OUTSET_PIN(FAULT_PIN); // High
- else OUTCLR_PIN(FAULT_PIN); // Low
- DIRSET_PIN(FAULT_PIN); // Output
-}
-
-
-bool estop_triggered() {
- return estop.triggered || switch_is_active(SW_ESTOP);
-}
-
-
-void estop_trigger(stat_t reason) {
- if (estop.triggered) return;
- estop.triggered = true;
-
- // Hard stop the motors and the spindle
- st_shutdown();
- spindle_estop();
-
- // Set machine state
- mp_state_estop();
-
- // Set axes not homed
- mach_set_not_homed();
-
- // Save reason
- _set_reason(reason);
-
- report_request();
-}
-
-
-void estop_clear() {
- // Check if estop switch is set
- if (switch_is_active(SW_ESTOP)) {
- if (_get_reason() != STAT_ESTOP_SWITCH) _set_reason(STAT_ESTOP_SWITCH);
- return; // Can't clear while estop switch is still active
- }
-
- // Clear fault signal
- OUTCLR_PIN(FAULT_PIN); // Low
-
- estop.triggered = false;
-
- // Clear reason
- _set_reason(STAT_OK);
-
- // Reboot
- // Note, hardware.c waits until any spindle stop command has been delivered
- hw_request_hard_reset();
-}
-
-
-bool get_estop() {
- return estop_triggered();
-}
-
-
-void set_estop(bool value) {
- if (value == estop_triggered()) return;
- if (value) estop_trigger(STAT_ESTOP_USER);
- else estop_clear();
-}
-
-
-PGM_P get_estop_reason() {
- return status_to_pgmstr(_get_reason());
-}
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2016 Buildbotics LLC
- All rights reserved.
-
- 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/>.
-
- The software is distributed in the hope that it will be useful, but
- WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
- Lesser General Public License for more details.
-
- You should have received a copy of the GNU Lesser General Public
- License along with the software. If not, see
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-#pragma once
-
-#include "status.h"
-
-#include <stdbool.h>
-
-
-void estop_init();
-bool estop_triggered();
-void estop_trigger(stat_t reason);
-void estop_clear();
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2016 Buildbotics LLC
- Copyright (c) 2010 - 2015 Alden S. Hart, Jr.
- All rights reserved.
-
- 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/>.
-
- The software is distributed in the hope that it will be useful, but
- WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
- Lesser General Public License for more details.
-
- You should have received a copy of the GNU Lesser General Public
- License along with the software. If not, see
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-#include "gcode_parser.h"
-
-#include "machine.h"
-#include "plan/arc.h"
-#include "probing.h"
-#include "homing.h"
-#include "axis.h"
-#include "util.h"
-
-#include <stdbool.h>
-#include <string.h>
-#include <ctype.h>
-#include <stdlib.h>
-#include <math.h>
-
-
-parser_t parser = {{0}};
-
-
-#define SET_MODAL(m, parm, val) \
- {parser.gn.parm = val; parser.gf.parm = true; modals[m] += 1; break;}
-#define SET_NON_MODAL(parm, val) \
- {parser.gn.parm = val; parser.gf.parm = true; break;}
-#define EXEC_FUNC(f, parm) if (parser.gf.parm) f(parser.gn.parm)
-
-
-static uint8_t modals[MODAL_GROUP_COUNT]; // collects modal groups in a block
-
-
-/* Normalize a block (line) of gcode in place
- *
- * Normalization functions:
- * - 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
- *
- * So this: " g1 x100 Y100 f400" becomes this: "G1X100Y100F400"
- *
- * Comment and message handling:
- * - Comments field start with a '(' char or alternately a semicolon ';'
- * - Comments and messages are not normalized - they are left alone
- * - The 'MSG' specifier in comment can have mixed case but cannot cannot
- * have embedded white spaces
- * - Normalization returns true if there was a message to display, false
- * otherwise
- * - Comments always terminate the block - i.e. leading or embedded comments
- * are not supported
- * - Valid cases (examples) Notes:
- * G0X10 - command only - no comment
- * (comment text) - There is no command on this line
- * G0X10 (comment text)
- * G0X10 (comment text - It's OK to drop the trailing paren
- * G0X10 ;comment text - It's OK to drop the trailing paren
- *
- * - Invalid cases (examples) Notes:
- * G0X10 comment text - Comment with no separator
- * N10 (comment) G0X10 - embedded comment. G0X10 will be ignored
- * (comment) G0X10 - leading comment. G0X10 will be ignored
- * G0X10 # comment - invalid separator
- *
- * Returns:
- * - com points to comment string or to 0 if no comment
- * - msg points to message string or to 0 if no comment
- * - block_delete_flag is set true if block delete encountered, false otherwise
- */
-
-static void _normalize_gcode_block(char *str, char **com, char **msg,
- uint8_t *block_delete_flag) {
- char *rd = str; // read pointer
- char *wr = str; // write pointer
-
- // mark block deletes
- *block_delete_flag = *rd == '/';
-
- // normalize the command block & find the comment (if any)
- for (; *wr; rd++)
- if (!*rd) *wr = 0;
- else if (*rd == '(' || *rd == ';') {*wr = 0; *com = rd + 1;}
- else if (isalnum(*rd) || strchr("-.", *rd)) // all valid characters
- *wr++ = toupper(*rd);
-
- // Perform Octal stripping - remove invalid leading zeros in number strings
- rd = str;
- while (*rd) {
- if (*rd == '.') break; // don't strip past a decimal point
- if (!isdigit(*rd) && *(rd + 1) == '0' && isdigit(*(rd + 2))) {
- wr = rd + 1;
- while (*wr) {*wr = *(wr + 1); wr++;} // copy forward w/overwrite
- continue;
- }
-
- rd++;
- }
-
- // process comments and messages
- if (**com) {
- rd = *com;
- while (isspace(*rd)) rd++; // skip any leading spaces before "msg"
-
- if (tolower(*rd) == 'm' && tolower(*(rd + 1)) == 's' &&
- tolower(*(rd + 2)) == 'g')
- *msg = rd + 3;
-
- for (; *rd; rd++)
- // 0 terminate on trailing parenthesis, if any
- if (*rd == ')') *rd = 0;
- }
-}
-
-
-/* Get gcode word consisting of a letter and a value
- *
- * This function requires the Gcode string to be normalized.
- * Normalization must remove any leading zeros or they will be converted to
- * octal. G0X... is not interpreted as hexadecimal. This is trapped.
- */
-static stat_t _get_next_gcode_word(char **pstr, char *letter, float *value) {
- if (!**pstr) return STAT_COMPLETE; // no more words to process
-
- // get letter part
- if (!isupper(**pstr)) return STAT_INVALID_OR_MALFORMED_COMMAND;
- *letter = **pstr;
- (*pstr)++;
-
- // 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
- }
-
- // get-value general case
- char *end;
- *value = strtod(*pstr, &end);
- // more robust test then checking for value == 0
- if (end == *pstr) return STAT_GCODE_COMMAND_UNSUPPORTED;
- *pstr = end; // pointer points to next character after the word
-
- return STAT_OK;
-}
-
-
-/// Isolate the decimal point value as an integer
-static uint8_t _point(float value) {return value * 10 - trunc(value) * 10;}
-
-
-#if 0
-static bool _axis_changed() {
- for (int axis = 0; axis < AXES; axis++)
- if (parser.gf.target[axis]) return true;
- return false;
-}
-#endif
-
-
-/// Check for some gross Gcode block semantic violations
-static stat_t _validate_gcode_block() {
- // 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 use axis words. If an
- // axis word-using G-code from group 1 is implicitly in effect on
- // a line (by having been activated on an earlier line), and a
- // group 0 G-code that 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 (modals[MODAL_GROUP_G0] && modals[MODAL_GROUP_G1])
- return STAT_MODAL_GROUP_VIOLATION;
-
-#if 0 // This check fails for arcs which may have offsets but no axis word
- // look for commands that require an axis word to be present
- if (modals[MODAL_GROUP_G0] || modals[MODAL_GROUP_G1])
- if (!_axis_changed()) return STAT_GCODE_AXIS_IS_MISSING;
-#endif
-
- return STAT_OK;
-}
-
-
-/* Execute parsed block
- *
- * Conditionally (based on whether a flag is set in gf) call the
- * machining functions in order of execution as per RS274NGC_3 table 8
- * (below, with modifications):
- *
- * 0. record the line number
- * 1. comment (includes message) [handled during block normalization]
- * 2. set feed rate mode (G93, G94 - inverse time or per minute)
- * 3. set feed rate (F)
- * 3a. set feed override rate (M50)
- * 4. set spindle speed (S)
- * 4a. set spindle override rate (M51)
- * 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)
- * 10. dwell (G4)
- * 11. set active plane (G17, G18, G19)
- * 12. set length units (G20, G21)
- * 13. cutter radius compensation on or off (G40, G41, G42)
- * 14. cutter length compensation on or off (G43, G49)
- * 15. coordinate system selection (G54, G55, G56, G57, G58, G59)
- * 16. set path control mode (G61, G61.1, G64)
- * 17. set distance mode (G90, G91, G90.1, G91.1)
- * 18. set retract mode (G98, G99)
- * 19a. homing functions (G28.2, G28.3, G28.1, G28, G30)
- * 19b. update system data (G10)
- * 19c. set axis offsets (G92, G92.1, G92.2, G92.3)
- * 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
- * to calling the machine functions (which does the unit conversions)
- */
-static stat_t _execute_gcode_block() {
- stat_t status = STAT_OK;
-
- mach_set_line(parser.gn.line);
- EXEC_FUNC(mach_set_feed_mode, feed_mode);
- EXEC_FUNC(mach_set_feed_rate, feed_rate);
- EXEC_FUNC(mach_feed_override_enable, feed_override_enable);
- EXEC_FUNC(mach_set_spindle_speed, spindle_speed);
- EXEC_FUNC(mach_spindle_override_enable, spindle_override_enable);
- EXEC_FUNC(mach_select_tool, tool);
- EXEC_FUNC(mach_change_tool, tool_change);
- EXEC_FUNC(mach_set_spindle_mode, spindle_mode);
- EXEC_FUNC(mach_mist_coolant_control, mist_coolant);
- EXEC_FUNC(mach_flood_coolant_control, flood_coolant);
- EXEC_FUNC(mach_override_enables, override_enables);
-
- if (parser.gn.next_action == NEXT_ACTION_DWELL) // G4 - dwell
- RITORNO(mach_dwell(parser.gn.parameter));
-
- EXEC_FUNC(mach_set_plane, plane);
- EXEC_FUNC(mach_set_units, units);
- //--> cutter radius compensation goes here
- //--> cutter length compensation goes here
- EXEC_FUNC(mach_set_coord_system, coord_system);
- EXEC_FUNC(mach_set_path_mode, path_mode);
- EXEC_FUNC(mach_set_distance_mode, distance_mode);
- EXEC_FUNC(mach_set_arc_distance_mode, arc_distance_mode);
- //--> set retract mode goes here
-
- switch (parser.gn.next_action) {
- case NEXT_ACTION_SET_G28_POSITION: // G28.1
- mach_set_g28_position();
- break;
- case NEXT_ACTION_GOTO_G28_POSITION: // G28
- status = mach_goto_g28_position(parser.gn.target, parser.gf.target);
- break;
- case NEXT_ACTION_SET_G30_POSITION: // G30.1
- mach_set_g30_position();
- break;
- case NEXT_ACTION_GOTO_G30_POSITION: // G30
- status = mach_goto_g30_position(parser.gn.target, parser.gf.target);
- break;
- case NEXT_ACTION_SEARCH_HOME: // G28.2
- mach_homing_cycle_start();
- break;
- case NEXT_ACTION_SET_ABSOLUTE_ORIGIN: // G28.3
- mach_set_absolute_origin(parser.gn.target, parser.gf.target);
- break;
- case NEXT_ACTION_HOMING_NO_SET: // G28.4
- mach_homing_cycle_start_no_set();
- break;
- case NEXT_ACTION_STRAIGHT_PROBE: // G38.2
- status = mach_probe(parser.gn.target, parser.gf.target);
- break;
- case NEXT_ACTION_SET_COORD_DATA:
- mach_set_coord_offsets(parser.gn.parameter, parser.gn.target,
- parser.gf.target);
- break;
- case NEXT_ACTION_SET_ORIGIN_OFFSETS:
- mach_set_origin_offsets(parser.gn.target, parser.gf.target);
- break;
- case NEXT_ACTION_RESET_ORIGIN_OFFSETS:
- mach_reset_origin_offsets();
- break;
- case NEXT_ACTION_SUSPEND_ORIGIN_OFFSETS:
- mach_suspend_origin_offsets();
- break;
- case NEXT_ACTION_RESUME_ORIGIN_OFFSETS:
- mach_resume_origin_offsets();
- break;
- case NEXT_ACTION_DWELL: break; // Handled above
-
- case NEXT_ACTION_DEFAULT:
- // apply override setting to gm struct
- mach_set_absolute_mode(parser.gn.absolute_mode);
-
- switch (parser.gn.motion_mode) {
- case MOTION_MODE_CANCEL_MOTION_MODE:
- mach_set_motion_mode(parser.gn.motion_mode);
- break;
- case MOTION_MODE_RAPID:
- status = mach_rapid(parser.gn.target, parser.gf.target);
- break;
- case MOTION_MODE_FEED:
- status = mach_feed(parser.gn.target, parser.gf.target);
- break;
- case MOTION_MODE_CW_ARC: case MOTION_MODE_CCW_ARC:
- // gf.radius sets radius mode if radius was collected in gn
- status = mach_arc_feed(parser.gn.target, parser.gf.target,
- parser.gn.arc_offset, parser.gf.arc_offset,
- parser.gn.arc_radius, parser.gf.arc_radius,
- parser.gn.parameter, parser.gf.parameter,
- modals[MODAL_GROUP_G1], parser.gn.motion_mode);
- break;
- default: break; // Should not get here
- }
- }
- // un-set absolute override once the move is planned
- mach_set_absolute_mode(false);
-
- // do the program stops and ends : M0, M1, M2, M30, M60
- if (parser.gf.program_flow)
- switch (parser.gn.program_flow) {
- case PROGRAM_STOP: mach_program_stop(); break;
- case PROGRAM_OPTIONAL_STOP: mach_optional_program_stop(); break;
- case PROGRAM_PALLET_CHANGE_STOP: mach_pallet_change_stop(); break;
- case PROGRAM_END: mach_program_end(); break;
- }
-
- return status;
-}
-
-
-/* Parses one line of 0 terminated G-Code.
- *
- * All the parser does is load the state values in gn (next model
- * state) and set flags in gf (model state flags). The execute
- * routine applies them. The buffer is assumed to contain only
- * uppercase characters and signed floats (no whitespace).
- *
- * A number of implicit things happen when the gn struct is zeroed:
- * - inverse feed rate mode is canceled - set back to units_per_minute mode
- */
-static stat_t _parse_gcode_block(char *buf) {
- char *pstr = buf; // persistent pointer 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)
- stat_t status = STAT_OK;
-
- // set initial state for new move
- memset(modals, 0, sizeof(modals)); // clear all parser values
- memset(&parser.gf, 0, sizeof(gcode_flags_t)); // clear all next-state flags
- memset(&parser.gn, 0, sizeof(gcode_state_t)); // clear all next-state values
-
- // get motion mode from previous block
- parser.gn.motion_mode = mach_get_motion_mode();
-
- // extract commands and parameters
- while ((status = _get_next_gcode_word(&pstr, &letter, &value)) == STAT_OK) {
- switch (letter) {
- case 'G':
- switch ((uint8_t)value) {
- case 0:
- SET_MODAL(MODAL_GROUP_G1, motion_mode, MOTION_MODE_RAPID);
- case 1:
- SET_MODAL(MODAL_GROUP_G1, motion_mode, MOTION_MODE_FEED);
- case 2: SET_MODAL(MODAL_GROUP_G1, motion_mode, MOTION_MODE_CW_ARC);
- case 3: SET_MODAL(MODAL_GROUP_G1, motion_mode, MOTION_MODE_CCW_ARC);
- case 4: SET_NON_MODAL(next_action, NEXT_ACTION_DWELL);
- case 10:
- SET_MODAL(MODAL_GROUP_G0, next_action, NEXT_ACTION_SET_COORD_DATA);
- case 17: SET_MODAL(MODAL_GROUP_G2, plane, PLANE_XY);
- case 18: SET_MODAL(MODAL_GROUP_G2, plane, PLANE_XZ);
- case 19: SET_MODAL(MODAL_GROUP_G2, plane, PLANE_YZ);
- case 20: SET_MODAL(MODAL_GROUP_G6, units, INCHES);
- case 21: SET_MODAL(MODAL_GROUP_G6, units, MILLIMETERS);
- case 28:
- switch (_point(value)) {
- case 0:
- SET_MODAL(MODAL_GROUP_G0, next_action, NEXT_ACTION_GOTO_G28_POSITION);
- case 1:
- SET_MODAL(MODAL_GROUP_G0, next_action, NEXT_ACTION_SET_G28_POSITION);
- case 2: SET_NON_MODAL(next_action, NEXT_ACTION_SEARCH_HOME);
- case 3: SET_NON_MODAL(next_action, NEXT_ACTION_SET_ABSOLUTE_ORIGIN);
- case 4: SET_NON_MODAL(next_action, NEXT_ACTION_HOMING_NO_SET);
- default: status = STAT_GCODE_COMMAND_UNSUPPORTED;
- }
- break;
-
- case 30:
- switch (_point(value)) {
- case 0:
- SET_MODAL(MODAL_GROUP_G0, next_action, NEXT_ACTION_GOTO_G30_POSITION);
- case 1:
- SET_MODAL(MODAL_GROUP_G0, next_action, NEXT_ACTION_SET_G30_POSITION);
- default: status = STAT_GCODE_COMMAND_UNSUPPORTED;
- }
- break;
-
- case 38:
- switch (_point(value)) {
- case 2: SET_NON_MODAL(next_action, NEXT_ACTION_STRAIGHT_PROBE);
- default: status = STAT_GCODE_COMMAND_UNSUPPORTED;
- }
- break;
-
- case 40: break; // ignore cancel cutter radius compensation
- case 49: break; // ignore cancel tool length offset comp.
- case 53: SET_NON_MODAL(absolute_mode, true);
- 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);
- case 57: SET_MODAL(MODAL_GROUP_G12, coord_system, G57);
- case 58: SET_MODAL(MODAL_GROUP_G12, coord_system, G58);
- case 59: SET_MODAL(MODAL_GROUP_G12, coord_system, G59);
- case 61:
- switch (_point(value)) {
- case 0: SET_MODAL(MODAL_GROUP_G13, path_mode, PATH_EXACT_PATH);
- case 1: SET_MODAL(MODAL_GROUP_G13, path_mode, PATH_EXACT_STOP);
- default: status = STAT_GCODE_COMMAND_UNSUPPORTED;
- }
- break;
-
- case 64: SET_MODAL(MODAL_GROUP_G13,path_mode, PATH_CONTINUOUS);
- case 80: SET_MODAL(MODAL_GROUP_G1, motion_mode,
- MOTION_MODE_CANCEL_MOTION_MODE);
- // case 90: SET_MODAL(MODAL_GROUP_G3, distance_mode, ABSOLUTE_MODE);
- // case 91: SET_MODAL(MODAL_GROUP_G3, distance_mode, INCREMENTAL_MODE);
- case 90:
- switch (_point(value)) {
- case 0: SET_MODAL(MODAL_GROUP_G3, distance_mode, ABSOLUTE_MODE);
- case 1: SET_MODAL(MODAL_GROUP_G3, arc_distance_mode, ABSOLUTE_MODE);
- default: status = STAT_GCODE_COMMAND_UNSUPPORTED;
- }
- break;
-
- case 91:
- switch (_point(value)) {
- case 0: SET_MODAL(MODAL_GROUP_G3, distance_mode, INCREMENTAL_MODE);
- case 1: SET_MODAL(MODAL_GROUP_G3, arc_distance_mode, INCREMENTAL_MODE);
- default: status = STAT_GCODE_COMMAND_UNSUPPORTED;
- }
- break;
-
- 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);
- default: status = STAT_GCODE_COMMAND_UNSUPPORTED;
- }
- break;
-
- case 93: SET_MODAL(MODAL_GROUP_G5, feed_mode, INVERSE_TIME_MODE);
- case 94: SET_MODAL(MODAL_GROUP_G5, feed_mode, UNITS_PER_MINUTE_MODE);
- // case 95:
- // SET_MODAL(MODAL_GROUP_G5, feed_mode, UNITS_PER_REVOLUTION_MODE);
- // case 96: // Spindle Constant Surface Speed (not currently supported)
- case 97: break; // Spindle RPM mode (only mode curently supported)
- default: status = STAT_GCODE_COMMAND_UNSUPPORTED;
- }
- break;
-
- case 'M':
- switch ((uint8_t)value) {
- case 0:
- SET_MODAL(MODAL_GROUP_M4, program_flow, PROGRAM_STOP);
- case 1:
- SET_MODAL(MODAL_GROUP_M4, program_flow, PROGRAM_OPTIONAL_STOP);
- case 60:
- SET_MODAL(MODAL_GROUP_M4, program_flow, PROGRAM_PALLET_CHANGE_STOP);
- case 2: case 30:
- SET_MODAL(MODAL_GROUP_M4, program_flow, PROGRAM_END);
- case 3: SET_MODAL(MODAL_GROUP_M7, spindle_mode, SPINDLE_CW);
- case 4: SET_MODAL(MODAL_GROUP_M7, spindle_mode, SPINDLE_CCW);
- case 5: SET_MODAL(MODAL_GROUP_M7, spindle_mode, 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); // Also mist
- case 48: SET_MODAL(MODAL_GROUP_M9, override_enables, true);
- case 49: SET_MODAL(MODAL_GROUP_M9, override_enables, false);
- case 50: SET_MODAL(MODAL_GROUP_M9, feed_override_enable, true);
- case 51: SET_MODAL(MODAL_GROUP_M9, spindle_override_enable, true);
- default: status = STAT_MCODE_COMMAND_UNSUPPORTED;
- }
- break;
-
- case 'T': SET_NON_MODAL(tool, (uint8_t)trunc(value));
- case 'F': SET_NON_MODAL(feed_rate, value);
- // used for dwell time, G10 coord select, rotations
- case 'P': SET_NON_MODAL(parameter, value);
- case 'S': SET_NON_MODAL(spindle_speed, value);
- case 'X': SET_NON_MODAL(target[AXIS_X], value);
- case 'Y': SET_NON_MODAL(target[AXIS_Y], value);
- case 'Z': SET_NON_MODAL(target[AXIS_Z], value);
- 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 '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 'R': SET_NON_MODAL(arc_radius, value);
- case 'N': SET_NON_MODAL(line, (uint32_t)value); // line number
- case 'L': break; // not used for anything
- case 0: break;
- default: status = STAT_GCODE_COMMAND_UNSUPPORTED;
- }
-
- if (status != STAT_OK) break;
- }
-
- if (status != STAT_OK && status != STAT_COMPLETE) return status;
- RITORNO(_validate_gcode_block());
-
- return _execute_gcode_block(); // if successful execute the block
-}
-
-
-/// Parse a block (line) of gcode
-/// Top level of gcode parser. Normalizes block and looks for special cases
-stat_t gc_gcode_parser(char *block) {
- char *str = block; // gcode command or 0 string
- char none = 0;
- char *com = &none; // gcode comment or 0 string
- char *msg = &none; // gcode message or 0 string
- uint8_t block_delete_flag;
-
- _normalize_gcode_block(str, &com, &msg, &block_delete_flag);
-
- // Block delete omits the line if a / char is present in the first space
- // For now this is unconditional and will always delete
- if (block_delete_flag) return STAT_NOOP;
-
- // queue a "(MSG" response
- if (*msg) mach_message(msg); // queue the message
-
- return _parse_gcode_block(block);
-}
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2016 Buildbotics LLC
- Copyright (c) 2010 - 2015 Alden S. Hart, Jr.
- All rights reserved.
-
- 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/>.
-
- The software is distributed in the hope that it will be useful, but
- WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
- Lesser General Public License for more details.
-
- You should have received a copy of the GNU Lesser General Public
- License along with the software. If not, see
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-#pragma once
-
-
-#include "status.h"
-#include "machine.h"
-
-
-typedef enum { // Used for detecting gcode errors. See NIST section 3.4
- MODAL_GROUP_G0, // {G10,G28,G28.1,G92} non-modal axis commands
- MODAL_GROUP_G1, // {G0,G1,G2,G3,G80} motion
- MODAL_GROUP_G2, // {G17,G18,G19} plane selection
- MODAL_GROUP_G3, // {G90,G91} distance mode
- MODAL_GROUP_G5, // {G93,G94} feed rate mode
- MODAL_GROUP_G6, // {G20,G21} units
- MODAL_GROUP_G7, // {G40,G41,G42} cutter radius compensation
- MODAL_GROUP_G8, // {G43,G49} tool length offset
- MODAL_GROUP_G9, // {G98,G99} return mode in canned cycles
- MODAL_GROUP_G12, // {G54,G55,G56,G57,G58,G59} coordinate system selection
- MODAL_GROUP_G13, // {G61,G61.1,G64} path control mode
- MODAL_GROUP_M4, // {M0,M1,M2,M30,M60} stopping
- MODAL_GROUP_M6, // {M6} tool change
- MODAL_GROUP_M7, // {M3,M4,M5} spindle turning
- MODAL_GROUP_M8, // {M7,M8,M9} coolant
- MODAL_GROUP_M9, // {M48,M49} speed/feed override switches
-} modal_group_t;
-
-#define MODAL_GROUP_COUNT (MODAL_GROUP_M9 + 1)
-
-
-typedef struct {
- gcode_state_t gn; // gcode input values
- gcode_flags_t gf; // gcode input flags
-} parser_t;
-
-
-extern parser_t parser;
-
-
-stat_t gc_gcode_parser(char *block);
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2016 Buildbotics LLC
- All rights reserved.
-
- 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/>.
-
- The software is distributed in the hope that it will be useful, but
- WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
- Lesser General Public License for more details.
-
- You should have received a copy of the GNU Lesser General Public
- License along with the software. If not, see
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-#include "gcode_state.h"
-
-
-PGM_P gs_get_units_pgmstr(units_t mode) {
- switch (mode) {
- case INCHES: return PSTR("IN");
- case MILLIMETERS: return PSTR("MM");
- case DEGREES: return PSTR("DEG");
- }
-
- return PSTR("INVALID");
-}
-
-
-PGM_P gs_get_feed_mode_pgmstr(feed_mode_t mode) {
- switch (mode) {
- case INVERSE_TIME_MODE: return PSTR("INVERSE TIME");
- case UNITS_PER_MINUTE_MODE: return PSTR("PER MIN");
- case UNITS_PER_REVOLUTION_MODE: return PSTR("PER REV");
- }
-
- return PSTR("INVALID");
-}
-
-
-PGM_P gs_get_plane_pgmstr(plane_t plane) {
- switch (plane) {
- case PLANE_XY: return PSTR("XY");
- case PLANE_XZ: return PSTR("XZ");
- case PLANE_YZ: return PSTR("YZ");
- }
-
- return PSTR("INVALID");
-}
-
-
-PGM_P gs_get_coord_system_pgmstr(coord_system_t cs) {
- switch (cs) {
- case ABSOLUTE_COORDS: return PSTR("ABS");
- case G54: return PSTR("G54");
- case G55: return PSTR("G55");
- case G56: return PSTR("G56");
- case G57: return PSTR("G57");
- case G58: return PSTR("G58");
- case G59: return PSTR("G59");
- }
-
- return PSTR("INVALID");
-}
-
-
-PGM_P gs_get_path_mode_pgmstr(path_mode_t mode) {
- switch (mode) {
- case PATH_EXACT_PATH: return PSTR("EXACT PATH");
- case PATH_EXACT_STOP: return PSTR("EXACT STOP");
- case PATH_CONTINUOUS: return PSTR("CONTINUOUS");
- }
-
- return PSTR("INVALID");
-}
-
-
-PGM_P gs_get_distance_mode_pgmstr(distance_mode_t mode) {
- switch (mode) {
- case ABSOLUTE_MODE: return PSTR("ABSOLUTE");
- case INCREMENTAL_MODE: return PSTR("INCREMENTAL");
- }
-
- return PSTR("INVALID");
-}
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2016 Buildbotics LLC
- Copyright (c) 2010 - 2015 Alden S. Hart, Jr.
- All rights reserved.
-
- 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/>.
-
- The software is distributed in the hope that it will be useful, but
- WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
- Lesser General Public License for more details.
-
- You should have received a copy of the GNU Lesser General Public
- License along with the software. If not, see
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-MEMBER(uint32_t, line) // Gcode block line number
-
-MEMBER(uint8_t, tool) // T - sets this value
-
-MEMBER(float, feed_rate) // F - mm/min or inverse time mode
-MEMBER(feed_mode_t, feed_mode)
-MEMBER(float, feed_override) // 1.0000 x F feed rate
-MEMBER(bool, feed_override_enable) // M48, M49
-
-MEMBER(float, spindle_speed) // in RPM
-MEMBER(spindle_mode_t, spindle_mode)
-MEMBER(float, spindle_override) // 1.0000 x S spindle speed
-MEMBER(bool, spindle_override_enable) // true = override enabled
-
-MEMBER(motion_mode_t, motion_mode) // Group 1 modal motion
-MEMBER(plane_t, plane) // G17, G18, G19
-MEMBER(units_t, units) // G20, G21
-MEMBER(coord_system_t, coord_system) // G54-G59 - select coord system 1-9
-MEMBER(bool, absolute_mode) // G53 move in machine coordinates
-MEMBER(path_mode_t, path_mode) // G61
-MEMBER(distance_mode_t, distance_mode) // G91
-MEMBER(distance_mode_t, arc_distance_mode) // G91.1
-
-MEMBER(bool, mist_coolant) // mist on (M7), off (M9)
-MEMBER(bool, flood_coolant) // mist on (M8), off (M9)
-
-MEMBER(next_action_t, next_action) // G group 1 moves & non-modals
-MEMBER(program_flow_t, program_flow) // used only by the gcode_parser
-
-// TODO unimplemented gcode parameters
-// MEMBER(float cutter_radius) // D - cutter radius compensation (0 is off)
-// MEMBER(float cutter_length) // H - cutter length compensation (0 is off)
-
-// Used for input only
-MEMBER(float, target[AXES]) // XYZABC where the move should go
-MEMBER(bool, override_enables) // feed and spindle enable
-MEMBER(bool, tool_change) // M6 tool change flag
-
-MEMBER(float, parameter) // P - dwell & G10 coord select
-MEMBER(float, arc_radius) // R - in arc radius mode
-MEMBER(float, arc_offset[3]) // IJK - used by arc commands
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2016 Buildbotics LLC
- Copyright (c) 2010 - 2015 Alden S. Hart, Jr.
- All rights reserved.
-
- 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/>.
-
- The software is distributed in the hope that it will be useful, but
- WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
- Lesser General Public License for more details.
-
- You should have received a copy of the GNU Lesser General Public
- License along with the software. If not, see
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-#pragma once
-
-#include "config.h"
-
-#include <avr/pgmspace.h>
-
-#include <stdint.h>
-#include <stdbool.h>
-
-
-/* The difference between next_action_t and motion_mode_t is that
- * next_action_t is used by the current block, and may carry non-modal
- * commands, whereas motion_mode_t persists across blocks as G modal group 1
- */
-
-/// these are in order to optimized CASE statement
-typedef enum {
- NEXT_ACTION_DEFAULT, // Must be zero (invokes motion modes)
- NEXT_ACTION_SEARCH_HOME, // G28.2 homing cycle
- NEXT_ACTION_SET_ABSOLUTE_ORIGIN, // G28.3 origin set
- NEXT_ACTION_HOMING_NO_SET, // G28.4 homing cycle no coord setting
- NEXT_ACTION_SET_G28_POSITION, // G28.1 set position in abs coordinates
- NEXT_ACTION_GOTO_G28_POSITION, // G28 go to machine position
- NEXT_ACTION_SET_G30_POSITION, // G30.1
- NEXT_ACTION_GOTO_G30_POSITION, // G30
- NEXT_ACTION_SET_COORD_DATA, // G10
- 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_DWELL, // G4
- NEXT_ACTION_STRAIGHT_PROBE, // G38.2
-} next_action_t;
-
-
-typedef enum { // G Modal Group 1
- MOTION_MODE_RAPID, // G0 - rapid
- MOTION_MODE_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
-} motion_mode_t;
-
-
-typedef enum { // plane - translates to:
- // axis_0 axis_1 axis_2
- PLANE_XY, // G17 X Y Z
- PLANE_XZ, // G18 X Z Y
- PLANE_YZ, // G19 Y Z X
-} plane_t;
-
-
-typedef enum {
- INCHES, // G20
- MILLIMETERS, // G21
- DEGREES, // ABC axes (this value used for displays only)
-} units_t;
-
-
-typedef enum {
- ABSOLUTE_COORDS, // machine coordinate system
- G54, G55, G56, G57, G58, G59,
-} coord_system_t;
-
-
-/// G Modal Group 13
-typedef enum {
- PATH_EXACT_PATH, // G61 hits corners but stops only if needed
- PATH_EXACT_STOP, // G61.1 stops at all corners
- PATH_CONTINUOUS, // G64 and typically the default mode
-} path_mode_t;
-
-
-typedef enum {
- ABSOLUTE_MODE, // G90
- INCREMENTAL_MODE, // G91
-} distance_mode_t;
-
-
-typedef enum {
- INVERSE_TIME_MODE, // G93
- UNITS_PER_MINUTE_MODE, // G94
- UNITS_PER_REVOLUTION_MODE, // G95 (unimplemented)
-} feed_mode_t;
-
-
-typedef enum {
- ORIGIN_OFFSET_SET, // G92 - set origin offsets
- ORIGIN_OFFSET_CANCEL, // G92.1 - zero out origin offsets
- ORIGIN_OFFSET_SUSPEND, // G92.2 - do not apply offsets, but preserve values
- ORIGIN_OFFSET_RESUME, // G92.3 - resume application of the suspended offsets
-} origin_offset_t;
-
-
-typedef enum {
- PROGRAM_STOP,
- PROGRAM_OPTIONAL_STOP,
- PROGRAM_PALLET_CHANGE_STOP,
- PROGRAM_END,
-} program_flow_t;
-
-
-/// spindle state settings
-typedef enum {
- SPINDLE_OFF,
- SPINDLE_CW,
- SPINDLE_CCW,
-} spindle_mode_t;
-
-
-/// mist and flood coolant states
-typedef enum {
- COOLANT_OFF, // all coolant off
- COOLANT_ON, // request coolant on or indicate both coolants are on
- COOLANT_MIST, // indicates mist coolant on
- COOLANT_FLOOD, // indicates flood coolant on
-} coolant_state_t;
-
-
-/// used for spindle and arc dir
-typedef enum {
- DIRECTION_CW,
- DIRECTION_CCW,
-} direction_t;
-
-
-/* Gcode model
- *
- * - mach.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 machine layer and
- * should be accessed only through mach_*() routines.
- *
- * - parser.gn is used by the gcode parser 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.
- *
- * - parser.gf is used by the gcode parser to hold flags for any data that has
- * changed in gn during the parse.
- */
-
-
-/// Gcode model state
-typedef struct {
-#define MEMBER(TYPE, VAR) TYPE VAR;
-#include "gcode_state.def"
-#undef MEMBER
-} gcode_state_t;
-
-
-typedef struct {
-#define MEMBER(TYPE, VAR) bool VAR;
-#include "gcode_state.def"
-#undef MEMBER
-} gcode_flags_t;
-
-
-PGM_P gs_get_units_pgmstr(units_t mode);
-PGM_P gs_get_feed_mode_pgmstr(feed_mode_t mode);
-PGM_P gs_get_plane_pgmstr(plane_t plane);
-PGM_P gs_get_coord_system_pgmstr(coord_system_t cs);
-PGM_P gs_get_path_mode_pgmstr(path_mode_t mode);
-PGM_P gs_get_distance_mode_pgmstr(distance_mode_t mode);
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2016 Buildbotics LLC
- Copyright (c) 2010 - 2015 Alden S. Hart, Jr.
- All rights reserved.
-
- 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/>.
-
- The software is distributed in the hope that it will be useful, but
- WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
- Lesser General Public License for more details.
-
- You should have received a copy of the GNU Lesser General Public
- License along with the software. If not, see
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-#include "hardware.h"
-#include "rtc.h"
-#include "usart.h"
-#include "huanyang.h"
-#include "config.h"
-
-#include <avr/interrupt.h>
-#include <avr/pgmspace.h>
-#include <avr/eeprom.h>
-#include <avr/wdt.h>
-
-#include <stdbool.h>
-#include <stddef.h>
-
-
-typedef struct {
- char id[26];
- bool hard_reset; // flag to perform a hard reset
- bool bootloader; // flag to enter the bootloader
-} hw_t;
-
-static hw_t hw = {{0}};
-
-
-#define PROD_SIGS (*(NVM_PROD_SIGNATURES_t *)0x0000)
-#define HEXNIB(x) "0123456789abcdef"[(x) & 0xf]
-
-
-/// This routine is lifted and modified from Boston Android and from
-/// http://www.avrfreaks.net/index.php?name=PNphpBB2&file=viewtopic&p=711659
-static void _init_clock() {
-#if defined(__CLOCK_EXTERNAL_8MHZ) // external 8 Mhx Xtal w/ 4x PLL = 32 Mhz
- // 2-9 MHz crystal; 0.4-16 MHz XTAL w/ 16K CLK startup
- OSC.XOSCCTRL = OSC_FRQRANGE_2TO9_gc | OSC_XOSCSEL_XTAL_16KCLK_gc;
- OSC.CTRL = OSC_XOSCEN_bm; // enable external crystal oscillator
- while (!(OSC.STATUS & OSC_XOSCRDY_bm)); // wait for oscillator ready
-
- OSC.PLLCTRL = OSC_PLLSRC_XOSC_gc | 4; // PLL source, 4x (32 MHz sys clock)
- OSC.CTRL = OSC_PLLEN_bm | OSC_XOSCEN_bm; // Enable PLL & External Oscillator
- while (!(OSC.STATUS & OSC_PLLRDY_bm)); // wait for PLL ready
-
- CCP = CCP_IOREG_gc;
- CLK.CTRL = CLK_SCLKSEL_PLL_gc; // switch to PLL clock
-
- OSC.CTRL &= ~OSC_RC2MEN_bm; // disable internal 2 MHz clock
-
-#elif defined(__CLOCK_EXTERNAL_16MHZ) // external 16Mhz Xtal w/ 2x PLL = 32 Mhz
- // 12-16 MHz crystal; 0.4-16 MHz XTAL w/ 16K CLK startup
- OSC.XOSCCTRL = OSC_FRQRANGE_12TO16_gc | OSC_XOSCSEL_XTAL_16KCLK_gc;
- OSC.CTRL = OSC_XOSCEN_bm; // enable external crystal oscillator
- while (!(OSC.STATUS & OSC_XOSCRDY_bm)); // wait for oscillator ready
-
- OSC.PLLCTRL = OSC_PLLSRC_XOSC_gc | 2; // PLL source, 2x (32 MHz sys clock)
- OSC.CTRL = OSC_PLLEN_bm | OSC_XOSCEN_bm; // Enable PLL & External Oscillator
- while (!(OSC.STATUS & OSC_PLLRDY_bm)); // wait for PLL ready
-
- CCP = CCP_IOREG_gc;
- CLK.CTRL = CLK_SCLKSEL_PLL_gc; // switch to PLL clock
-
- OSC.CTRL &= ~OSC_RC2MEN_bm; // disable internal 2 MHz clock
-
-#elif defined(__CLOCK_INTERNAL_32MHZ) // 32 MHz internal clock
- OSC.CTRL = OSC_RC32MEN_bm; // enable internal 32MHz oscillator
- while (!(OSC.STATUS & OSC_RC32MRDY_bm)); // wait for oscillator ready
-
- CCP = CCP_IOREG_gc; // Security Signature to modify clk
- CLK.CTRL = CLK_SCLKSEL_RC32M_gc; // select sysclock 32MHz osc
-
-#else
-#error No clock defined
-#endif
-}
-
-
-static void _load_hw_id_byte(int i, register8_t *reg) {
- NVM.CMD = NVM_CMD_READ_CALIB_ROW_gc;
- uint8_t byte = pgm_read_byte(reg);
- NVM.CMD = NVM_CMD_NO_OPERATION_gc;
-
- hw.id[i] = HEXNIB(byte >> 4);
- hw.id[i + 1] = HEXNIB(byte);
-}
-
-
-static void _read_hw_id() {
- int i = 0;
- _load_hw_id_byte(i, &PROD_SIGS.LOTNUM5); i += 2;
- _load_hw_id_byte(i, &PROD_SIGS.LOTNUM4); i += 2;
- _load_hw_id_byte(i, &PROD_SIGS.LOTNUM3); i += 2;
- _load_hw_id_byte(i, &PROD_SIGS.LOTNUM2); i += 2;
- _load_hw_id_byte(i, &PROD_SIGS.LOTNUM1); i += 2;
- _load_hw_id_byte(i, &PROD_SIGS.LOTNUM0); i += 2;
- hw.id[i++] = '-';
- _load_hw_id_byte(i, &PROD_SIGS.WAFNUM); i += 2;
- hw.id[i++] = '-';
- _load_hw_id_byte(i, &PROD_SIGS.COORDX1); i += 2;
- _load_hw_id_byte(i, &PROD_SIGS.COORDX0); i += 2;
- hw.id[i++] = '-';
- _load_hw_id_byte(i, &PROD_SIGS.COORDY1); i += 2;
- _load_hw_id_byte(i, &PROD_SIGS.COORDY0); i += 2;
- hw.id[i] = 0;
-}
-
-
-/// Lowest level hardware init
-void hardware_init() {
- _init_clock(); // set system clock
- rtc_init(); // real time counter
- _read_hw_id();
-
- // Round-robin, interrupts in application section, all interupts levels
- CCP = CCP_IOREG_gc;
- PMIC.CTRL =
- PMIC_RREN_bm | PMIC_HILVLEN_bm | PMIC_MEDLVLEN_bm | PMIC_LOLVLEN_bm;
-}
-
-
-void hw_request_hard_reset() {hw.hard_reset = true;}
-
-
-/// Hard reset using watchdog timer
-/// software hard reset using the watchdog timer
-void hw_hard_reset() {
- usart_flush();
- cli();
- CCP = CCP_IOREG_gc;
- RST.CTRL = RST_SWRST_bm;
-}
-
-
-/// Controller's rest handler
-void hw_reset_handler() {
- if (hw.hard_reset) {
- while (huanyang_stopping() || !usart_tx_empty() || !eeprom_is_ready())
- continue;
-
- hw_hard_reset();
- }
-
- if (hw.bootloader) {
- // TODO enable bootloader interrupt vectors and jump to BOOT_SECTION_START
- hw.bootloader = false;
- }
-}
-
-
-/// Executes a software reset using CCPWrite
-void hw_request_bootloader() {hw.bootloader = true;}
-
-
-uint8_t hw_disable_watchdog() {
- uint8_t state = WDT.CTRL;
- wdt_disable();
- return state;
-}
-
-
-void hw_restore_watchdog(uint8_t state) {
- cli();
- CCP = CCP_IOREG_gc;
- WDT.CTRL = state | WDT_CEN_bm;
- sei();
-}
-
-
-const char *get_hw_id() {
- return hw.id;
-}
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2016 Buildbotics LLC
- Copyright (c) 2010 - 2015 Alden S. Hart, Jr.
- Copyright (c) 2012 - 2015 Rob Giseburt
- All rights reserved.
-
- 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/>.
-
- The software is distributed in the hope that it will be useful, but
- WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
- Lesser General Public License for more details.
-
- You should have received a copy of the GNU Lesser General Public
- License along with the software. If not, see
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-#pragma once
-
-#include "status.h"
-
-#include <stdint.h>
-
-
-void hardware_init();
-void hw_request_hard_reset();
-void hw_hard_reset();
-void hw_reset_handler();
-
-void hw_request_bootloader();
-
-uint8_t hw_disable_watchdog();
-void hw_restore_watchdog(uint8_t state);
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2016 Buildbotics LLC
- All rights reserved.
-
- 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/>.
-
- The software is distributed in the hope that it will be useful, but
- WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
- Lesser General Public License for more details.
-
- You should have received a copy of the GNU Lesser General Public
- License along with the software. If not, see
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-#include "home.h"
-
-#include "plan/runtime.h"
-#include "plan/line.h"
-#include "plan/state.h"
-#include "plan/exec.h"
-#include "axis.h"
-#include "motor.h"
-#include "util.h"
-#include "config.h"
-
-#include <stdint.h>
-#include <string.h>
-
-
-typedef enum {
- STATE_INIT,
- STATE_HOMING,
- STATE_STALLED,
- STATE_BACKOFF,
- STATE_DONE,
-} home_state_t;
-
-
-typedef struct {
- bool homed[AXES];
- unsigned axis;
- home_state_t state;
- float velocity;
- uint16_t microsteps;
-} home_t;
-
-static home_t home;
-
-
-void _stall_callback(int motor) {
- if (home.velocity == mp_runtime_get_velocity()) {
- mp_exec_abort();
- home.state = STATE_STALLED;
- }
-}
-
-
-void _move_axis(float travel) {
- float target[AXES];
- float *position = mp_runtime_get_position();
- copy_vector(target, position);
- target[home.axis] += travel;
- mp_aline(target, false, false, false, home.velocity, 1, -1);
-}
-
-
-void home_callback() {
- if (mp_get_cycle() != CYCLE_HOMING || !mp_is_quiescent() ||
- !mp_queue_is_empty()) return;
-
- while (true) {
- int motor = axis_get_motor(home.axis);
- homing_mode_t mode = axis_get_homing_mode(home.axis);
- int direction =
- (mode == HOMING_STALL_MIN || mode == HOMING_SWITCH_MIN) ? -1 : 1;
- float min = axis_get_travel_min(home.axis);
- float max = axis_get_travel_max(home.axis);
-
- switch (home.state) {
- case STATE_INIT: {
- if (motor == -1 || mode == HOMING_DISABLED) {
- home.state = STATE_DONE;
- break;
- }
-
- STATUS_INFO("Homing %c axis", axis_get_char(home.axis));
-
- // Set axis not homed
- home.homed[home.axis] = false;
-
- // Determine homing type: switch or stall
-
- // Configure driver, set stall callback and compute homing velocity
- home.microsteps = motor_get_microsteps(motor);
- motor_set_microsteps(motor, 8);
- motor_set_stall_callback(motor, _stall_callback);
- //home.velocity = axis_get_stall_homing_velocity(home.axis);
- home.velocity = axis_get_search_velocity(home.axis);
-
- // Move in home direction
- float travel = max - min; // TODO consider ramping distance
- _move_axis(travel * direction);
-
- home.state = STATE_HOMING;
- return;
- }
-
- case STATE_HOMING:
- case STATE_STALLED:
- // Restore motor driver config
- motor_set_microsteps(motor, home.microsteps);
- motor_set_stall_callback(motor, 0);
-
- if (home.state == STATE_HOMING) {
- STATUS_ERROR(STAT_HOMING_CYCLE_FAILED, "Failed to find %c axis home",
- axis_get_char(home.axis));
- mp_set_cycle(CYCLE_MACHINING);
-
- } else {
- STATUS_INFO("Backing off %c axis", axis_get_char(home.axis));
- mach_set_axis_position(home.axis, direction < 0 ? min : max);
- _move_axis(axis_get_zero_backoff(home.axis) * direction * -1);
- home.state = STATE_BACKOFF;
- }
- return;
-
- case STATE_BACKOFF:
- STATUS_INFO("Homed %c axis", axis_get_char(home.axis));
-
- // Set axis position & homed
- mach_set_axis_position(home.axis, min);
- home.homed[home.axis] = true;
- home.state = STATE_DONE;
- break;
-
- case STATE_DONE:
- if (home.axis == AXIS_X) {
- // Done
- mp_set_cycle(CYCLE_MACHINING);
- return;
- }
-
- // Next axis
- home.axis--;
- home.state = STATE_INIT;
- break;
- }
- }
-}
-
-
-uint8_t command_home(int argc, char *argv[]) {
- if (mp_get_cycle() != CYCLE_MACHINING || mp_get_state() != STATE_READY)
- return 0;
-
- // Init
- memset(&home, 0, sizeof(home));
- home.axis = AXIS_Z;
-
- mp_set_cycle(CYCLE_HOMING);
-
- return 0;
-}
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2016 Buildbotics LLC
- All rights reserved.
-
- 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/>.
-
- The software is distributed in the hope that it will be useful, but
- WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
- Lesser General Public License for more details.
-
- You should have received a copy of the GNU Lesser General Public
- License along with the software. If not, see
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-#pragma once
-
-void home_init();
-void home_callback();
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2016 Buildbotics LLC
- Copyright (c) 2010 - 2015 Alden S. Hart, Jr.
- All rights reserved.
-
- 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/>.
-
- The software is distributed in the hope that it will be useful, but
- WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
- Lesser General Public License for more details.
-
- You should have received a copy of the GNU Lesser General Public
- License along with the software. If not, see
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-#include "homing.h"
-
-#include "axis.h"
-#include "machine.h"
-#include "switch.h"
-#include "gcode_parser.h"
-#include "report.h"
-
-#include "plan/planner.h"
-#include "plan/runtime.h"
-#include "plan/state.h"
-
-
-typedef void (*homing_func_t)(int8_t axis);
-
-static void _homing_axis_start(int8_t axis);
-
-
-typedef enum {
- HOMING_NOT_HOMED, // machine is not homed
- HOMING_HOMED, // machine is homed
- HOMING_WAITING, // machine waiting to be homed
-} homing_state_t;
-
-
-/// persistent homing runtime variables
-typedef struct {
- homing_state_t state; // homing cycle sub-state machine
- bool homed[AXES]; // individual axis homing flags
-
- // controls for homing cycle
- int8_t axis; // axis currently being homed
-
- /// homing switch for current axis (index into switch flag table)
- int8_t homing_switch;
- int8_t limit_switch; // limit switch for current axis or -1 if none
-
- uint8_t homing_closed; // 0=open, 1=closed
- uint8_t limit_closed; // 0=open, 1=closed
- /// G28.4 flag. true = set coords to zero at the end of homing cycle
- uint8_t set_coordinates;
- homing_func_t func; // binding for callback function state machine
-
- // per-axis parameters
- /// set to 1 for positive (max), -1 for negative (to min);
- float direction;
- float search_travel; // signed distance to travel in search
- float search_velocity; // search speed as positive number
- float latch_velocity; // latch speed as positive number
- /// max distance to back off switch during latch phase
- float latch_backoff;
- /// distance to back off switch before setting zero
- float zero_backoff;
- /// maximum distance of switch clearing backoffs before erring out
- float max_clear_backoff;
-
- // state saved from gcode model
- uint8_t saved_units; // G20,G21 global setting
- uint8_t saved_coord_system; // G54 - G59 setting
- uint8_t saved_distance_mode; // G90,G91 global setting
- uint8_t saved_feed_mode; // G93,G94 global setting
- float saved_feed_rate; // F setting
- float saved_jerk; // saved and restored for each axis homed
-} homing_t;
-
-
-static homing_t hm = {0};
-
-
-// G28.2 homing cycle
-
-/* Homing works from a G28.2 according to the following writeup:
- *
- * https://github.com/synthetos
- * /TinyG/wiki/Homing-and-Limits-Description-and-Operation
- *
- * How does this work?
- *
- * Homing is invoked using a G28.2 command with 1 or more axes specified in the
- * command: e.g. g28.2 x0 y0 z0 (FYI: the number after each axis is irrelevant)
- *
- * Homing is always run in the following order - for each enabled axis:
- * Z,X,Y,A Note: B and C cannot be homed
- *
- * At the start of a homing cycle those switches configured for homing
- * (or for homing and limits) are treated as homing switches (they are modal).
- *
- * After initialization the following sequence is run for each axis to be homed:
- *
- * 0. If a homing or limit switch is closed on invocation, clear the switch
- * 1. Drive towards the homing switch at search velocity until switch is hit
- * 2. Drive away from the homing switch at latch velocity until switch opens
- * 3. Back off switch by the zero backoff distance and set zero for that axis
- *
- * Homing works as a state machine that is driven by registering a callback
- * function at hm.func() for the next state to be run. Once the axis is
- * initialized each callback basically does two things (1) start the move
- * for the current function, and (2) register the next state with hm.func().
- * When a move is started it will either be interrupted if the homing switch
- * changes state, This will cause the move to stop with a feedhold. The other
- * thing that can happen is the move will run to its full length if no switch
- * change is detected (hit or open),
- *
- * Once all moves for an axis are complete the next axis in the sequence is
- * homed.
- *
- * When a homing cycle is initiated the homing state is set to HOMING_NOT_HOMED
- * When homing completes successfully this is set to HOMING_HOMED, otherwise it
- * remains HOMING_NOT_HOMED.
- *
- * Notes:
- *
- * 1. When coding a cycle (like this one) you get to perform one queued
- * move per entry into the continuation, then you must exit.
- *
- * 2. When coding a cycle (like this one) you must wait until
- * the last move has actually been queued (or has finished) before
- * declaring the cycle to be done. Otherwise there is a nasty race
- * condition that will accept the next command before the position of
- * the final move has been recorded in the Gcode model.
- */
-
-
-/*** Return next axis in sequence based on axis in arg
- *
- * Accepts "axis" arg as the current axis; or -1 to retrieve the first axis
- * Returns next axis based on "axis" argument and if that axis is flagged for
- * homing in the gf struct
- * Returns -1 when all axes have been processed
- * Returns -2 if no axes are specified (Gcode calling error)
- * Homes Z first, then the rest in sequence
- *
- * Isolating this function facilitates implementing more complex and
- * user-specified axis homing orders
- */
-static int8_t _get_next_axis(int8_t axis) {
- switch (axis) {
- case -1: if (parser.gf.target[AXIS_Z]) return AXIS_Z;
- case AXIS_Z: if (parser.gf.target[AXIS_X]) return AXIS_X;
- case AXIS_X: if (parser.gf.target[AXIS_Y]) return AXIS_Y;
- case AXIS_Y: if (parser.gf.target[AXIS_A]) return AXIS_A;
- case AXIS_A: if (parser.gf.target[AXIS_B]) return AXIS_B;
- case AXIS_B: if (parser.gf.target[AXIS_C]) return AXIS_C;
- }
-
- return axis == -1 ? -2 : -1; // error or done
-}
-
-
-/// Helper to finalize homing, third part of return to home
-static void _homing_finalize_exit() {
- mp_flush_planner(); // should be stopped, but in case of switch closure
-
- // Restore saved machine state
- mach_set_coord_system(hm.saved_coord_system);
- mach_set_units(hm.saved_units);
- mach_set_distance_mode(hm.saved_distance_mode);
- mach_set_feed_mode(hm.saved_feed_mode);
- mach_set_feed_rate(hm.saved_feed_rate);
- mach_set_motion_mode(MOTION_MODE_CANCEL_MOTION_MODE);
-
- mp_set_cycle(CYCLE_MACHINING); // Default cycle
-}
-
-
-static void _homing_error_exit(stat_t status) {
- _homing_finalize_exit();
- status_error(status);
-}
-
-
-/// Execute moves
-static void _homing_axis_move(int8_t axis, float target, float velocity) {
- float vect[AXES] = {0};
- bool flags[AXES] = {0};
-
- vect[axis] = target;
- flags[axis] = true;
- mach_set_feed_rate(velocity);
- mp_flush_planner(); // don't use mp_request_flush() here
-
- stat_t status = mach_feed(vect, flags);
- if (status) _homing_error_exit(status);
-}
-
-
-/// End homing cycle in progress
-static void _homing_abort(int8_t axis) {
- axis_set_jerk_max(axis, hm.saved_jerk); // restore the max jerk value
-
- // homing state remains HOMING_NOT_HOMED
- _homing_error_exit(STAT_HOMING_CYCLE_FAILED);
-
- report_request();
-}
-
-
-/// set zero and finish up
-static void _homing_axis_set_zero(int8_t axis) {
- if (hm.set_coordinates) {
- mach_set_axis_position(axis, 0);
- hm.homed[axis] = true;
-
- } else // do not set axis if in G28.4 cycle
- mach_set_axis_position(axis, mp_runtime_get_work_position(axis));
-
- axis_set_jerk_max(axis, hm.saved_jerk); // restore the max jerk value
-
- hm.func = _homing_axis_start;
-}
-
-
-/// backoff to zero position
-static void _homing_axis_zero_backoff(int8_t axis) {
- _homing_axis_move(axis, hm.zero_backoff, hm.search_velocity);
- hm.func = _homing_axis_set_zero;
-}
-
-
-/// Slow reverse until switch opens again
-static void _homing_axis_latch(int8_t axis) {
- // verify assumption that we arrived here because of homing switch closure
- // rather than user-initiated feedhold or other disruption
- if (!switch_is_active(hm.homing_switch)) hm.func = _homing_abort;
-
- else {
- _homing_axis_move(axis, hm.latch_backoff, hm.latch_velocity);
- hm.func = _homing_axis_zero_backoff;
- }
-}
-
-
-/// Fast search for switch, closes switch
-static void _homing_axis_search(int8_t axis) {
- // use the homing jerk for search onward
- _homing_axis_move(axis, hm.search_travel, hm.search_velocity);
- hm.func = _homing_axis_latch;
-}
-
-
-/// Initiate a clear to move off a switch that is thrown at the start
-static void _homing_axis_clear(int8_t axis) {
- // Handle an initial switch closure by backing off the closed switch
- // NOTE: Relies on independent switches per axis (not shared)
-
- if (switch_is_active(hm.homing_switch))
- _homing_axis_move(axis, hm.latch_backoff, hm.search_velocity);
-
- else if (switch_is_active(hm.limit_switch))
- _homing_axis_move(axis, -hm.latch_backoff, hm.search_velocity);
-
- hm.func = _homing_axis_search;
-}
-
-
-/// Get next axis, initialize variables, call the clear
-static void _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
- hm.state = HOMING_HOMED;
- _homing_finalize_exit();
- return;
-
- } else if (axis == -2) // -2 is error
- return _homing_error_exit(STAT_HOMING_ERROR_BAD_OR_NO_AXIS);
- }
-
- // clear the homed flag for axis to move w/o triggering soft limits
- hm.homed[axis] = false;
-
- // trap axis mis-configurations
- if (fp_ZERO(axis_get_search_velocity(axis)))
- return _homing_error_exit(STAT_HOMING_ERROR_ZERO_SEARCH_VELOCITY);
- if (fp_ZERO(axis_get_latch_velocity(axis)))
- return _homing_error_exit(STAT_HOMING_ERROR_ZERO_LATCH_VELOCITY);
- if (axis_get_latch_backoff(axis) < 0)
- return _homing_error_exit(STAT_HOMING_ERROR_NEGATIVE_LATCH_BACKOFF);
-
- // calculate and test travel distance
- float travel_distance =
- fabs(axis_get_travel_max(axis) - axis_get_travel_min(axis)) +
- axis_get_latch_backoff(axis);
- if (fp_ZERO(travel_distance))
- return _homing_error_exit(STAT_HOMING_ERROR_TRAVEL_MIN_MAX_IDENTICAL);
-
- hm.axis = axis; // persist the axis
- // search velocity is always positive
- hm.search_velocity = fabs(axis_get_search_velocity(axis));
- // latch velocity is always positive
- hm.latch_velocity = fabs(axis_get_latch_velocity(axis));
-
- // determine which switch to use
- bool min_enabled = switch_is_enabled(MIN_SWITCH(axis));
- bool max_enabled = switch_is_enabled(MAX_SWITCH(axis));
-
- if (min_enabled) {
- // setup parameters homing to the minimum switch
- hm.homing_switch = MIN_SWITCH(axis); // min is the homing switch
- hm.limit_switch = MAX_SWITCH(axis); // max would be limit switch
- hm.search_travel = -travel_distance; // in negative direction
- hm.latch_backoff = axis_get_latch_backoff(axis); // in positive direction
- hm.zero_backoff = axis_get_zero_backoff(axis);
-
- } else if (max_enabled) {
- // setup parameters for positive travel (homing to the maximum switch)
- hm.homing_switch = MAX_SWITCH(axis); // max is homing switch
- hm.limit_switch = MIN_SWITCH(axis); // min would be limit switch
- hm.search_travel = travel_distance; // in positive direction
- hm.latch_backoff = -axis_get_latch_backoff(axis); // in negative direction
- hm.zero_backoff = -axis_get_zero_backoff(axis);
-
- } else {
- // if homing is disabled for the axis then skip to the next axis
- hm.limit_switch = -1; // disable the limit switch parameter
- hm.func = _homing_axis_start;
- return;
- }
-
- hm.saved_jerk = axis_get_jerk_max(axis); // save the max jerk value
- hm.func = _homing_axis_clear; // start the clear
-}
-
-
-bool mach_is_homing() {
- return mp_get_cycle() == CYCLE_HOMING;
-}
-
-
-void mach_set_not_homed() {
- for (int axis = 0; axis < AXES; axis++)
- mach_set_homed(axis, false);
-}
-
-
-bool mach_get_homed(int axis) {
- return hm.homed[axis];
-}
-
-
-void mach_set_homed(int axis, bool homed) {
- // TODO save homed to EEPROM
- hm.homed[axis] = homed;
-}
-
-
-/// G28.2 homing cycle using limit switches
-void mach_homing_cycle_start() {
- // save relevant non-axis parameters from Gcode model
- hm.saved_units = mach_get_units();
- hm.saved_coord_system = mach_get_coord_system();
- hm.saved_distance_mode = mach_get_distance_mode();
- hm.saved_feed_mode = mach_get_feed_mode();
- hm.saved_feed_rate = mach_get_feed_rate();
-
- // set working values
- mach_set_units(MILLIMETERS);
- mach_set_distance_mode(INCREMENTAL_MODE);
- mach_set_coord_system(ABSOLUTE_COORDS); // in machine coordinates
- mach_set_feed_mode(UNITS_PER_MINUTE_MODE);
- hm.set_coordinates = true;
-
- hm.axis = -1; // set to retrieve initial axis
- hm.func = _homing_axis_start; // bind initial processing function
- mp_set_cycle(CYCLE_HOMING);
- hm.state = HOMING_NOT_HOMED;
-}
-
-
-void mach_homing_cycle_start_no_set() {
- mach_homing_cycle_start();
- hm.set_coordinates = false; // don't update position variables at end of cycle
-}
-
-
-/// Main loop callback for running the homing cycle
-void mach_homing_callback() {
- if (mp_get_cycle() != CYCLE_HOMING || mp_get_state() != STATE_READY) return;
- hm.func(hm.axis); // execute the current homing move
-}
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2016 Buildbotics LLC
- All rights reserved.
-
- 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/>.
-
- The software is distributed in the hope that it will be useful, but
- WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
- Lesser General Public License for more details.
-
- You should have received a copy of the GNU Lesser General Public
- License along with the software. If not, see
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-#pragma once
-
-#include <stdbool.h>
-
-
-bool mach_is_homing();
-void mach_set_not_homed();
-bool mach_get_homed(int axis);
-void mach_set_homed(int axis, bool homed);
-void mach_homing_cycle_start();
-void mach_homing_cycle_start_no_set();
-void mach_homing_callback();
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2016 Buildbotics LLC
- All rights reserved.
-
- 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/>.
-
- The software is distributed in the hope that it will be useful, but
- WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
- Lesser General Public License for more details.
-
- You should have received a copy of the GNU Lesser General Public
- License along with the software. If not, see
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-#include "huanyang.h"
-#include "config.h"
-#include "rtc.h"
-#include "report.h"
-
-#include <avr/io.h>
-#include <avr/interrupt.h>
-#include <avr/pgmspace.h>
-#include <util/crc16.h>
-
-#include <stdio.h>
-#include <string.h>
-#include <math.h>
-
-
-enum {
- HUANYANG_FUNC_READ = 1,
- HUANYANG_FUNC_WRITE,
- HUANYANG_CTRL_WRITE,
- HUANYANG_CTRL_READ,
- HUANYANG_FREQ_WRITE,
- HUANYANG_RESERVED_1,
- HUANYANG_RESERVED_2,
- HUANYANG_LOOP_TEST
-};
-
-
-enum {
- HUANYANG_BASE_FREQ = 4,
- HUANYANG_MAX_FREQ = 5,
- HUANYANG_MIN_FREQ = 11,
- HUANYANG_RATED_MOTOR_VOLTAGE = 141,
- HUANYANG_RATED_MOTOR_CURRENT = 142,
- HUANYANG_MOTOR_POLE = 143,
- HUANYANG_RATED_RPM = 144,
-};
-
-
-enum {
- HUANYANG_TARGET_FREQ,
- HUANYANG_ACTUAL_FREQ,
- HUANYANG_ACTUAL_CURRENT,
- HUANYANG_ACTUAL_RPM,
- HUANYANG_DC_VOLTAGE,
- HUANYANG_AC_VOLTAGE,
- HUANYANG_CONT,
- HUANYANG_TEMPERATURE,
-};
-
-
-enum {
- HUANYANG_FORWARD = 1,
- HUANYANG_STOP = 8,
- HUANYANG_REVERSE = 17,
-};
-
-
-enum {
- HUANYANG_RUN = 1 << 0,
- HUANYANG_JOG = 1 << 1,
- HUANYANG_COMMAND_RF = 1 << 2,
- HUANYANG_RUNNING = 1 << 3,
- HUANYANG_JOGGING = 1 << 4,
- HUANYANG_RUNNING_RF = 1 << 5,
- HUANYANG_BRACKING = 1 << 6,
- HUANYANG_TRACK_START = 1 << 7,
-};
-
-
-typedef bool (*next_command_cb_t)(int index);
-
-
-typedef struct {
- uint8_t id;
- bool debug;
-
- next_command_cb_t next_command_cb;
- uint8_t command_index;
- uint8_t current_offset;
- uint8_t command[8];
- uint8_t command_length;
- uint8_t response_length;
- uint8_t response[10];
- uint32_t last;
- uint8_t retry;
-
- bool connected;
- bool changed;
- bool estop;
- spindle_mode_t mode;
- float speed;
-
- float actual_freq;
- float actual_current;
- uint16_t actual_rpm;
- uint16_t dc_voltage;
- uint16_t ac_voltage;
- uint16_t temperature;
-
- float max_freq;
- float min_freq;
- uint16_t rated_rpm;
-
- uint8_t status;
-} huanyang_t;
-
-
-static huanyang_t ha = {0};
-
-
-#define CTRL_STATUS_RESPONSE(R) ((uint16_t)R[4] << 8 | R[5])
-#define FUNC_RESPONSE(R) (R[2] == 2 ? R[4] : ((uint16_t)R[4] << 8 | R[5]))
-
-
-static uint16_t _crc16(const uint8_t *buffer, unsigned length) {
- uint16_t crc = 0xffff;
-
- for (unsigned i = 0; i < length; i++)
- crc = _crc16_update(crc, buffer[i]);
-
- return crc;
-}
-
-
-static void _set_baud(uint16_t bsel, uint8_t bscale) {
- HUANYANG_PORT.BAUDCTRLB = (uint8_t)((bscale << 4) | (bsel >> 8));
- HUANYANG_PORT.BAUDCTRLA = bsel;
-}
-
-
-static void _set_write(bool x) {
- if (x) OUTSET_PIN(RS485_RW_PIN); // High
- else OUTCLR_PIN(RS485_RW_PIN); // Low
-}
-
-
-static void _set_dre_interrupt(bool enable) {
- if (enable) HUANYANG_PORT.CTRLA |= USART_DREINTLVL_MED_gc;
- else HUANYANG_PORT.CTRLA &= ~USART_DREINTLVL_MED_gc;
-}
-
-
-static void _set_txc_interrupt(bool enable) {
- if (enable) HUANYANG_PORT.CTRLA |= USART_TXCINTLVL_MED_gc;
- else HUANYANG_PORT.CTRLA &= ~USART_TXCINTLVL_MED_gc;
-}
-
-
-static void _set_rxc_interrupt(bool enable) {
- if (enable) HUANYANG_PORT.CTRLA |= USART_RXCINTLVL_MED_gc;
- else HUANYANG_PORT.CTRLA &= ~USART_RXCINTLVL_MED_gc;
-}
-
-
-static void _set_command1(int code, uint8_t arg0) {
- ha.command_length = 4;
- ha.command[1] = code;
- ha.command[2] = 1;
- ha.command[3] = arg0;
-}
-
-
-static void _set_command2(int code, uint8_t arg0, uint8_t arg1) {
- ha.command_length = 5;
- ha.command[1] = code;
- ha.command[2] = 2;
- ha.command[3] = arg0;
- ha.command[4] = arg1;
-}
-
-
-static void _set_command3(int code, uint8_t arg0, uint8_t arg1, uint8_t arg2) {
- ha.command_length = 6;
- ha.command[1] = code;
- ha.command[2] = 3;
- ha.command[3] = arg0;
- ha.command[4] = arg1;
- ha.command[5] = arg2;
-}
-
-
-static int _response_length(int code) {
- switch (code) {
- case HUANYANG_FUNC_READ: return 8;
- case HUANYANG_FUNC_WRITE: return 8;
- case HUANYANG_CTRL_WRITE: return 6;
- case HUANYANG_CTRL_READ: return 8;
- case HUANYANG_FREQ_WRITE: return 7;
- default: return -1;
- }
-}
-
-
-static bool _update(int index) {
- // Read response
- switch (index) {
- case 1: ha.status = ha.response[5]; break;
- case 2: ha.max_freq = FUNC_RESPONSE(ha.response) * 0.01; break;
- case 3: ha.min_freq = FUNC_RESPONSE(ha.response) * 0.01; break;
- case 4: ha.rated_rpm = FUNC_RESPONSE(ha.response); break;
- default: break;
- }
-
- // Setup next command
- uint8_t var;
- switch (index) {
- case 0: { // Update mode
- uint8_t state;
- switch (ha.mode) {
- case SPINDLE_CW: state = HUANYANG_FORWARD; break;
- case SPINDLE_CCW: state = HUANYANG_REVERSE; break;
- default: state = HUANYANG_STOP; break;
- }
-
- _set_command1(HUANYANG_CTRL_WRITE, state);
-
- return true;
- }
-
- case 1: var = HUANYANG_MAX_FREQ; break;
- case 2: var = HUANYANG_MIN_FREQ; break;
- case 3: var = HUANYANG_RATED_RPM; break;
-
- case 4: { // Update freqency
- // Compute frequency in Hz
- float freq = fabs(ha.speed * 50 / ha.rated_rpm);
-
- // Clamp frequency
- if (ha.max_freq < freq) freq = ha.max_freq;
- if (freq < ha.min_freq) freq = ha.min_freq;
-
- // Frequency write command
- uint16_t f = freq * 100;
- _set_command2(HUANYANG_FREQ_WRITE, f >> 8, f);
-
- return true;
- }
-
- default:
- report_request();
- return false;
- }
-
- _set_command3(HUANYANG_FUNC_READ, var, 0, 0);
-
- return true;
-}
-
-
-static bool _query_status(int index) {
- // Read response
- switch (index) {
- case 1: ha.actual_freq = CTRL_STATUS_RESPONSE(ha.response) * 0.01; break;
- case 2: ha.actual_current = CTRL_STATUS_RESPONSE(ha.response) * 0.1; break;
- case 3: ha.actual_rpm = CTRL_STATUS_RESPONSE(ha.response); break;
- case 4: ha.dc_voltage = CTRL_STATUS_RESPONSE(ha.response); break;
- case 5: ha.ac_voltage = CTRL_STATUS_RESPONSE(ha.response); break;
- case 6: ha.temperature = CTRL_STATUS_RESPONSE(ha.response); break;
- default: break;
- }
-
- // Setup next command
- uint8_t var;
- switch (index) {
- case 0: var = HUANYANG_ACTUAL_FREQ; break;
- case 1: var = HUANYANG_ACTUAL_CURRENT; break;
- case 2: var = HUANYANG_ACTUAL_RPM; break;
- case 3: var = HUANYANG_DC_VOLTAGE; break;
- case 4: var = HUANYANG_AC_VOLTAGE; break;
- case 5: var = HUANYANG_TEMPERATURE; break;
- default:
- report_request();
- return false;
- }
-
- _set_command1(HUANYANG_CTRL_READ, var);
-
- return true;
-}
-
-
-static void _next_command();
-
-
-static void _next_state() {
- if (ha.changed) {
- ha.next_command_cb = _update;
- ha.changed = false;
-
- } else ha.next_command_cb = _query_status;
-
- _next_command();
-}
-
-
-static bool _check_response() {
- // Check CRC
- uint16_t computed = _crc16(ha.response, ha.response_length - 2);
- uint16_t expected = (ha.response[ha.response_length - 1] << 8) |
- ha.response[ha.response_length - 2];
-
- if (computed != expected) {
- STATUS_WARNING("huanyang: invalid CRC, expected=0x%04u got=0x%04u",
- expected, computed);
- return false;
- }
-
- // Check return function code matches sent
- if (ha.command[1] != ha.response[1]) {
- STATUS_WARNING("huanyang: invalid function code, expected=%u got=%u",
- ha.command[2], ha.response[2]);
- return false;
- }
-
- return true;
-}
-
-
-static void _next_command() {
- if (ha.next_command_cb && ha.next_command_cb(ha.command_index++)) {
- ha.response_length = _response_length(ha.command[1]);
-
- ha.command[0] = ha.id;
-
- uint16_t crc = _crc16(ha.command, ha.command_length);
- ha.command[ha.command_length++] = crc;
- ha.command[ha.command_length++] = crc >> 8;
-
- _set_dre_interrupt(true);
-
- return;
- }
-
- ha.command_index = 0;
- _next_state();
-}
-
-
-static void _retry_command() {
- ha.last = 0;
- ha.current_offset = 0;
- ha.retry++;
-
- _set_write(true); // RS485 write mode
-
- _set_txc_interrupt(false);
- _set_rxc_interrupt(false);
- _set_dre_interrupt(true);
-
- if (ha.debug) STATUS_DEBUG("huanyang: retry %d", ha.retry);
-}
-
-
-// Data register empty interrupt
-ISR(HUANYANG_DRE_vect) {
- HUANYANG_PORT.DATA = ha.command[ha.current_offset++];
-
- if (ha.current_offset == ha.command_length) {
- _set_dre_interrupt(false);
- _set_txc_interrupt(true);
- ha.current_offset = 0;
- }
-}
-
-
-/// Transmit complete interrupt
-ISR(HUANYANG_TXC_vect) {
- _set_txc_interrupt(false);
- _set_rxc_interrupt(true);
- _set_write(false); // RS485 read mode
- ha.last = rtc_get_time(); // Set timeout timer
-}
-
-
-// Data received interrupt
-ISR(HUANYANG_RXC_vect) {
- ha.response[ha.current_offset++] = USARTD1.DATA;
-
- if (ha.current_offset == ha.response_length) {
- _set_rxc_interrupt(false);
- ha.current_offset = 0;
- _set_write(true); // RS485 write mode
-
- if (_check_response()) {
- ha.last = 0; // Clear timeout timer
- ha.retry = 0; // Reset retry counter
- ha.connected = true;
-
- _next_command();
- }
- }
-}
-
-
-void huanyang_init() {
- PR.PRPD &= ~PR_USART1_bm; // Disable power reduction
-
- DIRCLR_PIN(RS485_RO_PIN); // Input
- OUTSET_PIN(RS485_DI_PIN); // High
- DIRSET_PIN(RS485_DI_PIN); // Output
- OUTSET_PIN(RS485_RW_PIN); // High
- DIRSET_PIN(RS485_RW_PIN); // Output
-
- _set_baud(3325, 0b1101); // 9600 @ 32MHz with 2x USART
-
- // No parity, 8 data bits, 1 stop bit
- USARTD1.CTRLC = USART_CMODE_ASYNCHRONOUS_gc | USART_PMODE_DISABLED_gc |
- USART_CHSIZE_8BIT_gc;
-
- // Configure receiver and transmitter
- USARTD1.CTRLB = USART_RXEN_bm | USART_TXEN_bm | USART_CLK2X_bm;
-
- ha.id = HUANYANG_ID;
- huanyang_reset();
-}
-
-
-void huanyang_set(spindle_mode_t mode, float speed) {
- if ((ha.mode != mode || ha.speed != speed) && !ha.estop) {
- if (ha.debug) STATUS_DEBUG("huanyang: mode=%d, speed=%0.2f", mode, speed);
-
- ha.mode = mode;
- ha.speed = speed;
- ha.changed = true;
- }
-}
-
-
-void huanyang_reset() {
- _set_dre_interrupt(false);
- _set_txc_interrupt(false);
- _set_rxc_interrupt(false);
- _set_write(true); // RS485 write mode
-
- // Flush USART
- uint8_t x = USARTD1.DATA;
- x = USARTD1.STATUS;
- x = x;
-
- // Save settings
- uint8_t id = ha.id;
- spindle_mode_t mode = ha.mode;
- float speed = ha.speed;
- bool debug = ha.debug;
-
- // Clear state
- memset(&ha, 0, sizeof(ha));
-
- // Restore settings
- ha.id = id;
- ha.mode = mode;
- ha.speed = speed;
- ha.debug = debug;
- ha.changed = true;
-
- _next_state();
-}
-
-
-void huanyang_rtc_callback() {
- if (ha.last && rtc_expired(ha.last + HUANYANG_TIMEOUT)) {
- if (ha.retry < HUANYANG_RETRIES) _retry_command();
- else {
- if (ha.debug) STATUS_DEBUG("huanyang: timedout");
-
- if (ha.debug && ha.current_offset) {
- const uint8_t buf_len = 8 * 2 + 1;
- char sent[buf_len];
- char received[buf_len];
-
- uint8_t i;
- for (i = 0; i < ha.command_length; i++)
- sprintf(sent + i * 2, "%02x", ha.command[i]);
- sent[i * 2] = 0;
-
- for (i = 0; i < ha.current_offset; i++)
- sprintf(received + i * 2, "%02x", ha.response[i]);
- received[i * 2] = 0;
-
- STATUS_DEBUG("huanyang: sent 0x%s received 0x%s expected %u bytes",
- sent, received, ha.response_length);
- }
-
- huanyang_reset();
- }
- }
-}
-
-
-void huanyang_estop() {
- huanyang_set(SPINDLE_OFF, 0);
- huanyang_reset();
- ha.estop = true;
-}
-
-
-bool huanyang_stopping() {
- return ha.estop && (ha.changed || ha.next_command_cb == _update);
-}
-
-
-uint8_t get_huanyang_id() {return ha.id;}
-void set_huanyang_id(uint8_t value) {ha.id = value;}
-bool get_huanyang_debug() {return ha.debug;}
-void set_huanyang_debug(bool value) {ha.debug = value;}
-bool get_huanyang_connected() {return ha.connected;}
-float get_huanyang_freq() {return ha.actual_freq;}
-float get_huanyang_current() {return ha.actual_current;}
-uint16_t get_huanyang_rpm() {return ha.actual_rpm;}
-uint16_t get_huanyang_dcv() {return ha.dc_voltage;}
-uint16_t get_huanyang_acv() {return ha.ac_voltage;}
-uint16_t get_huanyang_temp() {return ha.temperature;}
-float get_huanyang_max_freq() {return ha.max_freq;}
-float get_huanyang_min_freq() {return ha.min_freq;}
-uint16_t get_huanyang_rated_rpm() {return ha.rated_rpm;}
-float get_huanyang_status() {return ha.status;}
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2016 Buildbotics LLC
- All rights reserved.
-
- 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/>.
-
- The software is distributed in the hope that it will be useful, but
- WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
- Lesser General Public License for more details.
-
- You should have received a copy of the GNU Lesser General Public
- License along with the software. If not, see
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-#pragma once
-
-#include "machine.h"
-
-
-void huanyang_init();
-void huanyang_set(spindle_mode_t mode, float speed);
-void huanyang_reset();
-void huanyang_rtc_callback();
-void huanyang_estop();
-bool huanyang_stopping();
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2016 Buildbotics LLC
- Copyright (c) 2010 - 2013 Alden S. Hart Jr.
- All rights reserved.
-
- 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/>.
-
- The software is distributed in the hope that it will be useful, but
- WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
- Lesser General Public License for more details.
-
- You should have received a copy of the GNU Lesser General Public
- License along with the software. If not, see
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-#include "i2c.h"
-
-#include <avr/interrupt.h>
-
-#include <stdbool.h>
-
-
-typedef struct {
- i2c_read_cb_t read_cb;
- i2c_write_cb_t write_cb;
- uint8_t data[I2C_MAX_DATA];
- uint8_t length;
- bool done;
- bool write;
-} i2c_t;
-
-static i2c_t i2c = {0};
-
-
-static void _i2c_reset_command() {
- i2c.length = 0;
- i2c.done = true;
- i2c.write = false;
-}
-
-
-static void _i2c_end_command() {
- if (i2c.length && !i2c.write && i2c.read_cb)
- i2c.read_cb(*i2c.data, i2c.data + 1, i2c.length - 1);
-
- _i2c_reset_command();
-}
-
-
-static void _i2c_command_byte(uint8_t byte) {
- i2c.data[i2c.length++] = byte;
-}
-
-
-ISR(I2C_ISR) {
- uint8_t status = I2C_DEV.SLAVE.STATUS;
-
- // Error or collision
- if (status & (TWI_SLAVE_BUSERR_bm | TWI_SLAVE_COLL_bm)) {
- _i2c_reset_command();
- return; // Ignore
-
- } else if ((status & TWI_SLAVE_APIF_bm) && (status & TWI_SLAVE_AP_bm)) {
- // START + address match
- I2C_DEV.SLAVE.CTRLB = TWI_SLAVE_CMD_RESPONSE_gc; // ACK address byte
- _i2c_end_command(); // Handle repeated START
-
- } else if (status & TWI_SLAVE_APIF_bm) {
- // STOP
- I2C_DEV.SLAVE.STATUS = TWI_SLAVE_APIF_bm; // Clear interrupt flag
- _i2c_end_command();
-
- } else if (status & TWI_SLAVE_DIF_bm) {
- i2c.write = status & TWI_SLAVE_DIR_bm;
-
- // DATA
- if (i2c.write) { // Write
- // Check if master ACKed last byte sent
- if (i2c.length && (status & TWI_SLAVE_RXACK_bm || i2c.done))
- I2C_DEV.SLAVE.CTRLB = TWI_SLAVE_CMD_COMPTRANS_gc; // End transaction
-
- else {
- // Send some data
- i2c.done = false;
- I2C_DEV.SLAVE.DATA = i2c.write_cb(i2c.length++, &i2c.done);
- I2C_DEV.SLAVE.CTRLB = TWI_SLAVE_CMD_RESPONSE_gc; // Continue transaction
- }
-
- } else { // Read
- _i2c_command_byte(I2C_DEV.SLAVE.DATA);
-
- // ACK and continue transaction
- I2C_DEV.SLAVE.CTRLB = TWI_SLAVE_CMD_RESPONSE_gc;
- }
- }
-}
-
-
-static uint8_t _i2c_default_write_cb(uint8_t offset, bool *done) {
- *done = true;
- return 0;
-}
-
-
-void i2c_init() {
- i2c_set_write_callback(_i2c_default_write_cb);
-
- I2C_DEV.SLAVE.CTRLA = TWI_SLAVE_INTLVL_HI_gc | TWI_SLAVE_DIEN_bm |
- TWI_SLAVE_ENABLE_bm | TWI_SLAVE_APIEN_bm | TWI_SLAVE_PIEN_bm;
- I2C_DEV.SLAVE.ADDR = I2C_ADDR << 1;
-}
-
-
-void i2c_set_read_callback(i2c_read_cb_t cb) {i2c.read_cb = cb;}
-void i2c_set_write_callback(i2c_write_cb_t cb) {i2c.write_cb = cb;}
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2016 Buildbotics LLC
- Copyright (c) 2010 - 2013 Alden S. Hart Jr.
- All rights reserved.
-
- 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/>.
-
- The software is distributed in the hope that it will be useful, but
- WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
- Lesser General Public License for more details.
-
- You should have received a copy of the GNU Lesser General Public
- License along with the software. If not, see
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-#pragma once
-
-#include "config.h"
-
-#include <stdbool.h>
-
-
-typedef enum {
- I2C_NULL,
- I2C_ESTOP,
- I2C_CLEAR,
- I2C_PAUSE,
- I2C_OPTIONAL_PAUSE,
- I2C_RUN,
- I2C_STEP,
- I2C_FLUSH,
- I2C_REPORT,
- I2C_HOME,
- I2C_REBOOT,
- I2C_ZERO,
-} i2c_cmd_t;
-
-
-typedef void (*i2c_read_cb_t)(i2c_cmd_t cmd, uint8_t *data, uint8_t length);
-typedef uint8_t (*i2c_write_cb_t)(uint8_t offset, bool *done);
-
-
-void i2c_init();
-void i2c_set_read_callback(i2c_read_cb_t cb);
-void i2c_set_write_callback(i2c_write_cb_t cb);
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2016 Buildbotics LLC
- Copyright (c) 2010 - 2015 Alden S. Hart, Jr.
- Copyright (c) 2012 - 2015 Rob Giseburt
- All rights reserved.
-
- 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/>.
-
- The software is distributed in the hope that it will be useful, but
- WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
- Lesser General Public License for more details.
-
- You should have received a copy of the GNU Lesser General Public
- License along with the software. If not, see
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-/* This code is a loose implementation of Kramer, Proctor and Messina's
- * machining functions as described in the NIST RS274/NGC v3
- *
- * The machine is the layer between the Gcode parser and the motion control code
- * for a specific robot. It keeps state and executes commands - passing the
- * stateless commands to the motion planning layer.
- *
- * Synchronizing command execution:
- *
- * "Synchronous commands" are commands that affect the runtime and need to be
- * synchronized with movement. Examples include G4 dwells, program stops and
- * ends, and most M commands. These are queued into the planner queue and
- * execute from the queue. Synchronous commands work like this:
- *
- * - Call the mach_xxx_xxx() function which will do any input validation and
- * return an error if it detects one.
- *
- * - The mach_ function calls mp_queue_push(). Arguments are a callback to
- * the _exec_...() function, which is the runtime execution routine, and any
- * arguments that are needed by the runtime.
- *
- * - mp_queue_push() stores the callback and the args in a planner buffer.
- *
- * - When planner execution reaches the buffer it executes the callback w/ the
- * args. Take careful note that the callback executes under an interrupt.
- *
- * Note: The synchronous command execution mechanism uses 2 vectors in the bf
- * buffer to store and return values for the callback. It's obvious, but
- * impractical to pass the entire bf buffer to the callback as some of these
- * commands are actually executed locally and have no buffer.
- */
-
-#include "machine.h"
-
-#include "config.h"
-#include "axis.h"
-#include "gcode_parser.h"
-#include "spindle.h"
-#include "coolant.h"
-#include "homing.h"
-
-#include "plan/planner.h"
-#include "plan/runtime.h"
-#include "plan/dwell.h"
-#include "plan/arc.h"
-#include "plan/line.h"
-#include "plan/state.h"
-
-
-typedef struct { // struct to manage mach globals and cycles
- float offset[COORDS + 1][AXES]; // coordinate systems & offsets G53-G59
- float origin_offset[AXES]; // G92 offsets
- bool origin_offset_enable; // G92 offsets enabled / disabled
-
- float position[AXES]; // model position
- float g28_position[AXES]; // stored machine position for G28
- float g30_position[AXES]; // stored machine position for G30
-
- gcode_state_t gm; // core gcode model state
-} machine_t;
-
-
-static machine_t mach = {
- // Offsets
- .offset = {
- {}, // ABSOLUTE_COORDS
-
- {0, 0, 0, 0, 0, 0}, // G54
- {X_TRAVEL_MAX / 2, Y_TRAVEL_MAX / 2, 0, 0, 0, 0}, // G55
- {0, 0, 0, 0, 0, 0}, // G56
- {0, 0, 0, 0, 0, 0}, // G57
- {0, 0, 0, 0, 0, 0}, // G58
- {0, 0, 0, 0, 0, 0}, // G59
- },
-
- // State
- .gm = {.motion_mode = MOTION_MODE_CANCEL_MOTION_MODE},
-};
-
-
-// Machine State functions
-uint32_t mach_get_line() {return mach.gm.line;}
-float mach_get_feed_rate() {return mach.gm.feed_rate;}
-feed_mode_t mach_get_feed_mode() {return mach.gm.feed_mode;}
-
-
-bool mach_is_inverse_time_mode() {
- return mach.gm.feed_mode == INVERSE_TIME_MODE;
-}
-
-
-float mach_get_feed_override() {
- return mach.gm.feed_override_enable ? mach.gm.feed_override : 1;
-}
-
-
-float mach_get_spindle_override() {
- return mach.gm.spindle_override_enable ? mach.gm.spindle_override : 1;
-}
-
-
-motion_mode_t mach_get_motion_mode() {return mach.gm.motion_mode;}
-bool mach_is_rapid() {return mach.gm.motion_mode == MOTION_MODE_RAPID;}
-plane_t mach_get_plane() {return mach.gm.plane;}
-units_t mach_get_units() {return mach.gm.units;}
-coord_system_t mach_get_coord_system() {return mach.gm.coord_system;}
-bool mach_get_absolute_mode() {return mach.gm.absolute_mode;}
-path_mode_t mach_get_path_mode() {return mach.gm.path_mode;}
-bool mach_is_exact_stop() {return mach.gm.path_mode == PATH_EXACT_STOP;}
-distance_mode_t mach_get_distance_mode() {return mach.gm.distance_mode;}
-distance_mode_t mach_get_arc_distance_mode() {return mach.gm.arc_distance_mode;}
-
-
-void mach_set_motion_mode(motion_mode_t motion_mode) {
- mach.gm.motion_mode = motion_mode;
-}
-
-
-/// Spindle speed callback from planner queue
-static stat_t _exec_spindle_speed(mp_buffer_t *bf) {
- spindle_set_speed(bf->value);
- return STAT_NOOP; // No move queued
-}
-
-
-/// Queue the S parameter to the planner buffer
-void mach_set_spindle_speed(float speed) {
- mp_buffer_t *bf = mp_queue_get_tail();
- bf->value = speed * mach_get_spindle_override();
- mp_queue_push_nonstop(_exec_spindle_speed, mach_get_line());
-}
-
-
-/// execute the spindle command (called from planner)
-static stat_t _exec_spindle_mode(mp_buffer_t *bf) {
- spindle_set_mode(bf->value);
- return STAT_NOOP; // No move queued
-}
-
-
-/// Queue the spindle command to the planner buffer
-void mach_set_spindle_mode(spindle_mode_t mode) {
- mp_buffer_t *bf = mp_queue_get_tail();
- bf->value = mode;
- mp_queue_push_nonstop(_exec_spindle_mode, mach_get_line());
-}
-
-
-void mach_set_absolute_mode(bool absolute_mode) {
- mach.gm.absolute_mode = absolute_mode;
-}
-
-
-void mach_set_line(uint32_t line) {mach.gm.line = line;}
-
-
-/* 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
- *
- * Notes on Coordinate System and Offset functions
- *
- * All positional information in the 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.
- *
- * 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
- * - G28 and G30 moves; these are run in absolute coordinates
- *
- * The offsets themselves are considered static, are kept in mach, and are
- * supposed to be persistent.
- *
- * To reduce complexity and data load the following is done:
- * - Full data for coordinates/offsets is only accessible by the
- * machine, not the downstream
- * - 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
- */
-
-/* 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.
- */
-float mach_get_active_coord_offset(uint8_t axis) {
- // no offset in absolute override mode
- if (mach.gm.absolute_mode) return 0;
- float offset = mach.offset[mach.gm.coord_system][axis];
-
- if (mach.origin_offset_enable)
- offset += mach.origin_offset[axis]; // includes G5x and G92 components
-
- return offset;
-}
-
-
-static stat_t _exec_update_work_offsets(mp_buffer_t *bf) {
- mp_runtime_set_work_offsets(bf->target);
- return STAT_NOOP; // No move queued
-}
-
-
-// Capture coord offsets from the model into absolute values
-void mach_update_work_offsets() {
- static float work_offset[AXES] = {0};
- bool same = true;
-
- for (int axis = 0; axis < AXES; axis++) {
- float offset = mach_get_active_coord_offset(axis);
-
- if (offset != work_offset[axis]) {
- work_offset[axis] = offset;
- same = false;
- }
- }
-
- if (!same) {
- mp_buffer_t *bf = mp_queue_get_tail();
- copy_vector(bf->target, work_offset);
- mp_queue_push_nonstop(_exec_update_work_offsets, mach_get_line());
- }
-}
-
-
-const float *mach_get_position() {return mach.position;}
-
-
-void mach_set_position(const float position[]) {
- copy_vector(mach.position, position);
-}
-
-
-/*** Get position of axis in absolute coordinates
- *
- * NOTE: Machine position is always returned in mm mode. No units conversion
- * is performed.
- */
-float mach_get_axis_position(uint8_t axis) {return mach.position[axis];}
-
-
-/* Critical helpers
- *
- * Core functions supporting the machining functions
- * These functions are not part of the NIST defined functions
- */
-
-
-
-/*** Calculate target vector
- *
- * This is a core routine. It handles:
- * - conversion of linear units to internal canonical form (mm)
- * - conversion of relative mode to absolute (internal canonical form)
- * - translation of work coordinates to machine coordinates (internal
- * canonical form)
- * - application of axis radius mode
- *
- * Target coordinates are provided in @param values.
- * Axes that need processing are signaled in @param flags.
- */
-void mach_calc_target(float target[], const float values[],
- const bool flags[]) {
- for (int axis = 0; axis < AXES; axis++) {
- target[axis] = mach.position[axis];
- if (!flags[axis] || !axis_is_enabled(axis)) continue;
-
- target[axis] = mach.gm.distance_mode == ABSOLUTE_MODE ?
- mach_get_active_coord_offset(axis) : mach.position[axis];
-
- float radius = axis_get_radius(axis);
- if (radius) // Handle radius mode if radius is non-zero
- target[axis] += TO_MM(values[axis]) * 360 / (2 * M_PI * radius);
- // For ABC axes no mm conversion - it's already in degrees
- else if (AXIS_Z < axis) target[axis] += values[axis];
- else target[axis] += TO_MM(values[axis]);
- }
-}
-
-
-/*** Return error code if soft limit is exceeded
- *
- * Must be called with target properly set in GM struct. Best done
- * after mach_calc_target().
- *
- * Tests for soft limit for any homed axis if min and max are
- * different values. You can set min and max to 0,0 to disable soft
- * limits for an axis. Also will not test a min or a max if the value
- * is < -1000000 (negative one million). This allows a single end to
- * be tested w/the other disabled, should that requirement ever arise.
- */
-stat_t mach_test_soft_limits(float target[]) {
- for (int axis = 0; axis < AXES; axis++) {
- if (!axis_get_homed(axis)) continue; // don't test axes that arent homed
-
- float min = axis_get_travel_min(axis);
- float max = axis_get_travel_max(axis);
-
- // min == max means no soft limits
- if (fp_EQ(min, max)) continue;
-
- if ((min > DISABLE_SOFT_LIMIT && target[axis] < min) ||
- (max > DISABLE_SOFT_LIMIT && target[axis] > max))
- return STAT_SOFT_LIMIT_EXCEEDED;
- }
-
- return STAT_OK;
-}
-
-
-/* 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
- */
-
-// Initialization and Termination (4.3.2)
-
-void machine_init() {
- // Set gcode defaults
- mach_set_units(GCODE_DEFAULT_UNITS);
- mach_set_coord_system(GCODE_DEFAULT_COORD_SYSTEM);
- mach_set_plane(GCODE_DEFAULT_PLANE);
- mach_set_path_mode(GCODE_DEFAULT_PATH_CONTROL);
- mach_set_distance_mode(GCODE_DEFAULT_DISTANCE_MODE);
- mach_set_arc_distance_mode(GCODE_DEFAULT_ARC_DISTANCE_MODE);
- mach_set_feed_mode(UNITS_PER_MINUTE_MODE); // always the default
-
- // Sub-system inits
- spindle_init();
- coolant_init();
-}
-
-
-// Representation (4.3.3)
-//
-// Affect the Gcode model only (asynchronous)
-// These functions assume input validation occurred upstream.
-
-/// G17, G18, G19 select axis plane
-void mach_set_plane(plane_t plane) {mach.gm.plane = plane;}
-
-
-/// G20, G21
-void mach_set_units(units_t mode) {mach.gm.units = mode;}
-
-
-/// G90, G91
-void mach_set_distance_mode(distance_mode_t mode) {
- mach.gm.distance_mode = mode;
-}
-
-
-/// G90.1, G91.1
-void mach_set_arc_distance_mode(distance_mode_t mode) {
- mach.gm.arc_distance_mode = mode;
-}
-
-
-/* G10 L2 Pn, delayed persistence
- *
- * This function applies the offset to the GM model.
- */
-void mach_set_coord_offsets(coord_system_t coord_system, float offset[],
- bool flags[]) {
- if (coord_system < G54 || G59 < coord_system) return;
-
- for (int axis = 0; axis < AXES; axis++)
- if (flags[axis])
- mach.offset[coord_system][axis] = TO_MM(offset[axis]);
-}
-
-
-/// G54-G59
-void mach_set_coord_system(coord_system_t coord_system) {
- mach.gm.coord_system = coord_system;
-}
-
-
-/* Set the position of a single axis in the model, planner and runtime
- *
- * 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 !!!!!
- *
- * 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.
- */
-void mach_set_axis_position(unsigned axis, float position) {
- //if (!mp_is_quiescent()) ALARM(STAT_MACH_NOT_QUIESCENT);
- if (AXES <= axis) return;
-
- mach.position[axis] = position;
- mp_set_axis_position(axis, position);
- mp_runtime_set_axis_position(axis, position);
- mp_runtime_set_steps_from_position();
-}
-
-
-stat_t mach_zero_all() {
- for (unsigned axis = 0; axis < AXES; axis++) {
- stat_t status = mach_zero_axis(axis);
- if (status != STAT_OK) return status;
- }
-
- return STAT_OK;
-}
-
-
-stat_t mach_zero_axis(unsigned axis) {
- if (!mp_is_quiescent()) return STAT_MACH_NOT_QUIESCENT;
- if (AXES <= axis) return STAT_INVALID_AXIS;
-
- mach_set_axis_position(axis, 0);
-
- return STAT_OK;
-}
-
-
-// G28.3 functions and support
-static stat_t _exec_absolute_origin(mp_buffer_t *bf) {
- const float *origin = bf->target;
- const float *flags = bf->unit;
-
- for (int axis = 0; axis < AXES; axis++)
- if (flags[axis]) {
- mp_runtime_set_axis_position(axis, origin[axis]);
- mach_set_homed(axis, true); // G28.3 is not considered homed until here
- }
-
- mp_runtime_set_steps_from_position();
-
- return STAT_NOOP; // No move queued
-}
-
-
-/* G28.3 - model, planner and queue to runtime
- *
- * Takes a vector of origins (presumably 0's, but not necessarily) and
- * applies them to all axes where the corresponding position in the
- * flags vector is true (1).
- *
- * This is a 2 step process. The model and planner contexts are set
- * immediately, the runtime command is queued and synchronized with
- * the planner queue. This includes the runtime position and the step
- * recording done by the encoders. At that point any axis that is set
- * is also marked as homed.
- */
-void mach_set_absolute_origin(float origin[], bool flags[]) {
- float value[AXES];
-
- for (int axis = 0; axis < AXES; axis++)
- if (flags[axis]) {
- value[axis] = TO_MM(origin[axis]);
- mach.position[axis] = value[axis]; // set model position
- mp_set_axis_position(axis, value[axis]); // set mm position
- }
-
- mp_buffer_t *bf = mp_queue_get_tail();
- copy_vector(bf->target, origin);
- copy_vector(bf->unit, flags);
- mp_queue_push_nonstop(_exec_absolute_origin, mach_get_line());
-}
-
-
-/* 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
- */
-
-/// G92
-void mach_set_origin_offsets(float offset[], bool flags[]) {
- // set offsets in the Gcode model extended context
- mach.origin_offset_enable = true;
- for (int axis = 0; axis < AXES; axis++)
- if (flags[axis])
- mach.origin_offset[axis] = mach.position[axis] -
- mach.offset[mach.gm.coord_system][axis] - TO_MM(offset[axis]);
-}
-
-
-/// G92.1
-void mach_reset_origin_offsets() {
- mach.origin_offset_enable = false;
- for (int axis = 0; axis < AXES; axis++)
- mach.origin_offset[axis] = 0;
-}
-
-
-/// G92.2
-void mach_suspend_origin_offsets() {
- mach.origin_offset_enable = false;
-}
-
-
-/// G92.3
-void mach_resume_origin_offsets() {
- mach.origin_offset_enable = true;
-}
-
-
-stat_t mach_plan_line(float target[]) {
- return mp_aline(target, mach_is_rapid(), mach_is_inverse_time_mode(),
- mach_is_exact_stop(), mach.gm.feed_rate,
- mach_get_feed_override(), mach_get_line());
-}
-
-
-// Free Space Motion (4.3.4)
-static stat_t _feed(float values[], bool flags[]) {
- float target[AXES];
- mach_calc_target(target, values, flags);
-
- // test soft limits
- stat_t status = mach_test_soft_limits(target);
- if (status != STAT_OK) return ALARM(status);
-
- // prep and plan the move
- mach_update_work_offsets(); // update resolved offsets
- mach_plan_line(target);
- copy_vector(mach.position, target); // update model position
-
- return status;
-}
-
-
-/// G0 linear rapid
-stat_t mach_rapid(float values[], bool flags[]) {
- mach.gm.motion_mode = MOTION_MODE_RAPID;
- return _feed(values, flags);
-}
-
-
-/// G28.1
-void mach_set_g28_position() {copy_vector(mach.g28_position, mach.position);}
-
-
-/// G28
-stat_t mach_goto_g28_position(float target[], bool flags[]) {
- mach_set_absolute_mode(true);
-
- // move through intermediate point, or skip
- mach_rapid(target, flags);
-
- // execute actual stored move
- bool f[] = {true, true, true, true, true, true};
- return mach_rapid(mach.g28_position, f);
-}
-
-
-/// G30.1
-void mach_set_g30_position() {copy_vector(mach.g30_position, mach.position);}
-
-
-/// G30
-stat_t mach_goto_g30_position(float target[], bool flags[]) {
- mach_set_absolute_mode(true);
-
- // move through intermediate point, or skip
- mach_rapid(target, flags);
-
- // execute actual stored move
- bool f[] = {true, true, true, true, true, true};
- return mach_rapid(mach.g30_position, f);
-}
-
-
-// Machining Attributes (4.3.5)
-
-/// F parameter
-/// Normalize feed rate to mm/min or to minutes if in inverse time mode
-void mach_set_feed_rate(float feed_rate) {
- if (mach.gm.feed_mode == INVERSE_TIME_MODE)
- // normalize to minutes (active for this gcode block only)
- mach.gm.feed_rate = feed_rate ? 1 / feed_rate : 0; // Avoid div by zero
-
- else mach.gm.feed_rate = TO_MM(feed_rate);
-}
-
-
-/// G93, G94
-void mach_set_feed_mode(feed_mode_t mode) {
- if (mach.gm.feed_mode == mode) return;
- mach.gm.feed_rate = 0; // Force setting feed rate after changing modes
- mach.gm.feed_mode = mode;
-}
-
-
-/// G61, G61.1, G64
-void mach_set_path_mode(path_mode_t mode) {
- mach.gm.path_mode = mode;
-}
-
-
-// Machining Functions (4.3.6) See arc.c
-
-/// G4, P parameter (seconds)
-stat_t mach_dwell(float seconds) {
- return mp_dwell(seconds, mach_get_line());
-}
-
-
-/// G1
-stat_t mach_feed(float values[], bool flags[]) {
- // trap zero feed rate condition
- if (fp_ZERO(mach.gm.feed_rate) ||
- (mach.gm.feed_mode == INVERSE_TIME_MODE && !parser.gf.feed_rate))
- return STAT_GCODE_FEEDRATE_NOT_SPECIFIED;
-
- mach.gm.motion_mode = MOTION_MODE_FEED;
- return _feed(values, flags);
-}
-
-
-// Spindle Functions (4.3.7) see spindle.c, spindle.h
-
-// Tool Functions (4.3.8)
-
-/// T parameter
-void mach_select_tool(uint8_t tool) {mach.gm.tool = tool;}
-
-
-static stat_t _exec_change_tool(mp_buffer_t *bf) {
- mp_runtime_set_tool(bf->value);
- mp_set_hold_reason(HOLD_REASON_TOOL_CHANGE);
- mp_state_holding();
- return STAT_NOOP; // No move queued
-}
-
-
-/// M6
-void mach_change_tool(bool x) {
- mp_buffer_t *bf = mp_queue_get_tail();
- bf->value = mach.gm.tool;
- mp_queue_push(_exec_change_tool, mach_get_line());
-}
-
-
-// Miscellaneous Functions (4.3.9)
-static stat_t _exec_mist_coolant(mp_buffer_t *bf) {
- coolant_set_mist(bf->value);
- return STAT_NOOP; // No move queued
-}
-
-
-/// M7
-void mach_mist_coolant_control(bool mist_coolant) {
- mp_buffer_t *bf = mp_queue_get_tail();
- bf->value = mist_coolant;
- mp_queue_push_nonstop(_exec_mist_coolant, mach_get_line());
-}
-
-
-static stat_t _exec_flood_coolant(mp_buffer_t *bf) {
- coolant_set_flood(bf->value);
- if (!bf->value) coolant_set_mist(false); // M9 special function
- return STAT_NOOP; // No move queued
-}
-
-
-/// M8, M9
-void mach_flood_coolant_control(bool flood_coolant) {
- mp_buffer_t *bf = mp_queue_get_tail();
- bf->value = flood_coolant;
- mp_queue_push_nonstop(_exec_flood_coolant, mach_get_line());
-}
-
-
-/* Override enables are kind of a mess in Gcode. This is an attempt to sort
- * them out. See
- * http://www.linuxcnc.org/docs/2.4/html/gcode_main.html#sec:M50:-Feed-Override
- */
-
-/// M48, M49
-void mach_override_enables(bool flag) {
- mach.gm.feed_override_enable = flag;
- mach.gm.spindle_override_enable = flag;
-}
-
-
-/// M50
-void mach_feed_override_enable(bool flag) {
- if (parser.gf.parameter && fp_ZERO(parser.gn.parameter))
- mach.gm.feed_override_enable = false;
- else {
- if (parser.gf.parameter) mach.gm.feed_override = parser.gf.parameter;
- mach.gm.feed_override_enable = true;
- }
-}
-
-
-/// M51
-void mach_spindle_override_enable(bool flag) {
- if (parser.gf.parameter && fp_ZERO(parser.gn.parameter))
- mach.gm.spindle_override_enable = false;
- else {
- if (parser.gf.parameter) mach.gm.spindle_override = parser.gf.parameter;
- mach.gm.spindle_override_enable = true;
- }
-}
-
-
-void mach_message(const char *message) {
- status_message_P(0, STAT_LEVEL_INFO, STAT_OK, PSTR("%s"), message);
-}
-
-
-/* Program Functions (4.3.10)
- *
- * This group implements stop, start, end, and hold.
- * It is extended beyond the NIST spec to handle various situations.
- *
- * mach_program_stop and mach_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).
- *
- * mach_program_end is a stop that also resets the machine to initial state
- */
-
-
-static stat_t _exec_program_stop(mp_buffer_t *bf) {
- // Machine should be stopped at this point. Go into hold so that a start is
- // needed before executing further instructions.
- mp_set_hold_reason(HOLD_REASON_PROGRAM_PAUSE);
- mp_state_holding();
- return STAT_NOOP; // No move queued
-}
-
-
-/// M0 Queue a program stop
-void mach_program_stop() {
- mp_queue_push(_exec_program_stop, mach_get_line());
-}
-
-
-static stat_t _exec_optional_program_stop(mp_buffer_t *bf) {
- mp_state_optional_pause(); // Pause here if it was requested by the user
- return STAT_NOOP; // No move queued
-}
-
-
-/// M1
-void mach_optional_program_stop() {
- mp_queue_push(_exec_optional_program_stop, mach_get_line());
-}
-
-
-static stat_t _exec_pallet_change_stop(mp_buffer_t *bf) {
- // Emit pallet change signal
- mp_set_hold_reason(HOLD_REASON_PALLET_CHANGE);
- mp_state_holding();
- return STAT_NOOP; // No move queued
-}
-
-
-/// M60
-void mach_pallet_change_stop() {
- mp_queue_push(_exec_pallet_change_stop, mach_get_line());
-}
-
-
-/*** mach_program_end() implements M2 and M30. End behaviors are defined by
- * NIST 3.6.1 are:
- *
- * 1. Axis offsets are set to zero (like G92.2) and origin offsets are set
- * to the default (like G54)
- * 2. Selected plane is set to 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)
- * 5. Feed and speed overrides are set to ON (like M48)
- * 6. Cutter compensation is turned off (like G40)
- * 7. The spindle is stopped (like M5)
- * 8. The current motion mode is set to G_1 (like G1)
- * 9. Coolant is turned off (like M9)
- *
- * mach_program_end() implements things slightly differently:
- *
- * 1. Axis offsets are set to G92.1 CANCEL offsets
- * (instead of using G92.2 SUSPEND Offsets)
- * Set default coordinate system
- * 2. Selected plane is set to default plane
- * 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
- * 7. The spindle is stopped (like M5)
- * 8. Motion mode is canceled like G80 (not set to G1)
- * 9. Coolant is turned off (like M9)
- * + Default INCHES or MM units mode is restored
- */
-
-
-/// M2, M30
-void mach_program_end() {
- mach_reset_origin_offsets(); // G92.1 - we do G91.1 instead of G92.2
- mach_set_coord_system(GCODE_DEFAULT_COORD_SYSTEM);
- mach_set_plane(GCODE_DEFAULT_PLANE);
- mach_set_distance_mode(GCODE_DEFAULT_DISTANCE_MODE);
- mach_set_arc_distance_mode(GCODE_DEFAULT_ARC_DISTANCE_MODE);
- mach_set_spindle_mode(SPINDLE_OFF); // M5
- mach_flood_coolant_control(false); // M9
- mach_set_feed_mode(UNITS_PER_MINUTE_MODE); // G94
- mach_set_motion_mode(MOTION_MODE_CANCEL_MOTION_MODE);
-}
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2016 Buildbotics LLC
- Copyright (c) 2010 - 2015 Alden S. Hart, Jr.
- All rights reserved.
-
- 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/>.
-
- The software is distributed in the hope that it will be useful, but
- WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
- Lesser General Public License for more details.
-
- You should have received a copy of the GNU Lesser General Public
- License along with the software. If not, see
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-#pragma once
-
-
-#include "config.h"
-#include "status.h"
-#include "gcode_state.h"
-
-
-#define TO_MM(a) (mach_get_units() == INCHES ? (a) * MM_PER_INCH : a)
-
-// Model state getters and setters
-uint32_t mach_get_line();
-float mach_get_feed_rate();
-bool mach_is_inverse_time_mode();
-feed_mode_t mach_get_feed_mode();
-float mach_get_feed_override();
-float mach_get_spindle_override();
-motion_mode_t mach_get_motion_mode();
-bool mach_is_rapid();
-plane_t mach_get_plane();
-units_t mach_get_units();
-coord_system_t mach_get_coord_system();
-bool mach_get_absolute_mode();
-path_mode_t mach_get_path_mode();
-bool mach_is_exact_stop();
-distance_mode_t mach_get_distance_mode();
-distance_mode_t mach_get_arc_distance_mode();
-
-void mach_set_motion_mode(motion_mode_t motion_mode);
-void mach_set_spindle_mode(spindle_mode_t spindle_mode);
-void mach_set_spindle_speed(float speed);
-void mach_set_absolute_mode(bool absolute_mode);
-void mach_set_line(uint32_t line);
-
-// Coordinate systems and offsets
-float mach_get_active_coord_offset(uint8_t axis);
-void mach_update_work_offsets();
-const float *mach_get_position();
-void mach_set_position(const float position[]);
-float mach_get_axis_position(uint8_t axis);
-
-// Critical helpers
-void mach_calc_target(float target[], const float values[], const bool flags[]);
-stat_t mach_test_soft_limits(float target[]);
-
-// machining functions defined by NIST [organized by NIST Gcode doc]
-
-// Initialization and termination (4.3.2)
-void machine_init();
-
-// Representation (4.3.3)
-void mach_set_plane(plane_t plane);
-void mach_set_units(units_t mode);
-void mach_set_distance_mode(distance_mode_t mode);
-void mach_set_arc_distance_mode(distance_mode_t mode);
-void mach_set_coord_offsets(coord_system_t coord_system, float offset[],
- bool flags[]);
-
-void mach_set_axis_position(unsigned axis, float position);
-void mach_set_absolute_origin(float origin[], bool flags[]);
-
-stat_t mach_zero_all();
-stat_t mach_zero_axis(unsigned axis);
-
-void mach_set_coord_system(coord_system_t coord_system);
-void mach_set_origin_offsets(float offset[], bool flags[]);
-void mach_reset_origin_offsets();
-void mach_suspend_origin_offsets();
-void mach_resume_origin_offsets();
-
-// Free Space Motion (4.3.4)
-stat_t mach_plan_line(float target[]);
-stat_t mach_rapid(float target[], bool flags[]);
-void mach_set_g28_position();
-stat_t mach_goto_g28_position(float target[], bool flags[]);
-void mach_set_g30_position();
-stat_t mach_goto_g30_position(float target[], bool flags[]);
-
-// Machining Attributes (4.3.5)
-void mach_set_feed_rate(float feed_rate);
-void mach_set_feed_mode(feed_mode_t mode);
-void mach_set_path_mode(path_mode_t mode);
-
-// Machining Functions (4.3.6) see arc.h
-stat_t mach_feed(float target[], bool flags[]);
-stat_t mach_dwell(float seconds);
-
-// Spindle Functions (4.3.7) see spindle.h
-
-// Tool Functions (4.3.8)
-void mach_select_tool(uint8_t tool);
-void mach_change_tool(bool x);
-
-// Miscellaneous Functions (4.3.9)
-void mach_mist_coolant_control(bool mist_coolant);
-void mach_flood_coolant_control(bool flood_coolant);
-
-void mach_override_enables(bool flag);
-void mach_feed_override_enable(bool flag);
-void mach_spindle_override_enable(bool flag);
-
-void mach_message(const char *message);
-
-// Program Functions (4.3.10)
-void mach_program_stop();
-void mach_optional_program_stop();
-void mach_pallet_change_stop();
-void mach_program_end();
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2016 Buildbotics LLC
- Copyright (c) 2010 - 2015 Alden S. Hart, Jr.
- Copyright (c) 2013 - 2015 Robert Giseburt
- All rights reserved.
-
- 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/>.
-
- The software is distributed in the hope that it will be useful, but
- WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
- Lesser General Public License for more details.
-
- You should have received a copy of the GNU Lesser General Public
- License along with the software. If not, see
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-#include "hardware.h"
-#include "machine.h"
-#include "stepper.h"
-#include "motor.h"
-#include "switch.h"
-#include "usart.h"
-#include "drv8711.h"
-#include "vars.h"
-#include "rtc.h"
-#include "report.h"
-#include "command.h"
-#include "estop.h"
-#include "probing.h"
-#include "homing.h"
-#include "home.h"
-#include "i2c.h"
-
-#include "plan/planner.h"
-#include "plan/arc.h"
-#include "plan/state.h"
-
-#include <avr/pgmspace.h>
-#include <avr/wdt.h>
-
-#include <stdio.h>
-#include <stdbool.h>
-
-
-int main() {
- //wdt_enable(WDTO_250MS); TODO
-
- // Init
- cli(); // disable interrupts
-
- hardware_init(); // hardware setup - must be first
- usart_init(); // serial port
- i2c_init(); // i2c port
- drv8711_init(); // motor drivers
- stepper_init(); // steppers
- motor_init(); // motors
- switch_init(); // switches
- mp_init(); // motion planning
- machine_init(); // gcode machine
- vars_init(); // configuration variables
- estop_init(); // emergency stop handler
- command_init();
-
- sei(); // enable interrupts
-
- // Splash
- fprintf_P(stdout, PSTR("\n{\"firmware\": \"Buildbotics AVR\", "
- "\"version\": \"" VERSION "\"}\n"));
-
- // Nominal segment time cannot be longer than maximum
- if (MAX_SEGMENT_TIME < NOM_SEGMENT_TIME) ALARM(STAT_INTERNAL_ERROR);
-
- // Main loop
- while (true) {
- hw_reset_handler(); // handle hard reset requests
- if (!estop_triggered()) {
- mp_state_callback();
- mach_arc_callback(); // arc generation runs
- home_callback();
- //mach_homing_callback(); // G28.2 continuation
- mach_probe_callback(); // G38.2 continuation
- }
- command_callback(); // process next command
- report_callback(); // report changes
- wdt_reset();
- }
-
- return 0;
-}
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2016 Buildbotics LLC
- All rights reserved.
-
- 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/>.
-
- The software is distributed in the hope that it will be useful, but
- WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
- Lesser General Public License for more details.
-
- You should have received a copy of the GNU Lesser General Public
- License along with the software. If not, see
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-// OS, communications and low-level status
-STAT_MSG(OK, "OK")
-STAT_MSG(EAGAIN, "Run command again")
-STAT_MSG(NOOP, "No op")
-STAT_MSG(COMPLETE, "Complete")
-STAT_MSG(BUSY, "Busy")
-STAT_MSG(NO_SUCH_DEVICE, "No such device")
-STAT_MSG(BUFFER_FULL, "Buffer full")
-STAT_MSG(BUFFER_FULL_FATAL, "Buffer full - fatal")
-STAT_MSG(EEPROM_DATA_INVALID, "EEPROM data invalid")
-STAT_MSG(STEP_CHECK_FAILED, "Step check failed")
-STAT_MSG(MACH_NOT_QUIESCENT, "Cannot perform action while machine active")
-STAT_MSG(INTERNAL_ERROR, "Internal error")
-
-STAT_MSG(MOTOR_STALLED, "Motor stalled")
-STAT_MSG(MOTOR_OVER_TEMP, "Motor over temperature")
-STAT_MSG(MOTOR_OVER_CURRENT, "Motor over temperature")
-STAT_MSG(MOTOR_DRIVER_FAULT, "Motor driver fault")
-STAT_MSG(MOTOR_UNDER_VOLTAGE, "Motor under voltage")
-
-STAT_MSG(PREP_LINE_MOVE_TIME_INFINITE, "Move time is infinite")
-STAT_MSG(PREP_LINE_MOVE_TIME_NAN, "Move time is NAN")
-STAT_MSG(MOVE_TARGET_INFINITE, "Move target is infinite")
-STAT_MSG(MOVE_TARGET_NAN, "Move target is NAN")
-STAT_MSG(EXCESSIVE_MOVE_ERROR, "Excessive move error")
-
-STAT_MSG(ESTOP_USER, "User triggered EStop")
-STAT_MSG(ESTOP_SWITCH, "Switch triggered EStop")
-
-// Generic data input errors
-STAT_MSG(UNRECOGNIZED_NAME, "Unrecognized command or variable name")
-STAT_MSG(INVALID_OR_MALFORMED_COMMAND, "Invalid or malformed command")
-STAT_MSG(TOO_MANY_ARGUMENTS, "Too many arguments to command")
-STAT_MSG(TOO_FEW_ARGUMENTS, "Too few arguments to command")
-STAT_MSG(BAD_NUMBER_FORMAT, "Bad number format")
-STAT_MSG(PARAMETER_IS_READ_ONLY, "Parameter is read-only")
-STAT_MSG(PARAMETER_CANNOT_BE_READ, "Parameter cannot be read")
-STAT_MSG(COMMAND_NOT_ACCEPTED, "Command not accepted at this time")
-STAT_MSG(INPUT_EXCEEDS_MAX_LENGTH, "Input exceeds max length")
-STAT_MSG(INPUT_LESS_THAN_MIN_VALUE, "Input less than minimum value")
-STAT_MSG(INPUT_EXCEEDS_MAX_VALUE, "Input exceeds maximum value")
-STAT_MSG(INPUT_VALUE_RANGE_ERROR, "Input value range error")
-
-// Gcode errors & warnings (Most originate from NIST)
-STAT_MSG(MODAL_GROUP_VIOLATION, "Modal group violation")
-STAT_MSG(GCODE_COMMAND_UNSUPPORTED, "Invalid or unsupported G-Code command")
-STAT_MSG(MCODE_COMMAND_UNSUPPORTED, "M code unsupported")
-STAT_MSG(GCODE_AXIS_IS_MISSING, "Axis word missing")
-STAT_MSG(GCODE_FEEDRATE_NOT_SPECIFIED, "Feedrate not specified")
-STAT_MSG(ARC_SPECIFICATION_ERROR, "Arc specification error")
-STAT_MSG(ARC_AXIS_MISSING_FOR_SELECTED_PLANE, "Arc missing axis")
-STAT_MSG(ARC_RADIUS_OUT_OF_TOLERANCE, "Arc radius arc out of tolerance")
-STAT_MSG(ARC_ENDPOINT_IS_STARTING_POINT, "Arc end point is starting point")
-STAT_MSG(ARC_OFFSETS_MISSING_FOR_PLANE, "arc offsets missing for plane")
-STAT_MSG(P_WORD_IS_NOT_A_POSITIVE_INTEGER, "P word is not a positive integer")
-
-// Errors and warnings
-STAT_MSG(MINIMUM_LENGTH_MOVE, "Move less than minimum length")
-STAT_MSG(MINIMUM_TIME_MOVE, "Move less than minimum time")
-STAT_MSG(MAXIMUM_TIME_MOVE, "Move greater than maximum time")
-STAT_MSG(MACHINE_ALARMED, "Machine alarmed - Command not processed")
-STAT_MSG(LIMIT_SWITCH_HIT, "Limit switch hit - Shutdown occurred")
-STAT_MSG(SOFT_LIMIT_EXCEEDED, "Soft limit exceeded")
-STAT_MSG(INVALID_AXIS, "Invalid axis")
-STAT_MSG(EXPECTED_MOVE, "A move was expected but none was queued")
-
-// Homing
-STAT_MSG(HOMING_CYCLE_FAILED, "Homing cycle failed")
-STAT_MSG(HOMING_ERROR_BAD_OR_NO_AXIS, "Homing Error - Bad or no axis specified")
-STAT_MSG(HOMING_ERROR_AXIS_MISCONFIGURED, "Homing Error - Axis misconfigured")
-STAT_MSG(HOMING_ERROR_ZERO_SEARCH_VELOCITY,
- "Homing Error - Search velocity is zero")
-STAT_MSG(HOMING_ERROR_ZERO_LATCH_VELOCITY,
- "Homing Error - Latch velocity is zero")
-STAT_MSG(HOMING_ERROR_TRAVEL_MIN_MAX_IDENTICAL,
- "Homing Error - Travel min & max are the same")
-STAT_MSG(HOMING_ERROR_NEGATIVE_LATCH_BACKOFF,
- "Homing Error - Negative latch backoff")
-STAT_MSG(HOMING_ERROR_SWITCH_MISCONFIGURATION,
- "Homing Error - Homing switches misconfigured")
-
-// Probing
-STAT_MSG(PROBE_INVALID_DEST, "Probing error - invalid destination")
-STAT_MSG(MOVE_DURING_PROBE, "Probing error - move during probe")
-
-// End of stats marker
-STAT_MSG(MAX, "")
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2016 Buildbotics LLC
- All rights reserved.
-
- 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/>.
-
- The software is distributed in the hope that it will be useful, but
- WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
- Lesser General Public License for more details.
-
- You should have received a copy of the GNU Lesser General Public
- License along with the software. If not, see
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-#include "motor.h"
-#include "config.h"
-#include "hardware.h"
-#include "cpp_magic.h"
-#include "rtc.h"
-#include "report.h"
-#include "stepper.h"
-#include "drv8711.h"
-#include "estop.h"
-#include "gcode_state.h"
-#include "axis.h"
-#include "util.h"
-
-#include "plan/runtime.h"
-#include "plan/calibrate.h"
-
-#include <avr/interrupt.h>
-#include <avr/pgmspace.h>
-
-#include <string.h>
-#include <math.h>
-#include <stdio.h>
-#include <stdlib.h>
-
-
-typedef enum {
- MOTOR_IDLE, // motor stopped and may be partially energized
- MOTOR_ENERGIZING,
- MOTOR_ACTIVE
-} motor_power_state_t;
-
-
-typedef struct {
- // Config
- uint8_t axis; // map motor to axis
- uint16_t microsteps; // microsteps per full step
- bool reverse;
- motor_power_mode_t power_mode;
- float step_angle; // degrees per whole step
- float travel_rev; // mm or deg of travel per motor revolution
- uint8_t step_pin;
- uint8_t dir_pin;
- TC0_t *timer;
- DMA_CH_t *dma;
- uint8_t dma_trigger;
-
- // Runtime state
- motor_power_state_t power_state; // state machine for managing motor power
- uint32_t timeout;
- motor_flags_t flags;
- int32_t encoder;
- int32_t commanded;
- int32_t steps; // Currently used by the x-axis only
- uint8_t last_clock;
- bool wasEnabled;
- int32_t error;
- bool last_negative; // Last step sign
- bool reading;
-
- // Move prep
- uint8_t timer_clock; // clock divisor setting or zero for off
- uint16_t timer_period; // clock period counter
- bool negative; // step sign
- direction_t direction; // travel direction corrected for polarity
- int32_t position;
- bool round_up; // toggle rounding direction
- float power;
-} motor_t;
-
-
-static motor_t motors[MOTORS] = {
- {
- .axis = M1_AXIS,
- .step_angle = M1_STEP_ANGLE,
- .travel_rev = M1_TRAVEL_PER_REV,
- .microsteps = M1_MICROSTEPS,
- .reverse = M1_REVERSE,
- .power_mode = M1_POWER_MODE,
- .step_pin = STEP_X_PIN,
- .dir_pin = DIR_X_PIN,
- .timer = &M1_TIMER,
- .dma = &M1_DMA_CH,
- .dma_trigger = M1_DMA_TRIGGER,
- }, {
- .axis = M2_AXIS,
- .step_angle = M2_STEP_ANGLE,
- .travel_rev = M2_TRAVEL_PER_REV,
- .microsteps = M2_MICROSTEPS,
- .reverse = M2_REVERSE,
- .power_mode = M2_POWER_MODE,
- .step_pin = STEP_Y_PIN,
- .dir_pin = DIR_Y_PIN,
- .timer = &M2_TIMER,
- .dma = &M2_DMA_CH,
- .dma_trigger = M2_DMA_TRIGGER,
- }, {
- .axis = M3_AXIS,
- .step_angle = M3_STEP_ANGLE,
- .travel_rev = M3_TRAVEL_PER_REV,
- .microsteps = M3_MICROSTEPS,
- .reverse = M3_REVERSE,
- .power_mode = M3_POWER_MODE,
- .step_pin = STEP_Z_PIN,
- .dir_pin = DIR_Z_PIN,
- .timer = &M3_TIMER,
- .dma = &M3_DMA_CH,
- .dma_trigger = M3_DMA_TRIGGER,
- }, {
- .axis = M4_AXIS,
- .step_angle = M4_STEP_ANGLE,
- .travel_rev = M4_TRAVEL_PER_REV,
- .microsteps = M4_MICROSTEPS,
- .reverse = M4_REVERSE,
- .power_mode = M4_POWER_MODE,
- .step_pin = STEP_A_PIN,
- .dir_pin = DIR_A_PIN,
- .timer = (TC0_t *)&M4_TIMER,
- .dma = &M4_DMA_CH,
- .dma_trigger = M4_DMA_TRIGGER,
- }
-};
-
-
-static uint8_t _dummy;
-
-
-void motor_init() {
- // Enable DMA
- DMA.CTRL = DMA_RESET_bm;
- DMA.CTRL = DMA_ENABLE_bm;
- DMA.INTFLAGS = 0xff; // clear all interrupts
-
- // Motor enable
- OUTSET_PIN(MOTOR_ENABLE_PIN); // Low (disabled)
- DIRSET_PIN(MOTOR_ENABLE_PIN); // Output
-
- for (int motor = 0; motor < MOTORS; motor++) {
- motor_t *m = &motors[motor];
-
- axis_set_motor(m->axis, motor);
-
- // IO pins
- DIRSET_PIN(m->step_pin); // Output
- DIRSET_PIN(m->dir_pin); // Output
-
- // Setup motor timer
- m->timer->CTRLB = TC_WGMODE_FRQ_gc | TC1_CCAEN_bm;
-
- // Setup DMA channel as timer event counter
- m->dma->ADDRCTRL = DMA_CH_SRCDIR_FIXED_gc | DMA_CH_DESTDIR_FIXED_gc;
- m->dma->TRIGSRC = m->dma_trigger;
- m->dma->REPCNT = 0;
-
- // Note, the DMA transfer must read CCA to clear the trigger
- m->dma->SRCADDR0 = (((uintptr_t)&m->timer->CCA) >> 0) & 0xff;
- m->dma->SRCADDR1 = (((uintptr_t)&m->timer->CCA) >> 8) & 0xff;
- m->dma->SRCADDR2 = 0;
-
- m->dma->DESTADDR0 = (((uintptr_t)&_dummy) >> 0) & 0xff;
- m->dma->DESTADDR1 = (((uintptr_t)&_dummy) >> 8) & 0xff;
- m->dma->DESTADDR2 = 0;
-
- m->dma->CTRLB = 0;
- m->dma->CTRLA =
- DMA_CH_REPEAT_bm | DMA_CH_SINGLE_bm | DMA_CH_BURSTLEN_1BYTE_gc;
- m->dma->CTRLA |= DMA_CH_ENABLE_bm;
-
- drv8711_set_microsteps(motor, m->microsteps);
- }
-}
-
-
-void motor_enable(int motor, bool enable) {
- if (enable) OUTSET_PIN(MOTOR_ENABLE_PIN); // Active high
- else {
- OUTCLR_PIN(MOTOR_ENABLE_PIN);
- motors[motor].power_state = MOTOR_IDLE;
- }
-}
-
-
-bool motor_is_enabled(int motor) {
- return motors[motor].power_mode != MOTOR_DISABLED;
-}
-
-
-int motor_get_axis(int motor) {return motors[motor].axis;}
-
-
-void motor_set_stall_callback(int motor, stall_callback_t cb) {
- drv8711_set_stall_callback(motor, cb);
-}
-
-
-/// @return computed homing velocity
-float motor_get_stall_homing_velocity(int motor) {
- // Compute velocity:
- // velocity = travel_rev * step_angle * 60 / (SMPLTH * mstep * 360 * 2)
- // SMPLTH = 50us = 0.00005s
- // mstep = 8
- return motors[motor].travel_rev * motors[motor].step_angle * 1667 /
- motors[motor].microsteps;
-}
-
-
-float motor_get_steps_per_unit(int motor) {
- return 360 * motors[motor].microsteps / motors[motor].travel_rev /
- motors[motor].step_angle;
-}
-
-
-float motor_get_units_per_step(int motor) {
- return motors[motor].travel_rev * motors[motor].step_angle /
- motors[motor].microsteps / 360;
-}
-
-
-float _get_max_velocity(int motor) {
- return
- axis_get_velocity_max(motors[motor].axis) * motor_get_steps_per_unit(motor);
-}
-
-
-uint16_t motor_get_microsteps(int motor) {return motors[motor].microsteps;}
-
-
-void motor_set_microsteps(int motor, uint16_t microsteps) {
- switch (microsteps) {
- case 1: case 2: case 4: case 8: case 16: case 32: case 64: case 128: case 256:
- break;
- default: return;
- }
-
- motors[motor].microsteps = microsteps;
- drv8711_set_microsteps(motor, microsteps);
-}
-
-
-int32_t motor_get_encoder(int motor) {return motors[motor].encoder;}
-
-
-void motor_set_encoder(int motor, float encoder) {
- //if (st_is_busy()) ALARM(STAT_INTERNAL_ERROR); TODO
-
- motor_t *m = &motors[motor];
- m->encoder = m->position = m->commanded = round(encoder);
- m->error = 0;
-}
-
-
-int32_t motor_get_error(int motor) {
- motor_t *m = &motors[motor];
-
- m->reading = true;
- int32_t error = motors[motor].error;
- m->reading = false;
-
- return error;
-}
-
-
-int32_t motor_get_position(int motor) {return motors[motor].position;}
-
-
-/// returns true if motor is in an error state
-bool motor_error(int m) {
- uint8_t flags = motors[m].flags;
- if (calibrate_busy()) flags &= ~MOTOR_FLAG_STALLED_bm;
- return flags & MOTOR_FLAG_ERROR_bm;
-}
-
-
-bool motor_stalled(int m) {
- return motors[m].flags & MOTOR_FLAG_STALLED_bm;
-}
-
-
-void motor_reset(int m) {
- motors[m].flags = 0;
-}
-
-
-/// Remove power from a motor
-static void _deenergize(int m) {
- if (motors[m].power_state == MOTOR_ACTIVE) {
- motors[m].power_state = MOTOR_IDLE;
- drv8711_disable(m);
- }
-}
-
-
-/// Apply power to a motor
-static void _energize(int m) {
- if (motors[m].power_state == MOTOR_IDLE && !motor_error(m)) {
- motors[m].power_state = MOTOR_ENERGIZING;
- drv8711_enable(m);
-
- motor_driver_callback(m); // TODO Shouldn't call this directly
- }
-
- // Reset timeout, regardless
- motors[m].timeout = rtc_get_time() + MOTOR_IDLE_TIMEOUT * 1000;
-}
-
-
-bool motor_energizing() {
- for (int m = 0; m < MOTORS; m++)
- if (motors[m].power_state == MOTOR_ENERGIZING)
- return true;
-
- return false;
-}
-
-
-void motor_driver_callback(int motor) {
- motor_t *m = &motors[motor];
-
- if (m->power_state == MOTOR_IDLE) m->flags &= ~MOTOR_FLAG_ENABLED_bm;
- else if (!estop_triggered()) {
- m->power_state = MOTOR_ACTIVE;
- m->flags |= MOTOR_FLAG_ENABLED_bm;
- motor_enable(motor, true);
- }
-
- report_request();
-}
-
-
-/// Callback to manage motor power sequencing and power-down timing.
-stat_t motor_rtc_callback() { // called by controller
- for (int motor = 0; motor < MOTORS; motor++)
- // Deenergize motor if disabled or timedout
- if (motors[motor].power_mode == MOTOR_DISABLED ||
- rtc_expired(motors[motor].timeout)) _deenergize(motor);
-
- return STAT_OK;
-}
-
-
-void motor_error_callback(int motor, motor_flags_t errors) {
- motor_t *m = &motors[motor];
-
- if (m->power_state != MOTOR_ACTIVE) return;
-
- m->flags |= errors & MOTOR_FLAG_ERROR_bm;
- report_request();
-
- if (motor_error(motor)) {
- if (m->flags & MOTOR_FLAG_STALLED_bm) ALARM(STAT_MOTOR_STALLED);
- if (m->flags & MOTOR_FLAG_OVER_TEMP_bm) ALARM(STAT_MOTOR_OVER_TEMP);
- if (m->flags & MOTOR_FLAG_OVER_CURRENT_bm) ALARM(STAT_MOTOR_OVER_CURRENT);
- if (m->flags & MOTOR_FLAG_DRIVER_FAULT_bm) ALARM(STAT_MOTOR_DRIVER_FAULT);
- if (m->flags & MOTOR_FLAG_UNDER_VOLTAGE_bm) ALARM(STAT_MOTOR_UNDER_VOLTAGE);
- }
-}
-
-
-void motor_load_move(int motor) {
- motor_t *m = &motors[motor];
-
- // Get actual step count from DMA channel
- uint16_t steps = 0xffff - m->dma->TRFCNT;
- m->dma->TRFCNT = 0xffff; // Reset DMA channel counter
- m->dma->CTRLB = DMA_CH_CHBUSY_bm | DMA_CH_CHPEND_bm;
- m->dma->CTRLA |= DMA_CH_ENABLE_bm;
-
- // Adjust clock count
- if (m->last_clock) {
- uint32_t count = m->timer->CNT;
- int8_t freq_change = m->last_clock - m->timer_clock;
-
- count <<= freq_change; // Adjust count
-
- if (m->timer_period < count) count -= m->timer_period;
- if (m->timer_period < count) count -= m->timer_period;
- if (m->timer_period < count) count = m->timer_period / 2;
-
- m->timer->CNT = count;
-
- } else m->timer->CNT = m->timer_period / 2;
-
- // Set or zero runtime clock and period
- m->timer->CCA = m->timer_period; // Set frequency
- m->timer->CTRLA = m->timer_clock; // Start or stop
- m->last_clock = m->timer_clock;
- m->timer_clock = 0; // Clear clock
-
- // Set direction
- if (m->direction == DIRECTION_CW) OUTCLR_PIN(m->dir_pin);
- else OUTSET_PIN(m->dir_pin);
-
- // Accumulate encoder
- if (!m->wasEnabled) steps = 0;
-
- m->encoder += m->last_negative ? -(int32_t)steps : steps;
- m->last_negative = m->negative;
-
- // Compute error
- if (!m->reading) m->error = m->commanded - m->encoder;
- m->commanded = m->position;
-
- // Set power
- drv8711_set_power(motor, m->power);
-}
-
-
-void motor_end_move(int motor) {
- motor_t *m = &motors[motor];
- m->wasEnabled = m->dma->CTRLA & DMA_CH_ENABLE_bm;
- m->dma->CTRLA &= ~DMA_CH_ENABLE_bm;
- m->timer->CTRLA = 0; // Stop clock
-}
-
-
-stat_t motor_prep_move(int motor, int32_t clocks, float target, int32_t error,
- float time) {
- motor_t *m = &motors[motor];
-
- // Validate input
- if (motor < 0 || MOTORS < motor) return ALARM(STAT_INTERNAL_ERROR);
- if (clocks < 0) return ALARM(STAT_INTERNAL_ERROR);
- if (isinf(target)) return ALARM(STAT_MOVE_TARGET_INFINITE);
- if (isnan(target)) return ALARM(STAT_MOVE_TARGET_NAN);
-
- // Compute motor timer clock and period. Rounding is performed to eliminate
- // a negative bias in the uint32_t conversion that results in long-term
- // negative drift.
- int32_t travel = round(target) - m->position + error;
- uint32_t ticks_per_step = travel ? labs(clocks / 2 / travel) : 0;
- m->position = round(target);
-
- // Setup the direction, compensating for polarity.
- m->negative = travel < 0;
- if (m->negative) m->direction = DIRECTION_CCW ^ m->reverse;
- else m->direction = DIRECTION_CW ^ m->reverse;
-
- // Find the clock rate that will fit the required number of steps
- if (ticks_per_step <= 0xffff) m->timer_clock = TC_CLKSEL_DIV1_gc;
- else if (ticks_per_step <= 0x1ffff) m->timer_clock = TC_CLKSEL_DIV2_gc;
- else if (ticks_per_step <= 0x3ffff) m->timer_clock = TC_CLKSEL_DIV4_gc;
- else if (ticks_per_step <= 0x7ffff) m->timer_clock = TC_CLKSEL_DIV8_gc;
- else m->timer_clock = 0; // Clock off, too slow
-
- // Note, we rely on the fact that TC_CLKSEL_DIV1_gc through TC_CLKSEL_DIV8_gc
- // equal 1, 2, 3 & 4 respectively.
- m->timer_period = ticks_per_step >> (m->timer_clock - 1);
-
- // Round up if DIV4 or DIV8 and the error is high enough
- if (0xffff < ticks_per_step && m->timer_period < 0xffff) {
- int8_t step_error = ticks_per_step & ((1 << (m->timer_clock - 1)) - 1);
- int8_t half_error = 1 << (m->timer_clock - 2);
-
- if (step_error == half_error) {
- if (m->round_up) m->timer_period++;
- m->round_up = !m->round_up;
-
- } else if (half_error < step_error) m->timer_period++;
- }
-
- if (!m->timer_period) m->timer_clock = 0;
- if (!m->timer_clock) m->timer_period = 0;
-
- // Power motor
- switch (m->power_mode) {
- case MOTOR_DISABLED: break;
-
- case MOTOR_POWERED_ONLY_WHEN_MOVING:
- if (!m->timer_clock) break; // Not moving
- // Fall through
-
- case MOTOR_ALWAYS_POWERED: case MOTOR_POWERED_IN_CYCLE:
- _energize(motor); // TODO is ~5ms enough time to enable the motor?
- break;
-
- default: break; // Shouldn't get here
- }
-
- // Compute power from axis velocity
- m->power = travel / (_get_max_velocity(motor) * time);
-
- return STAT_OK;
-}
-
-
-// Var callbacks
-char get_axis_name(int motor) {return axis_get_char(motors[motor].axis);}
-
-
-void set_axis_name(int motor, char axis) {
- int id = axis_get_id(axis);
- if (id != -1) motors[motor].axis = id;
-}
-
-
-float get_step_angle(int motor) {return motors[motor].step_angle;}
-void set_step_angle(int motor, float value) {motors[motor].step_angle = value;}
-float get_travel(int motor) {return motors[motor].travel_rev;}
-void set_travel(int motor, float value) {motors[motor].travel_rev = value;}
-uint16_t get_microstep(int motor) {return motors[motor].microsteps;}
-
-
-void set_microstep(int motor, uint16_t value) {
- if (motor < 0 || MOTORS <= motor) return;
- motor_set_microsteps(motor, value);
-}
-
-
-bool get_reverse(int motor) {
- if (motor < 0 || MOTORS <= motor) return 0;
- return motors[motor].reverse;
-}
-
-
-void set_reverse(int motor, bool value) {motors[motor].reverse = value;}
-uint8_t get_motor_axis(int motor) {return motors[motor].axis;}
-
-
-void set_motor_axis(int motor, uint8_t value) {
- if (MOTORS <= motor || AXES <= value || value == motors[motor].axis) return;
- axis_set_motor(motors[motor].axis, -1);
- motors[motor].axis = value;
- axis_set_motor(value, motor);
-}
-
-
-uint8_t get_power_mode(int motor) {return motors[motor].power_mode;}
-
-
-void set_power_mode(int motor, uint8_t value) {
- if (value < MOTOR_POWER_MODE_MAX_VALUE)
- motors[motor].power_mode = value;
-}
-
-
-uint8_t get_status_flags(int motor) {return motors[motor].flags;}
-
-
-void print_status_flags(uint8_t flags) {
- bool first = true;
-
- putchar('"');
-
- if (MOTOR_FLAG_ENABLED_bm & flags) {
- printf_P(PSTR("enable"));
- first = false;
- }
-
- if (MOTOR_FLAG_STALLED_bm & flags) {
- if (!first) printf_P(PSTR(", "));
- printf_P(PSTR("stall"));
- first = false;
- }
-
- if (MOTOR_FLAG_OVER_TEMP_bm & flags) {
- if (!first) printf_P(PSTR(", "));
- printf_P(PSTR("over temp"));
- first = false;
- }
-
- if (MOTOR_FLAG_OVER_CURRENT_bm & flags) {
- if (!first) printf_P(PSTR(", "));
- printf_P(PSTR("over current"));
- first = false;
- }
-
- if (MOTOR_FLAG_DRIVER_FAULT_bm & flags) {
- if (!first) printf_P(PSTR(", "));
- printf_P(PSTR("fault"));
- first = false;
- }
-
- if (MOTOR_FLAG_UNDER_VOLTAGE_bm & flags) {
- if (!first) printf_P(PSTR(", "));
- printf_P(PSTR("uvlo"));
- first = false;
- }
-
- putchar('"');
-}
-
-
-uint8_t get_status_strings(int m) {return get_status_flags(m);}
-
-
-// Command callback
-void command_mreset(int argc, char *argv[]) {
- if (argc == 1)
- for (int m = 0; m < MOTORS; m++)
- motor_reset(m);
-
- else {
- int m = atoi(argv[1]);
- if (m < MOTORS) motor_reset(m);
- }
-
- report_request();
-}
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2016 Buildbotics LLC
- All rights reserved.
-
- 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/>.
-
- The software is distributed in the hope that it will be useful, but
- WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
- Lesser General Public License for more details.
-
- You should have received a copy of the GNU Lesser General Public
- License along with the software. If not, see
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-#pragma once
-
-#include "status.h"
-
-#include <stdint.h>
-#include <stdbool.h>
-
-
-typedef enum {
- MOTOR_FLAG_ENABLED_bm = 1 << 0,
- MOTOR_FLAG_STALLED_bm = 1 << 1,
- MOTOR_FLAG_OVER_TEMP_bm = 1 << 2,
- MOTOR_FLAG_OVER_CURRENT_bm = 1 << 3,
- MOTOR_FLAG_DRIVER_FAULT_bm = 1 << 4,
- MOTOR_FLAG_UNDER_VOLTAGE_bm = 1 << 5,
- MOTOR_FLAG_ERROR_bm = (//MOTOR_FLAG_STALLED_bm |
- MOTOR_FLAG_OVER_TEMP_bm |
- MOTOR_FLAG_OVER_CURRENT_bm |
- MOTOR_FLAG_DRIVER_FAULT_bm |
- MOTOR_FLAG_UNDER_VOLTAGE_bm)
-} motor_flags_t;
-
-
-typedef enum {
- MOTOR_DISABLED, // 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, // idles shortly after stopped, even in cycle
- MOTOR_POWER_MODE_MAX_VALUE // for input range checking
-} motor_power_mode_t;
-
-
-typedef void (*stall_callback_t)(int motor);
-
-
-void motor_init();
-void motor_enable(int motor, bool enable);
-
-bool motor_is_enabled(int motor);
-int motor_get_axis(int motor);
-void motor_set_stall_callback(int motor, stall_callback_t cb);
-float motor_get_stall_homing_velocity(int motor);
-float motor_get_steps_per_unit(int motor);
-float motor_get_units_per_step(int motor);
-uint16_t motor_get_microsteps(int motor);
-void motor_set_microsteps(int motor, uint16_t microsteps);
-int32_t motor_get_encoder(int motor);
-void motor_set_encoder(int motor, float encoder);
-int32_t motor_get_error(int motor);
-int32_t motor_get_position(int motor);
-
-bool motor_error(int motor);
-bool motor_stalled(int motor);
-void motor_reset(int motor);
-
-bool motor_energizing();
-
-void motor_driver_callback(int motor);
-stat_t motor_rtc_callback();
-void motor_error_callback(int motor, motor_flags_t errors);
-
-void motor_load_move(int motor);
-void motor_end_move(int motor);
-stat_t motor_prep_move(int motor, int32_t clocks, float target, int32_t error,
- float time);
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2016 Buildbotics LLC
- All rights reserved.
-
- 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/>.
-
- The software is distributed in the hope that it will be useful, but
- WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
- Lesser General Public License for more details.
-
- You should have received a copy of the GNU Lesser General Public
- License along with the software. If not, see
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-#include "pins.h"
-
-
-PORT_t *pin_ports[] = {&PORTA, &PORTB, &PORTC, &PORTD, &PORTE, &PORTF};
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2016 Buildbotics LLC
- All rights reserved.
-
- 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/>.
-
- The software is distributed in the hope that it will be useful, but
- WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
- Lesser General Public License for more details.
-
- You should have received a copy of the GNU Lesser General Public
- License along with the software. If not, see
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-#pragma once
-
-#include <avr/io.h>
-
-
-enum {PORT_A = 1, PORT_B, PORT_C, PORT_D, PORT_E, PORT_F};
-
-extern PORT_t *pin_ports[];
-
-#define PORT(PIN) pin_ports[(PIN >> 3) - 1]
-#define BM(PIN) (1 << (PIN & 7))
-
-#define DIRSET_PIN(PIN) PORT(PIN)->DIRSET = BM(PIN)
-#define DIRCLR_PIN(PIN) PORT(PIN)->DIRCLR = BM(PIN)
-#define OUTCLR_PIN(PIN) PORT(PIN)->OUTCLR = BM(PIN)
-#define OUTSET_PIN(PIN) PORT(PIN)->OUTSET = BM(PIN)
-#define OUTTGL_PIN(PIN) PORT(PIN)->OUTTGL = BM(PIN)
-#define OUT_PIN(PIN) (!!(PORT(PIN)->OUT & BM(PIN)))
-#define IN_PIN(PIN) (!!(PORT(PIN)->IN & BM(PIN)))
-#define PINCTRL_PIN(PIN) ((&PORT(PIN)->PIN0CTRL)[PIN & 7])
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2016 Buildbotics LLC
- Copyright (c) 2010 - 2015 Alden S. Hart, Jr.
- All rights reserved.
-
- 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/>.
-
- The software is distributed in the hope that it will be useful, but
- WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
- Lesser General Public License for more details.
-
- You should have received a copy of the GNU Lesser General Public
- License along with the software. If not, see
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-/* This module actually contains some parts that belong in the machine, and
- * other parts that belong at the motion planner level, but the whole thing is
- * treated as if it were part of the motion planner.
- */
-
-#include "arc.h"
-
-#include "axis.h"
-#include "buffer.h"
-#include "line.h"
-#include "gcode_parser.h"
-#include "config.h"
-#include "util.h"
-
-#include <math.h>
-#include <stdbool.h>
-#include <string.h>
-
-
-typedef struct {
- bool running;
-
- float target[AXES]; // XYZABC where the move should go
- float position[AXES]; // end point of the current segment
-
- float theta; // total angle specified by arc
- float radius; // Raw R value, or computed via offsets
-
- uint8_t plane_axis_0; // arc plane axis 0 - e.g. X for G17
- uint8_t plane_axis_1; // arc plane axis 1 - e.g. Y for G17
- uint8_t linear_axis; // linear axis (normal to plane)
-
- uint32_t segments; // count of running segments
- float segment_theta; // angular motion per segment
- float segment_linear_travel; // linear motion per segment
- float center_0; // center at axis 0 (e.g. X for G17)
- float center_1; // center at axis 1 (e.g. Y for G17)
-} arc_t;
-
-arc_t arc = {0};
-
-
-/*** Returns a naive estimate of arc execution time to inform segment
- * calculation. The arc time is computed not to exceed the time taken
- * in the slowest dimension in the arc plane or in linear
- * travel. Maximum feed rates are compared in each dimension, but the
- * comparison assumes that the arc will have at least one segment
- * where the unit vector is 1 in that dimension. This is not true for
- * any arbitrary arc, with the result that the time returned may be
- * less than optimal.
- */
-static float _estimate_arc_time(float length, float linear_travel,
- float planar_travel) {
- // Determine move time at requested feed rate
- // Inverse feed rate is normalized to minutes
- float time = mach_is_inverse_time_mode() ?
- mach_get_feed_rate() : length / mach_get_feed_rate();
-
-
- // Apply feed override
- time /= mach_get_feed_override();
-
- // Downgrade the time if there is a rate-limiting axis
- return max4(time, planar_travel / axis_get_feedrate_max(arc.plane_axis_0),
- planar_travel / axis_get_feedrate_max(arc.plane_axis_1),
- fabs(linear_travel) / axis_get_feedrate_max(arc.linear_axis));
-}
-
-
-/*** Compute arc center (offset) from radius.
- *
- * Needs to calculate the center of the circle that has the designated radius
- * and passes through both the current position and the target position
- *
- * This method calculates the following set of equations where:
- *
- * [x,y] is the vector from current to target position,
- * d == magnitude of that vector,
- * h == hypotenuse of the triangle formed by the radius of the circle,
- * the distance to the center of the travel vector.
- *
- * A vector perpendicular to the travel vector [-y,x] is scaled to the length
- * of h [-y/d*h, x/d*h] and added to the center of the travel vector [x/2,y/2]
- * to form the new point [i,j] at [x/2-y/d*h, y/2+x/d*h] which will be the
- * center of the arc.
- *
- * d^2 == x^2 + y^2
- * h^2 == r^2 - (d/2)^2
- * i == x/2 - y/d*h
- * j == y/2 + x/d*h
- * O <- [i,j]
- * - |
- * r - |
- * - |
- * - | h
- * - |
- * [0,0] -> C -----------------+--------------- T <- [x,y]
- * | <------ d/2 ---->|
- *
- * C - Current position
- * T - Target position
- * O - center of circle that pass through both C and T
- * d - distance from C to T
- * r - designated radius
- * h - distance from center of CT to O
- *
- * Expanding the equations:
- *
- * d -> sqrt(x^2 + y^2)
- * h -> sqrt(4 * r^2 - x^2 - y^2)/2
- * i -> (x - (y * sqrt(4 * r^2 - x^2 - y^2)) / sqrt(x^2 + y^2)) / 2
- * j -> (y + (x * sqrt(4 * r^2 - x^2 - y^2)) / sqrt(x^2 + y^2)) / 2
- *
- * Which can be written:
- *
- * i -> (x - (y * sqrt(4 * r^2 - x^2 - y^2))/sqrt(x^2 + y^2))/2
- * j -> (y + (x * sqrt(4 * r^2 - x^2 - y^2))/sqrt(x^2 + y^2))/2
- *
- * Which we for size and speed reasons optimize to:
- *
- * h_x2_div_d = sqrt(4 * r^2 - x^2 - y^2)/sqrt(x^2 + y^2)
- * i = (x - (y * h_x2_div_d))/2
- * j = (y + (x * h_x2_div_d))/2
- *
- * Computing clockwise vs counter-clockwise motion
- *
- * The counter clockwise circle lies to the left of the target direction.
- * When offset is positive the left hand circle will be generated -
- * when it is negative the right hand circle is generated.
- *
- * T <-- Target position
- * ^
- * Clockwise circles with | Clockwise circles with
- * this center will have | this center will have
- * > 180 deg of angular travel | < 180 deg of angular travel,
- * \ | which is a good thing!
- * \ | /
- * center of arc when -> x <----- | -----> x <- center of arc when
- * h_x2_div_d is positive | h_x2_div_d is negative
- * |
- * C <-- Current position
- *
- *
- * Assumes arc singleton has been pre-loaded with target and position.
- * Parts of this routine were originally sourced from the grbl project.
- */
-static stat_t _compute_arc_offsets_from_radius(float offset[], bool clockwise) {
- // Calculate the change in position along each selected axis
- float x =
- arc.target[arc.plane_axis_0] - mach_get_axis_position(arc.plane_axis_0);
- float y =
- arc.target[arc.plane_axis_1] - mach_get_axis_position(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 negative indicating that the arc is offset into the complex plane
- // beyond the reach of any real CNC. However, numerical errors can flip the
- // sign of disc as it approaches zero (which happens as the arc angle
- // approaches 180 degrees). To avoid mishandling these arcs we use the
- // closest real solution (which will be 0 when disc <= 0). This 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 h_x2_div_d = disc > 0 ? -sqrt(disc) / hypotf(x, y) : 0;
-
- // Invert sign of h_x2_div_d if circle is counter clockwise (see header notes)
- if (!clockwise) h_x2_div_d = -h_x2_div_d;
-
- // Negative R is g-code-alese for "I want a circle with more than 180 degrees
- // of travel" (go figure!), even though it is advised against ever generating
- // such circles in a single line of g-code. By inverting the sign of
- // h_x2_div_d the center of the circles is placed on the opposite side of
- // the line of travel and thus we get the unadvisably long arcs as prescribed.
- if (arc.radius < 0) h_x2_div_d = -h_x2_div_d;
-
- // Complete the operation by calculating the actual center of the arc
- offset[arc.plane_axis_0] = (x - y * h_x2_div_d) / 2;
- offset[arc.plane_axis_1] = (y + x * h_x2_div_d) / 2;
- offset[arc.linear_axis] = 0;
-
- return STAT_OK;
-}
-
-
-/* Compute arc from I and J (arc center point)
- *
- * The theta calculation sets up an clockwise or counterclockwise arc
- * from the current position to the target position around the center
- * designated by the offset vector. All theta-values measured in
- * radians of deviance from the positive y-axis.
- *
- * | <- theta == 0
- * * * *
- * * *
- * * *
- * * O ----T <- theta_end (e.g. 90 degrees: theta_end == PI/2)
- * * /
- * C <- theta_start (e.g. -145 degrees: theta_start == -PI*(3/4))
- *
- * Parts of this routine were originally sourced from the grbl project.
- */
-static stat_t _compute_arc(bool radius_f, const float position[],
- float offset[], float rotations, bool clockwise,
- bool full_circle) {
- // Compute radius. A non-zero radius value indicates a radius arc
- if (radius_f) _compute_arc_offsets_from_radius(offset, clockwise);
- else // compute start radius
- arc.radius = hypotf(-offset[arc.plane_axis_0], -offset[arc.plane_axis_1]);
-
- // Test arc specification for correctness according to:
- // http://linuxcnc.org/docs/html/gcode/gcode.html#sec:G2-G3-Arc
- // "It is an error if: when the arc is projected on the selected
- // plane, the distance from the current point to the center differs
- // from the distance from the end point to the 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.target[arc.plane_axis_0] -
- position[arc.plane_axis_0] - offset[arc.plane_axis_0];
-
- float end_1 = arc.target[arc.plane_axis_1] -
- position[arc.plane_axis_1] - offset[arc.plane_axis_1];
-
- // end radius - start radius
- float err = fabs(hypotf(end_0, end_1) - arc.radius);
-
- if (err > ARC_RADIUS_ERROR_MAX ||
- (err < ARC_RADIUS_ERROR_MIN && err > arc.radius * ARC_RADIUS_TOLERANCE))
- return STAT_ARC_SPECIFICATION_ERROR;
-
- // Calculate the theta (angle) of the current point (position)
- // arc.theta is angular starting point for the arc (also needed later for
- // calculating center point)
- arc.theta = atan2(-offset[arc.plane_axis_0], -offset[arc.plane_axis_1]);
-
- // g18_correction is used to invert G18 XZ plane arcs for proper CW
- // orientation
- float g18_correction = mach_get_plane() == PLANE_XZ ? -1 : 1;
- float angular_travel = 0;
-
- if (full_circle) {
- // angular travel always starts as zero for full circles
- angular_travel = 0;
-
- // handle the valid case of a full circle arc w/ P=0
- if (fp_ZERO(rotations)) rotations = 1.0;
-
- } else {
- float theta_end = atan2(end_0, end_1);
-
- // Compute the angular travel
- if (fp_EQ(theta_end, arc.theta))
- // very large radii arcs can have zero angular travel (thanks PartKam)
- angular_travel = 0;
-
- else {
- // make the difference positive so we have clockwise travel
- if (theta_end < arc.theta) theta_end += 2 * M_PI * g18_correction;
-
- // compute positive angular travel
- angular_travel = theta_end - arc.theta;
-
- // reverse travel direction if it's CCW arc
- if (!clockwise) angular_travel -= 2 * M_PI * g18_correction;
- }
- }
-
- // Add in travel for rotations
- if (clockwise) angular_travel += 2 * M_PI * rotations * g18_correction;
- else angular_travel -= 2 * M_PI * rotations * g18_correction;
-
- // Calculate travel in the depth axis of the helix and compute the time it
- // should take to perform the move length is the total mm of travel of
- // the helix (or just a planar arc)
- float linear_travel = arc.target[arc.linear_axis] - position[arc.linear_axis];
- float planar_travel = angular_travel * arc.radius;
- // hypot is insensitive to +/- signs
- float length = hypotf(planar_travel, linear_travel);
-
- // trap zero length arcs that _compute_arc can throw
- if (fp_ZERO(length)) return STAT_MINIMUM_LENGTH_MOVE;
-
- // get an estimate of execution time to inform segment calculation
- float arc_time = _estimate_arc_time(length, linear_travel, planar_travel);
-
- // Find the minimum number of segments that meets these constraints...
- float segments_for_chordal_accuracy =
- length / sqrt(4 * CHORDAL_TOLERANCE * (2 * arc.radius - CHORDAL_TOLERANCE));
- float segments_for_minimum_distance = length / ARC_SEGMENT_LENGTH;
- float segments_for_minimum_time =
- arc_time * MICROSECONDS_PER_MINUTE / MIN_ARC_SEGMENT_USEC;
-
- float segments = floor(min3(segments_for_chordal_accuracy,
- segments_for_minimum_distance,
- segments_for_minimum_time));
- if (segments < 1) segments = 1; // at least 1 segment
-
- arc.segments = (uint32_t)segments;
- arc.segment_theta = angular_travel / segments;
- arc.segment_linear_travel = linear_travel / segments;
- arc.center_0 = position[arc.plane_axis_0] - sin(arc.theta) * arc.radius;
- arc.center_1 = position[arc.plane_axis_1] - cos(arc.theta) * arc.radius;
-
- // initialize the linear position
- arc.position[arc.linear_axis] = position[arc.linear_axis];
-
- return STAT_OK;
-}
-
-
-/*** Machine entry point for arc
- *
- * Generates an arc by queuing line segments to the move buffer. The arc is
- * approximated by generating a large number of tiny, linear segments.
- */
-stat_t mach_arc_feed(float values[], bool values_f[], // arc endpoints
- float offsets[], bool offsets_f[], // arc offsets
- float radius, bool radius_f, // radius
- float P, bool P_f, // parameter
- bool modal_g1_f,
- motion_mode_t motion_mode) { // defined motion mode
-
- // 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 simple as "I25" if CW or CCW motion mode was
- // already set by a previous block. Here are 2 cases to handle if CW or CCW
- // motion mode was set by a previous block:
- //
- // Case 1: F, P or other non modal is specified but no movement is specified
- // (no offsets or radius). This is OK: return STAT_OK
- //
- // Case 2: Movement is specified w/o a new G2 or G3 word in the (new) block.
- // This is OK: continue the move
- //
- if (!modal_g1_f && !offsets_f[0] && !offsets_f[1] && !offsets_f[2] &&
- !radius_f) return STAT_OK;
-
- // trap missing feed rate
- if (fp_ZERO(mach_get_feed_rate()) ||
- (mach_is_inverse_time_mode() && !parser.gf.feed_rate))
- return STAT_GCODE_FEEDRATE_NOT_SPECIFIED;
-
- // radius must be positive and > minimum
- if (radius_f && radius < MIN_ARC_RADIUS)
- return STAT_ARC_RADIUS_OUT_OF_TOLERANCE;
-
- // 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.
- switch (mach_get_plane()) {
- case PLANE_XY: // G17
- arc.plane_axis_0 = AXIS_X;
- arc.plane_axis_1 = AXIS_Y;
- arc.linear_axis = AXIS_Z;
- break;
-
- case PLANE_XZ: // G18
- arc.plane_axis_0 = AXIS_X;
- arc.plane_axis_1 = AXIS_Z;
- arc.linear_axis = AXIS_Y;
- break;
-
- case PLANE_YZ: // G19
- arc.plane_axis_0 = AXIS_Y;
- arc.plane_axis_1 = AXIS_Z;
- arc.linear_axis = AXIS_X;
- break;
- }
-
- bool clockwise = motion_mode == MOTION_MODE_CW_ARC;
-
- // Test if endpoints are specified in the selected plane
- bool full_circle = false;
- if (!values_f[arc.plane_axis_0] && !values_f[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 full_circle = true; // in center format arc specifies full circle
- }
-
- // Test radius arcs for radius tolerance
- if (radius_f) {
- arc.radius = TO_MM(radius); // set to internal format (mm)
- if (fabs(arc.radius) < MIN_ARC_RADIUS) // radius value must be > minimum
- return STAT_ARC_RADIUS_OUT_OF_TOLERANCE;
-
- // Test that center format absolute distance mode arcs have both offsets
- } else if (mach_get_arc_distance_mode() == ABSOLUTE_MODE &&
- !(offsets_f[arc.plane_axis_0] && offsets_f[arc.plane_axis_1]))
- return STAT_ARC_OFFSETS_MISSING_FOR_PLANE;
-
- // Set arc rotations
- float rotations = 0;
-
- if (P_f) {
- // If P is present it must be a positive integer
- if (P < 0 || 0 < floor(P) - P) return STAT_P_WORD_IS_NOT_A_POSITIVE_INTEGER;
-
- rotations = P;
-
- } else if (full_circle) rotations = 1; // default to 1 for full circles
-
- // Set model target
- const float *position = mach_get_position();
- mach_calc_target(arc.target, values, values_f);
-
- // in radius mode it's an error for start == end
- if (radius_f && fp_EQ(position[AXIS_X], arc.target[AXIS_X]) &&
- fp_EQ(position[AXIS_Y], arc.target[AXIS_Y]) &&
- fp_EQ(position[AXIS_Z], arc.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
- mach_set_motion_mode(motion_mode);
- mach_update_work_offsets(); // Update resolved offsets
- arc.radius = TO_MM(radius); // set arc radius or zero
-
- float offset[3];
- for (int i = 0; i < 3; i++) offset[i] = TO_MM(offsets[i]);
-
- if (mach_get_arc_distance_mode() == ABSOLUTE_MODE) {
- if (offsets_f[0]) offset[0] -= position[0];
- if (offsets_f[1]) offset[1] -= position[1];
- if (offsets_f[2]) offset[2] -= position[2];
- }
-
- // compute arc runtime values
- RITORNO(_compute_arc
- (radius_f, position, offset, rotations, clockwise, full_circle));
-
- // Note, arc soft limits are not tested here
-
- arc.running = true; // Enable arc run in callback
- mach_arc_callback(); // Queue initial arc moves
- mach_set_position(arc.target); // update model position
-
- return STAT_OK;
-}
-
-
-/*** Generate an arc
- *
- * 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.
- */
-void mach_arc_callback() {
- while (arc.running && mp_queue_get_room()) {
- if (arc.segments == 1) { // Final segment
- arc.position[arc.plane_axis_0] = arc.target[arc.plane_axis_0];
- arc.position[arc.plane_axis_1] = arc.target[arc.plane_axis_1];
- arc.position[arc.linear_axis] = arc.target[arc.linear_axis];
-
- } else {
- arc.theta += arc.segment_theta;
-
- arc.position[arc.plane_axis_0] =
- arc.center_0 + sin(arc.theta) * arc.radius;
- arc.position[arc.plane_axis_1] =
- arc.center_1 + cos(arc.theta) * arc.radius;
- arc.position[arc.linear_axis] += arc.segment_linear_travel;
- }
-
- // run the line
- mach_plan_line(arc.position);
-
- if (!--arc.segments) arc.running = false;
- }
-}
-
-
-bool mach_arc_active() {return arc.running;}
-
-
-/// Stop arc movement without maintaining position
-/// OK to call if no arc is running
-void mach_abort_arc() {arc.running = false;}
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2016 Buildbotics LLC
- Copyright (c) 2010 - 2015 Alden S. Hart, Jr.
- All rights reserved.
-
- 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/>.
-
- The software is distributed in the hope that it will be useful, but
- WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
- Lesser General Public License for more details.
-
- You should have received a copy of the GNU Lesser General Public
- License along with the software. If not, see
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-#pragma once
-
-#include "gcode_state.h"
-#include "status.h"
-
-
-#define ARC_SEGMENT_LENGTH 0.1 // mm
-#define MIN_ARC_RADIUS 0.1
-
-#define MIN_ARC_SEGMENT_USEC 10000.0 // minimum arc segment time
-#define MIN_ARC_SEGMENT_TIME (MIN_ARC_SEGMENT_USEC / MICROSECONDS_PER_MINUTE)
-
-
-stat_t mach_arc_feed(float target[], bool flags[], float offsets[],
- bool offset_f[], float radius, bool radius_f,
- float P, bool P_f, bool modal_g1_f,
- motion_mode_t motion_mode);
-void mach_arc_callback();
-bool mach_arc_active();
-void mach_abort_arc();
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2016 Buildbotics LLC
- Copyright (c) 2010 - 2015 Alden S. Hart, Jr.
- Copyright (c) 2012 - 2015 Rob Giseburt
- All rights reserved.
-
- 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/>.
-
- The software is distributed in the hope that it will be useful, but
- WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
- Lesser General Public License for more details.
-
- You should have received a copy of the GNU Lesser General Public
- License along with the software. If not, see
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-/* Planner buffers are used to queue and operate on Gcode blocks. Each
- * buffer contains one Gcode block which may be a move, M code or
- * other command that must be executed synchronously with movement.
- */
-
-#include "buffer.h"
-#include "state.h"
-#include "rtc.h"
-#include "util.h"
-
-#include <string.h>
-#include <math.h>
-#include <stdio.h>
-
-
-typedef struct {
- uint8_t space;
- mp_buffer_t *tail;
- mp_buffer_t *head;
- mp_buffer_t bf[PLANNER_BUFFER_POOL_SIZE];
-} buffer_pool_t;
-
-
-buffer_pool_t mb;
-
-
-/// Zeroes the contents of a buffer
-static void _clear_buffer(mp_buffer_t *bf) {
- mp_buffer_t *next = bf->next; // save pointers
- mp_buffer_t *prev = bf->prev;
- memset(bf, 0, sizeof(mp_buffer_t));
- bf->next = next; // restore pointers
- bf->prev = prev;
-}
-
-
-static void _push() {
- if (!mb.space) {
- ALARM(STAT_INTERNAL_ERROR);
- return;
- }
-
- mb.tail = mb.tail->next;
- mb.space--;
-}
-
-
-static void _pop() {
- if (mb.space == PLANNER_BUFFER_POOL_SIZE) {
- ALARM(STAT_INTERNAL_ERROR);
- return;
- }
-
- mb.head = mb.head->next;
- mb.space++;
-}
-
-
-/// Initializes or resets buffers
-void mp_queue_init() {
- memset(&mb, 0, sizeof(mb)); // clear all values
-
- mb.tail = mb.head = &mb.bf[0]; // init head and tail
- mb.space = PLANNER_BUFFER_POOL_SIZE;
-
- // Setup ring pointers
- for (int i = 0; i < mb.space; i++) {
- mb.bf[i].next = &mb.bf[i + 1];
- mb.bf[i].prev = &mb.bf[i - 1];
- }
-
- mb.bf[0].prev = &mb.bf[mb.space -1]; // Fix first->prev
- mb.bf[mb.space - 1].next = &mb.bf[0]; // Fix last->next
-
- mp_state_idle();
-}
-
-
-uint8_t mp_queue_get_room() {
- return mb.space < PLANNER_BUFFER_HEADROOM ?
- 0 : mb.space - PLANNER_BUFFER_HEADROOM;
-}
-
-
-uint8_t mp_queue_get_fill() {
- return PLANNER_BUFFER_POOL_SIZE - mb.space;
-}
-
-
-bool mp_queue_is_empty() {return mb.tail == mb.head;}
-
-
-/// Get pointer to next buffer, waiting until one is available.
-mp_buffer_t *mp_queue_get_tail() {
- while (!mb.space) continue; // Wait for a buffer
- return mb.tail;
-}
-
-
-/*** Commit the next buffer to the queue.
- *
- * WARNING: The routine calling mp_queue_push() must not use the write
- * buffer once it has been queued. Action may start on the buffer immediately,
- * invalidating its contents
- */
-void mp_queue_push(buffer_cb_t cb, uint32_t line) {
- mp_state_running();
-
- mb.tail->ts = rtc_get_time();
- mb.tail->cb = cb;
- mb.tail->line = line;
- mb.tail->state = BUFFER_NEW;
-
- _push();
-}
-
-
-mp_buffer_t *mp_queue_get_head() {
- return mp_queue_is_empty() ? 0 : mb.head;
-}
-
-
-/// Clear and release buffer to pool
-void mp_queue_pop() {
- _clear_buffer(mb.head);
- _pop();
-}
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2016 Buildbotics LLC
- All rights reserved.
-
- 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/>.
-
- The software is distributed in the hope that it will be useful, but
- WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
- Lesser General Public License for more details.
-
- You should have received a copy of the GNU Lesser General Public
- License along with the software. If not, see
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-#pragma once
-
-#include "machine.h"
-#include "config.h"
-
-#include <stdbool.h>
-
-
-typedef enum {
- BUFFER_OFF, // move inactive
- BUFFER_NEW, // initial value
- BUFFER_INIT, // first run
- BUFFER_ACTIVE, // subsequent runs
- BUFFER_RESTART, // restart buffer when done
-} buffer_state_t;
-
-
-// Callbacks
-struct mp_buffer_t;
-typedef stat_t (*buffer_cb_t)(struct mp_buffer_t *bf);
-
-
-typedef struct mp_buffer_t { // See Planning Velocity Notes
- struct mp_buffer_t *prev; // pointer to previous buffer
- struct mp_buffer_t *next; // pointer to next buffer
-
- uint32_t ts; // Time stamp
- int32_t line; // gcode block line number
- buffer_cb_t cb; // callback to buffer exec function
-
- buffer_state_t state; // buffer state
- bool replannable; // true if move can be re-planned
- bool hold; // hold at the start of this block
-
- float value; // used in dwell and other callbacks
-
- float target[AXES]; // XYZABC where the move should go
- float unit[AXES]; // unit vector for axis scaling & planning
-
- float length; // total length of line or helix in mm
- float head_length;
- float body_length;
- float tail_length;
-
- // See notes on these variables, in mp_aline()
- float entry_velocity; // entry velocity requested for the move
- float cruise_velocity; // cruise velocity requested & achieved
- float exit_velocity; // exit velocity requested for the move
- float braking_velocity; // current value for braking velocity
-
- float entry_vmax; // max junction velocity at entry of this move
- float cruise_vmax; // max cruise velocity requested for move
- float exit_vmax; // max exit velocity possible (redundant)
- float delta_vmax; // max velocity difference for this move
-
- float jerk; // maximum linear jerk term for this move
- float cbrt_jerk; // cube root of Jm (computed & cached)
-} mp_buffer_t;
-
-
-void mp_queue_init();
-
-uint8_t mp_queue_get_room();
-uint8_t mp_queue_get_fill();
-
-bool mp_queue_is_empty();
-
-mp_buffer_t *mp_queue_get_tail();
-void mp_queue_push(buffer_cb_t func, uint32_t line);
-
-mp_buffer_t *mp_queue_get_head();
-void mp_queue_pop();
-
-static inline mp_buffer_t *mp_buffer_prev(mp_buffer_t *bp) {return bp->prev;}
-static inline mp_buffer_t *mp_buffer_next(mp_buffer_t *bp) {return bp->next;}
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2016 Buildbotics LLC
- All rights reserved.
-
- 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/>.
-
- The software is distributed in the hope that it will be useful, but
- WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
- Lesser General Public License for more details.
-
- You should have received a copy of the GNU Lesser General Public
- License along with the software. If not, see
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-
-#include "calibrate.h"
-
-#include "buffer.h"
-#include "motor.h"
-#include "machine.h"
-#include "planner.h"
-#include "stepper.h"
-#include "rtc.h"
-#include "state.h"
-#include "config.h"
-
-#include <stdint.h>
-#include <stdio.h>
-#include <string.h>
-#include <stdlib.h>
-
-
-#define CAL_VELOCITIES 256
-#define CAL_MIN_VELOCITY 1000 // mm/sec
-#define CAL_TARGET_SG 100
-#define CAL_MAX_DELTA_SG 75
-#define CAL_WAIT_TIME 3 // ms
-
-
-enum {
- CAL_START,
- CAL_ACCEL,
- CAL_MEASURE,
- CAL_DECEL,
-};
-
-
-typedef struct {
- bool stall_valid;
- bool stalled;
- bool reverse;
-
- uint32_t wait;
- int state;
- int motor;
- int axis;
-
- float velocity;
- uint16_t stallguard;
-} calibrate_t;
-
-static calibrate_t cal = {0};
-
-
-static stat_t _exec_calibrate(mp_buffer_t *bf) {
- const float time = MIN_SEGMENT_TIME; // In minutes
- const float max_delta_v = CAL_ACCELERATION * time;
-
- do {
- if (rtc_expired(cal.wait))
- switch (cal.state) {
- case CAL_START: {
- cal.axis = motor_get_axis(cal.motor);
- cal.state = CAL_ACCEL;
- cal.velocity = 0;
- cal.stall_valid = false;
- cal.stalled = false;
- cal.reverse = false;
-
- //tmc2660_set_stallguard_threshold(cal.motor, 8);
- cal.wait = rtc_get_time() + CAL_WAIT_TIME;
-
- break;
- }
-
- case CAL_ACCEL:
- if (CAL_MIN_VELOCITY < cal.velocity) cal.stall_valid = true;
-
- if (cal.velocity < CAL_MIN_VELOCITY || CAL_TARGET_SG < cal.stallguard)
- cal.velocity += max_delta_v;
-
- if (cal.stalled) {
- if (cal.reverse) {
- int32_t steps = -motor_get_encoder(cal.motor);
- float mm = (float)steps / motor_get_steps_per_unit(cal.motor);
- STATUS_DEBUG("%"PRIi32" steps %0.2f mm", steps, mm);
-
- //tmc2660_set_stallguard_threshold(cal.motor, 63);
-
- mp_set_cycle(CYCLE_MACHINING); // Default cycle
-
- return STAT_NOOP; // Done, no move queued
-
- } else {
- motor_set_encoder(cal.motor, 0);
-
- cal.reverse = true;
- cal.velocity = 0;
- cal.stall_valid = false;
- cal.stalled = false;
- }
- }
- break;
- }
- } while (fp_ZERO(cal.velocity)); // Repeat if computed velocity was zero
-
- // Compute travel
- float travel[AXES] = {0}; // In mm
- travel[cal.axis] = time * cal.velocity * (cal.reverse ? -1 : 1);
-
- // Convert to steps
- float steps[MOTORS] = {0};
- mp_kinematics(travel, steps);
-
- // Queue segment
- st_prep_line(time, steps, 0);
-
- return STAT_EAGAIN;
-}
-
-
-bool calibrate_busy() {return mp_get_cycle() == CYCLE_CALIBRATING;}
-
-
-void calibrate_set_stallguard(int motor, uint16_t sg) {
- if (cal.motor != motor) return;
-
- if (cal.stall_valid) {
- int16_t delta = sg - cal.stallguard;
- if (!sg || CAL_MAX_DELTA_SG < abs(delta)) {
- cal.stalled = true;
- motor_end_move(cal.motor);
- }
- }
-
- cal.stallguard = sg;
-}
-
-
-uint8_t command_calibrate(int argc, char *argv[]) {
- if (mp_get_cycle() != CYCLE_MACHINING || mp_get_state() != STATE_READY)
- return 0;
-
- // Start
- memset(&cal, 0, sizeof(cal));
- mp_set_cycle(CYCLE_CALIBRATING);
- cal.motor = 1;
-
- mp_queue_push_nonstop(_exec_calibrate, -1);
-
- return 0;
-}
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2016 Buildbotics LLC
- All rights reserved.
-
- 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/>.
-
- The software is distributed in the hope that it will be useful, but
- WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
- Lesser General Public License for more details.
-
- You should have received a copy of the GNU Lesser General Public
- License along with the software. If not, see
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-#pragma once
-
-#include <stdbool.h>
-#include <stdint.h>
-
-
-bool calibrate_busy();
-void calibrate_set_stallguard(int motor, uint16_t sg);
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2016 Buildbotics LLC
- Copyright (c) 2010 - 2015 Alden S. Hart, Jr.
- Copyright (c) 2012 - 2015 Rob Giseburt
- All rights reserved.
-
- 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/>.
-
- The software is distributed in the hope that it will be useful, but
- WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
- Lesser General Public License for more details.
-
- You should have received a copy of the GNU Lesser General Public
- License along with the software. If not, see
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-#include "dwell.h"
-
-#include "buffer.h"
-#include "machine.h"
-#include "stepper.h"
-
-
-// Dwells are performed by passing a dwell move to the stepper drivers.
-
-
-/// Dwell execution
-static stat_t _exec_dwell(mp_buffer_t *bf) {
- st_prep_dwell(bf->value); // in seconds
- return STAT_OK; // Done
-}
-
-
-/// Queue a dwell
-stat_t mp_dwell(float seconds, int32_t line) {
- mp_buffer_t *bf = mp_queue_get_tail();
- bf->value = seconds; // in seconds, not minutes
- mp_queue_push(_exec_dwell, line);
-
- return STAT_OK;
-}
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2016 Buildbotics LLC
- All rights reserved.
-
- 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/>.
-
- The software is distributed in the hope that it will be useful, but
- WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
- Lesser General Public License for more details.
-
- You should have received a copy of the GNU Lesser General Public
- License along with the software. If not, see
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-#pragma once
-
-#include "status.h"
-
-stat_t mp_dwell(float seconds, int32_t line);
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2016 Buildbotics LLC
- Copyright (c) 2010 - 2015 Alden S. Hart, Jr.
- Copyright (c) 2012 - 2015 Rob Giseburt
- All rights reserved.
-
- 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/>.
-
- The software is distributed in the hope that it will be useful, but
- WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
- Lesser General Public License for more details.
-
- You should have received a copy of the GNU Lesser General Public
- License along with the software. If not, see
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-#include "planner.h"
-#include "buffer.h"
-#include "axis.h"
-#include "runtime.h"
-#include "state.h"
-#include "forward_dif.h"
-#include "stepper.h"
-#include "motor.h"
-#include "rtc.h"
-#include "usart.h"
-#include "config.h"
-
-#include <string.h>
-#include <stdbool.h>
-#include <math.h>
-#include <stdio.h>
-
-
-typedef struct {
- float unit[AXES]; // unit vector for axis scaling & planning
- float final_target[AXES]; // final target, used to correct rounding errors
- float waypoint[3][AXES]; // head/body/tail endpoints for correction
-
- // copies of bf variables of same name
- float head_length;
- float body_length;
- float tail_length;
- float entry_velocity;
- float cruise_velocity;
- float exit_velocity;
-
- uint32_t segment_count; // count of running segments
- float segment_velocity; // computed velocity for segment
- float segment_time; // actual time increment per segment
- forward_dif_t fdif; // forward difference levels
- bool hold_planned; // true when a feedhold has been planned
- move_section_t section; // current move section
- bool section_new; // true if it's a new section
- bool abort;
-} mp_exec_t;
-
-
-static mp_exec_t ex = {{0}};
-
-
-static stat_t _exec_aline_segment() {
- float target[AXES];
-
- // Set target position for the segment. If the segment ends on a section
- // waypoint, synchronize to the head, body or tail end. 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.
- if (!--ex.segment_count && !ex.section_new && !ex.hold_planned)
- copy_vector(target, ex.waypoint[ex.section]);
-
- else {
- float segment_length = ex.segment_velocity * ex.segment_time;
-
- for (int axis = 0; axis < AXES; axis++)
- target[axis] =
- mp_runtime_get_axis_position(axis) + ex.unit[axis] * segment_length;
- }
-
- mp_runtime_set_velocity(ex.segment_velocity);
- RITORNO(mp_runtime_move_to_target(target, ex.segment_time));
-
- // Return EAGAIN to continue or OK if this segment is done
- return ex.segment_count ? STAT_EAGAIN : STAT_OK;
-}
-
-
-/// Common code for head and tail sections
-static stat_t _exec_aline_section(float length, float vin, float vout) {
- if (ex.section_new) {
- if (fp_ZERO(length)) return STAT_NOOP; // end the section
-
- // len / avg. velocity
- float move_time = 2 * length / (vin + vout);
- float segments = ceil(move_time / NOM_SEGMENT_TIME);
- ex.segment_time = move_time / segments;
- ex.segment_count = round(segments);
-
- if (vin == vout) ex.segment_velocity = vin;
- else ex.segment_velocity =
- mp_init_forward_dif(ex.fdif, vin, vout, segments);
-
- if (ex.segment_time < MIN_SEGMENT_TIME)
- return STAT_MINIMUM_TIME_MOVE; // exit /wo advancing position
-
- // First segment
- if (_exec_aline_segment() == STAT_OK) {
- ex.section_new = false;
- return STAT_OK;
- }
-
- ex.section_new = false;
-
- } else {
- if (vin != vout) ex.segment_velocity += mp_next_forward_dif(ex.fdif);
-
- // Subsequent segments
- if (_exec_aline_segment() == STAT_OK) return STAT_OK;
- }
-
- return STAT_EAGAIN;
-}
-
-
-/// Callback for tail section
-static stat_t _exec_aline_tail() {
- ex.section = SECTION_TAIL;
- return
- _exec_aline_section(ex.tail_length, ex.cruise_velocity, ex.exit_velocity);
-}
-
-
-/// Callback for body section
-static stat_t _exec_aline_body() {
- ex.section = SECTION_BODY;
-
- stat_t status =
- _exec_aline_section(ex.body_length, ex.cruise_velocity, ex.cruise_velocity);
-
- switch (status) {
- case STAT_NOOP: return _exec_aline_tail();
- case STAT_OK:
- ex.section = SECTION_TAIL;
- ex.section_new = true;
-
- return STAT_EAGAIN;
-
- default: return status;
- }
-}
-
-
-/// Callback for head section
-static stat_t _exec_aline_head() {
- ex.section = SECTION_HEAD;
- stat_t status =
- _exec_aline_section(ex.head_length, ex.entry_velocity, ex.cruise_velocity);
-
- switch (status) {
- case STAT_NOOP: return _exec_aline_body();
- case STAT_OK:
- ex.section = SECTION_BODY;
- ex.section_new = true;
-
- return STAT_EAGAIN;
-
- default: return status;
- }
-}
-
-
-static float _compute_next_segment_velocity() {
- if (ex.section_new) {
- if (ex.section == SECTION_HEAD) return mp_runtime_get_velocity();
- else return ex.cruise_velocity;
- }
-
- return ex.segment_velocity +
- (ex.section == SECTION_BODY ? 0 : mp_next_forward_dif(ex.fdif));
-}
-
-
-/// Replan current move to execute hold
-///
-/// Holds are initiated by the planner entering STATE_STOPPING. In which case
-/// _plan_hold() is called to replan the current move towards zero. If it is
-/// unable to plan to zero in the remaining length of the current move it will
-/// decelerate as much as possible and then wait for the next move. Once it is
-/// possible to plan to zero velocity in the current move the remaining length
-/// is put into the run buffer, which is still allocated, and the run buffer
-/// becomes the hold point. The hold is left by a start request in state.c. At
-/// this point the remaining buffers, if any, are replanned from zero up to
-/// speed.
-static void _plan_hold() {
- mp_buffer_t *bf = mp_queue_get_head(); // working buffer pointer
- if (!bf) return; // Oops! nothing's running
-
- // Examine and process current buffer and compute length left for decel
- float available_length =
- axis_get_vector_length(ex.final_target, mp_runtime_get_position());
- // Compute next_segment velocity, velocity left to shed to brake to zero
- float braking_velocity = _compute_next_segment_velocity();
- // Distance to brake to zero from braking_velocity, bf is OK to use here
- float braking_length = mp_get_target_length(braking_velocity, 0, bf->jerk);
-
- // Hack to prevent Case 2 moves for perfect-fit decels. Happens when homing.
- if (available_length < braking_length && fp_ZERO(bf->exit_velocity))
- braking_length = available_length;
-
- // Replan to decelerate
- ex.section = SECTION_TAIL;
- ex.section_new = true;
- ex.cruise_velocity = braking_velocity;
- ex.hold_planned = true;
-
- // Avoid creating segments before or after the hold which are too small.
- if (fabs(available_length - braking_length) < HOLD_DECELERATION_TOLERANCE) {
- // Case 0: deceleration fits almost exactly
- ex.exit_velocity = 0;
- ex.tail_length = available_length;
-
- } else if (braking_length <= available_length) {
- // Case 1: deceleration fits entirely into the remaining length
- // Setup tail to perform the deceleration
- ex.exit_velocity = 0;
- ex.tail_length = braking_length;
-
- // Re-use bf to run the remaining block length
- bf->length = available_length - braking_length;
- bf->delta_vmax = mp_get_target_velocity(0, bf->length, bf);
- bf->entry_vmax = 0;
- bf->state = BUFFER_RESTART; // Restart the buffer when done
-
- } else {
- // Case 2: deceleration exceeds length remaining in buffer
- // Replan to minimum (but non-zero) exit velocity
- ex.tail_length = available_length;
- ex.exit_velocity =
- braking_velocity - mp_get_target_velocity(0, available_length, bf);
- }
-}
-
-
-/// Initializes move runtime with a new planner buffer
-static stat_t _exec_aline_init(mp_buffer_t *bf) {
- // Remove zero length lines. Short lines have already been removed.
- if (fp_ZERO(bf->length)) return STAT_NOOP;
-
- // Initialize move
- copy_vector(ex.unit, bf->unit);
- copy_vector(ex.final_target, bf->target);
-
- ex.head_length = bf->head_length;
- ex.body_length = bf->body_length;
- ex.tail_length = bf->tail_length;
- ex.entry_velocity = bf->entry_velocity;
- ex.cruise_velocity = bf->cruise_velocity;
- ex.exit_velocity = bf->exit_velocity;
-
- ex.section = SECTION_HEAD;
- ex.section_new = true;
- ex.hold_planned = false;
-
- // Generate waypoints for position correction at section ends. This helps
- // negate floating point errors in the forward differencing code.
- for (int axis = 0; axis < AXES; axis++) {
- float position = mp_runtime_get_axis_position(axis);
-
- ex.waypoint[SECTION_HEAD][axis] = position + ex.unit[axis] * ex.head_length;
- ex.waypoint[SECTION_BODY][axis] = position +
- ex.unit[axis] * (ex.head_length + ex.body_length);
- ex.waypoint[SECTION_TAIL][axis] = ex.final_target[axis];
- }
-
- return STAT_OK;
-}
-
-
-void mp_exec_abort() {ex.abort = true;}
-
-
-/// Aline execution routines
-///
-/// Everything here fires from interrupts and must be interrupt safe
-///
-/// Returns:
-///
-/// STAT_OK move is done
-/// STAT_EAGAIN move is not finished - has more segments to run
-/// STAT_NOOP cause no stepper operation - 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 correct behavior of these routines.
-/// Each call to _exec_aline() must execute and prep *one and only one*
-/// segment. If the segment is not the last segment in the bf buffer the
-/// _aline() returns STAT_EAGAIN. If it's the last segment it returns
-/// STAT_OK. If it encounters a fatal error that would terminate the move it
-/// returns a valid error code.
-///
-/// Notes:
-///
-/// [1] Returning STAT_OK ends the move and frees the bf buffer.
-/// Returning STAT_OK at does NOT advance position meaning
-/// any position error will be compensated by the next move.
-///
-/// Operation:
-///
-/// Aline generates jerk-controlled S-curves as per Ed Red's course notes:
-///
-/// http://www.et.byu.edu/~ered/ME537/Notes/Ch5.pdf
-/// http://www.scribd.com/doc/63521608/Ed-Red-Ch5-537-Jerk-Equations
-///
-/// A full trapezoid is divided into 5 periods. Periods 1 and 2 are the
-/// first and second halves of the acceleration ramp (the concave and convex
-/// parts of the S curve in the "head"). Periods 3 and 4 are the first
-/// and second parts of the deceleration ramp (the tail). There is also
-/// a period for the constant-velocity plateau of the trapezoid (the body).
-/// There are many possible degenerate trapezoids where any of the 5 periods
-/// may be zero length but note that either none or both of a ramping pair can
-/// be zero.
-///
-/// The equations that govern the acceleration and deceleration ramps are:
-///
-/// Period 1 V = Vi + Jm * (T^2) / 2
-/// Period 2 V = Vh + As * T - Jm * (T^2) / 2
-/// Period 3 V = Vi - Jm * (T^2) / 2
-/// Period 4 V = Vh + As * T + Jm * (T^2) / 2
-///
-/// move_time is the actual time of the move, accel_time is the time value
-/// needed to compute the velocity taking the initial velocity into account.
-/// move_time does not need to.
-stat_t mp_exec_aline(mp_buffer_t *bf) {
- stat_t status = STAT_OK;
-
- if (ex.abort) {
- ex.abort = false;
- mp_runtime_set_velocity(0); // Assume a hard stop
- return STAT_NOOP;
- }
-
- // Start a new move
- if (bf->state == BUFFER_INIT) {
- bf->state = BUFFER_ACTIVE;
- status = _exec_aline_init(bf);
- if (status != STAT_OK) return status;
- }
-
- // Plan holds
- if (mp_get_state() == STATE_STOPPING && !ex.hold_planned) _plan_hold();
-
- // Main segment dispatch
- switch (ex.section) {
- case SECTION_HEAD: status = _exec_aline_head(); break;
- case SECTION_BODY: status = _exec_aline_body(); break;
- case SECTION_TAIL: status = _exec_aline_tail(); break;
- }
-
- // Set runtime velocity on exit
- if (status != STAT_EAGAIN) mp_runtime_set_velocity(ex.exit_velocity);
-
- return status;
-}
-
-
-/// Dequeues buffers, initializes them, executes their callbacks and cleans up.
-///
-/// This is the guts of the planner runtime execution. Because this routine is
-/// run in an interrupt the state changes must be carefully ordered.
-stat_t mp_exec_move() {
- // Check if we can run a buffer
- mp_buffer_t *bf = mp_queue_get_head();
- if (mp_get_state() == STATE_ESTOPPED || mp_get_state() == STATE_HOLDING ||
- !bf) {
- mp_runtime_set_velocity(0);
- mp_runtime_set_busy(false);
-
- return STAT_NOOP; // Nothing running
- }
-
- // Process new buffers
- if (bf->state == BUFFER_NEW) {
- // On restart wait a bit to give planner queue a chance to fill
- if (!mp_runtime_is_busy() && mp_queue_get_fill() < PLANNER_EXEC_MIN_FILL &&
- !rtc_expired(bf->ts + PLANNER_EXEC_DELAY)) return STAT_NOOP;
-
- // Take control of buffer
- bf->state = BUFFER_INIT;
- bf->replannable = false;
-
- // Update runtime
- mp_runtime_set_line(bf->line);
- }
-
- // Execute the buffer
- stat_t status = bf->cb(bf);
-
- // Signal that we are busy only if a move was queued. This means that
- // nonstop buffers, i.e. non-plan-to-zero commands, will not cause the
- // runtime to enter the busy state. This causes mp_exec_move() to continue
- // to wait above for the planner buffer to fill when a new stream starts
- // with some nonstop buffers. If this weren't so, the code below
- // which marks the next buffer not replannable would lock the first move
- // buffer and cause it to be unnecessarily planned to zero.
- if (status == STAT_EAGAIN || status == STAT_OK) mp_runtime_set_busy(true);
-
- // Process finished buffers
- if (status != STAT_EAGAIN) {
- // Signal that we've encountered a stopping point
- if (fp_ZERO(mp_runtime_get_velocity()) &&
- (mp_get_state() == STATE_STOPPING || bf->hold)) mp_state_holding();
-
- // Handle buffer restarts and deallocation
- if (bf->state == BUFFER_RESTART) bf->state = BUFFER_NEW;
- else {
- // Solves a potential race condition where the current buffer ends but
- // the new buffer has not started because the current one is still
- // being run by the steppers. Planning can overwrite the new buffer.
- // See notes above.
- mp_buffer_next(bf)->replannable = false;
-
- mp_queue_pop(); // Release buffer
-
- // Enter READY state
- if (mp_queue_is_empty()) mp_state_idle();
- }
- }
-
- // Convert return status for stepper.c
- switch (status) {
- case STAT_NOOP:
- // Tell caller to call again if there is more in the queue
- return mp_queue_is_empty() ? STAT_NOOP : STAT_EAGAIN;
- case STAT_EAGAIN: return STAT_OK; // A move was queued, call again later
- default: return status;
- }
-}
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2016 Buildbotics LLC
- All rights reserved.
-
- 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/>.
-
- The software is distributed in the hope that it will be useful, but
- WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
- Lesser General Public License for more details.
-
- You should have received a copy of the GNU Lesser General Public
- License along with the software. If not, see
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-#pragma once
-
-#include "buffer.h"
-
-
-stat_t mp_exec_move();
-void mp_exec_abort();
-stat_t mp_exec_aline(mp_buffer_t *bf);
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2016 Buildbotics LLC
- Copyright (c) 2010 - 2015 Alden S. Hart, Jr.
- Copyright (c) 2012 - 2015 Rob Giseburt
- All rights reserved.
-
- 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/>.
-
- The software is distributed in the hope that it will be useful, but
- WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
- Lesser General Public License for more details.
-
- You should have received a copy of the GNU Lesser General Public
- License along with the software. If not, see
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-#include "forward_dif.h"
-
-#include <math.h>
-
-
-/// Forward differencing math
-///
-/// We are using a quintic (fifth-degree) Bezier polynomial for the velocity
-/// curve. This gives us a "linear pop" velocity curve; with pop being the
-/// sixth derivative of position: velocity - 1st, acceleration - 2nd, jerk -
-/// 3rd, snap - 4th, crackle - 5th, pop - 6th
-///
-/// The Bezier curve takes the form:
-///
-/// V(t) = P_0 * B_0(t) + P_1 * B_1(t) + P_2 * B_2(t) + P_3 * B_3(t) +
-/// P_4 * B_4(t) + P_5 * B_5(t)
-///
-/// Where 0 <= t <= 1, and V(t) is the velocity. P_0 through P_5 are
-/// the control points, and B_0(t) through B_5(t) are the Bernstein
-/// basis as follows:
-///
-/// B_0(t) = (1 - t)^5 = -t^5 + 5t^4 - 10t^3 + 10t^2 - 5t + 1
-/// B_1(t) = 5(1 - t)^4 * t = 5t^5 - 20t^4 + 30t^3 - 20t^2 + 5t
-/// B_2(t) = 10(1 - t)^3 * t^2 = -10t^5 + 30t^4 - 30t^3 + 10t^2
-/// B_3(t) = 10(1 - t)^2 * t^3 = 10t^5 - 20t^4 + 10t^3
-/// B_4(t) = 5(1 - t) * t^4 = -5t^5 + 5t^4
-/// B_5(t) = t^5 = t^5
-///
-/// ^ ^ ^ ^ ^ ^
-/// A B C D E F
-///
-/// We use forward-differencing to calculate each position through the curve.
-/// This requires a formula of the form:
-///
-/// V_f(t) = A * t^5 + B * t^4 + C * t^3 + D * t^2 + E * t + F
-///
-/// Looking at the above B_0(t) through B_5(t) expanded forms, if we take the
-/// coefficients of t^5 through t of the Bezier form of V(t), we can determine
-/// that:
-///
-/// A = -P_0 + 5 * P_1 - 10 * P_2 + 10 * P_3 - 5 * P_4 + P_5
-/// B = 5 * P_0 - 20 * P_1 + 30 * P_2 - 20 * P_3 + 5 * P_4
-/// C = -10 * P_0 + 30 * P_1 - 30 * P_2 + 10 * P_3
-/// D = 10 * P_0 - 20 * P_1 + 10 * P_2
-/// E = - 5 * P_0 + 5 * P_1
-/// F = P_0
-///
-/// Now, since we will (currently) *always* want the initial acceleration and
-/// jerk values to be 0, We set P_i = P_0 = P_1 = P_2 (initial velocity), and
-/// P_t = P_3 = P_4 = P_5 (target velocity), which, after simplification,
-/// resolves to:
-///
-/// A = - 6 * P_i + 6 * P_t
-/// B = 15 * P_i - 15 * P_t
-/// C = -10 * P_i + 10 * P_t
-/// D = 0
-/// E = 0
-/// F = P_i
-///
-/// Given an interval count of I to get from P_i to P_t, we get the parametric
-/// "step" size of h = 1/I. We need to calculate the initial value of forward
-/// differences (F_0 - F_5) such that the inital velocity V = P_i, then we
-/// iterate over the following I times:
-///
-/// V += F_5
-/// F_5 += F_4
-/// F_4 += F_3
-/// F_3 += F_2
-/// F_2 += F_1
-///
-/// See
-/// http://www.drdobbs.com/forward-difference-calculation-of-bezier/184403417
-/// for an example of how to calculate F_0 - F_5 for a cubic bezier curve. Since
-/// this is a quintic bezier curve, we need to extend the formulas somewhat.
-/// I'll not go into the long-winded step-by-step here, but it gives the
-/// resulting formulas:
-///
-/// a = A, b = B, c = C, d = D, e = E, f = F
-///
-/// F_5(t + h) - F_5(t) = (5ah)t^4 + (10ah^2 + 4bh)t^3 +
-/// (10ah^3 + 6bh^2 + 3ch)t^2 + (5ah^4 + 4bh^3 + 3ch^2 + 2dh)t + ah^5 +
-/// bh^4 + ch^3 + dh^2 + eh
-///
-/// a = 5ah
-/// b = 10ah^2 + 4bh
-/// c = 10ah^3 + 6bh^2 + 3ch
-/// d = 5ah^4 + 4bh^3 + 3ch^2 + 2dh
-///
-/// After substitution, simplification, and rearranging:
-///
-/// F_4(t + h) - F_4(t) = (20ah^2)t^3 + (60ah^3 + 12bh^2)t^2 +
-/// (70ah^4 + 24bh^3 + 6ch^2)t + 30ah^5 + 14bh^4 + 6ch^3 + 2dh^2
-///
-/// a = 20ah^2
-/// b = 60ah^3 + 12bh^2
-/// c = 70ah^4 + 24bh^3 + 6ch^2
-///
-/// After substitution, simplification, and rearranging:
-///
-/// F_3(t + h) - F_3(t) = (60ah^3)t^2 + (180ah^4 + 24bh^3)t + 150ah^5 +
-/// 36bh^4 + 6ch^3
-///
-/// You get the picture...
-///
-/// F_2(t + h) - F_2(t) = (120ah^4)t + 240ah^5 + 24bh^4
-/// F_1(t + h) - F_1(t) = 120ah^5
-///
-/// Normally, we could then assign t = 0, use the A-F values from above, and get
-/// out initial F_* values. However, for the sake of "averaging" the velocity
-/// of each segment, we actually want to have the initial V be be at t = h/2 and
-/// iterate I-1 times. So, the resulting F_* values are (steps not shown):
-///
-/// F_5 = 121Ah^5 / 16 + 5Bh^4 + 13Ch^3 / 4 + 2Dh^2 + Eh
-/// F_4 = 165Ah^5 / 2 + 29Bh^4 + 9Ch^3 + 2Dh^2
-/// F_3 = 255Ah^5 + 48Bh^4 + 6Ch^3
-/// F_2 = 300Ah^5 + 24Bh^4
-/// F_1 = 120Ah^5
-///
-/// Note that with our current control points, D and E are actually 0.
-///
-/// This can be simplified even further by subsituting Ah, Bh & Ch back in and
-/// reducing to:
-///
-/// F_5 = (32.5 * s^2 - 75 * s + 45.375)(Vt - Vi) * h^5
-/// F_4 = (90.0 * s^2 - 435 * s + 495.0 )(Vt - Vi) * h^5
-/// F_3 = (60.0 * s^2 - 720 * s + 1530.0 )(Vt - Vi) * h^5
-/// F_2 = ( - 360 * s + 1800.0 )(Vt - Vi) * h^5
-/// F_1 = ( 720.0 )(Vt - Vi) * h^5
-///
-float mp_init_forward_dif(forward_dif_t fdif, float Vi, float Vt, float s) {
- const float h = 1 / s;
- const float s2 = square(s);
- const float Vdxh5 = (Vt - Vi) * h * h * h * h * h;
-
- fdif[4] = (32.5 * s2 - 75.0 * s + 45.375) * Vdxh5;
- fdif[3] = (90.0 * s2 - 435.0 * s + 495.0 ) * Vdxh5;
- fdif[2] = (60.0 * s2 - 720.0 * s + 1530.0 ) * Vdxh5;
- fdif[1] = ( - 360.0 * s + 1800.0 ) * Vdxh5;
- fdif[0] = ( 720.0 ) * Vdxh5;
-
- // Calculate the initial velocity by calculating:
- //
- // V(h / 2) =
- //
- // ( -6Vi + 6Vt) / (2^5 * s^8) +
- // ( 15Vi - 15Vt) / (2^4 * s^8) +
- // (-10Vi + 10Vt) / (2^3 * s^8) + Vi =
- //
- // (Vt - Vi) * 1/2 * h^8 + Vi
- return (Vt - Vi) * 0.5 * square(square(square(h))) + Vi;
-}
-
-
-float mp_next_forward_dif(forward_dif_t fdif) {
- float delta = fdif[4];
-
- for (int i = 4; i; i--)
- fdif[i] += fdif[i - 1];
-
- return delta;
-}
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2016 Buildbotics LLC
- All rights reserved.
-
- 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/>.
-
- The software is distributed in the hope that it will be useful, but
- WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
- Lesser General Public License for more details.
-
- You should have received a copy of the GNU Lesser General Public
- License along with the software. If not, see
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-#pragma once
-
-
-typedef float forward_dif_t[5];
-
-float mp_init_forward_dif(forward_dif_t fdif, float Vi, float Vt, float s);
-float mp_next_forward_dif(forward_dif_t fdif);
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2016 Buildbotics LLC
- All rights reserved.
-
- 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/>.
-
- The software is distributed in the hope that it will be useful, but
- WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
- Lesser General Public License for more details.
-
- You should have received a copy of the GNU Lesser General Public
- License along with the software. If not, see
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-#include "jog.h"
-
-#include "axis.h"
-#include "planner.h"
-#include "buffer.h"
-#include "line.h"
-#include "forward_dif.h"
-#include "runtime.h"
-#include "machine.h"
-#include "state.h"
-#include "config.h"
-
-#include <stdbool.h>
-#include <math.h>
-#include <string.h>
-#include <stdio.h>
-#include <stdlib.h>
-
-
-typedef struct {
- bool writing;
- float Vi;
- float Vt;
- forward_dif_t fdifs[AXES];
- unsigned segments[AXES];
-
- int sign[AXES];
- float velocity[AXES];
- float next_velocity[AXES];
- float target_velocity[AXES];
-} jog_runtime_t;
-
-
-static jog_runtime_t jr;
-
-
-static stat_t _exec_jog(mp_buffer_t *bf) {
- // Load next velocity
- bool changed = false;
- bool done = true;
- if (!jr.writing)
- for (int axis = 0; axis < AXES; axis++) {
- float Vn = jr.next_velocity[axis] * axis_get_velocity_max(axis);
- float Vi = jr.velocity[axis];
- float Vt = jr.target_velocity[axis];
-
- if (JOG_MIN_VELOCITY < fabs(Vn)) done = false;
-
- if (!fp_ZERO(Vi) && (Vn < 0) != (Vi < 0))
- Vn = 0; // Plan to zero on sign change
-
- if (fabs(Vn) < JOG_MIN_VELOCITY) Vn = 0;
-
- if (Vt != Vn) {
- jr.target_velocity[axis] = Vn;
- if (Vn) jr.sign[axis] = Vn < 0 ? -1 : 1;
- changed = true;
- }
- }
-
- float velocity_sqr = 0;
-
- for (int axis = 0; axis < AXES; axis++) {
- float Vi = fabs(jr.velocity[axis]);
- float Vt = fabs(jr.target_velocity[axis]);
-
- if (changed) {
- if (fp_EQ(Vi, Vt)) {
- Vi = Vt;
- jr.segments[axis] = 0;
-
- } else {
- // Compute axis max jerk
- float jerk = axis_get_jerk_max(axis) * JERK_MULTIPLIER;
-
- // Compute length to velocity given max jerk
- float length = mp_get_target_length(Vi, Vt, jerk * JOG_JERK_MULT);
-
- // Compute move time
- float move_time = 2 * length / (Vi + Vt);
- if (move_time < MIN_SEGMENT_TIME) {
- Vi = Vt;
- jr.segments[axis] = 0;
-
- } else {
- jr.segments[axis] = ceil(move_time / NOM_SEGMENT_TIME);
-
- Vi = mp_init_forward_dif(jr.fdifs[axis], Vi, Vt, jr.segments[axis]);
- jr.segments[axis]--;
- }
- }
-
- } else if (jr.segments[axis]) {
- Vi += mp_next_forward_dif(jr.fdifs[axis]);
- jr.segments[axis]--;
-
- } else Vi = Vt;
-
- if (JOG_MIN_VELOCITY < Vi || JOG_MIN_VELOCITY < Vt) done = false;
-
- velocity_sqr += square(Vi);
- jr.velocity[axis] = Vi * jr.sign[axis];
- }
-
- // Check if we are done
- if (done) {
- // Update machine position
- for (int axis = 0; axis < AXES; axis++)
- mach_set_axis_position(axis, mp_runtime_get_work_position(axis));
-
- mp_set_cycle(CYCLE_MACHINING); // Default cycle
-
- return STAT_NOOP; // Done, no move executed
- }
-
- // Compute target from velocity
- float target[AXES];
- for (int axis = 0; axis < AXES; axis++)
- target[axis] = mp_runtime_get_axis_position(axis) +
- jr.velocity[axis] * NOM_SEGMENT_TIME;
-
- // Set velocity and target
- mp_runtime_set_velocity(sqrt(velocity_sqr));
- stat_t status = mp_runtime_move_to_target(target, NOM_SEGMENT_TIME);
- if (status != STAT_OK) return status;
-
- return STAT_EAGAIN;
-}
-
-
-bool mp_jog_busy() {return mp_get_cycle() == CYCLE_JOGGING;}
-
-
-uint8_t command_jog(int argc, char *argv[]) {
- if (!mp_jog_busy() &&
- (mp_get_state() != STATE_READY || mp_get_cycle() != CYCLE_MACHINING))
- return STAT_NOOP;
-
- float velocity[AXES];
-
- for (int axis = 0; axis < AXES; axis++)
- if (axis < argc - 1) velocity[axis] = atof(argv[axis + 1]);
- else velocity[axis] = 0;
-
- // Reset
- if (!mp_jog_busy()) memset(&jr, 0, sizeof(jr));
-
- jr.writing = true;
- for (int axis = 0; axis < AXES; axis++)
- jr.next_velocity[axis] = velocity[axis];
- jr.writing = false;
-
- if (!mp_jog_busy()) {
- mp_set_cycle(CYCLE_JOGGING);
- mp_queue_push_nonstop(_exec_jog, -1);
- }
-
- return STAT_OK;
-}
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2016 Buildbotics LLC
- All rights reserved.
-
- 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/>.
-
- The software is distributed in the hope that it will be useful, but
- WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
- Lesser General Public License for more details.
-
- You should have received a copy of the GNU Lesser General Public
- License along with the software. If not, see
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-#pragma once
-
-#include <stdbool.h>
-
-
-bool mp_jog_busy();
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2016 Buildbotics LLC
- Copyright (c) 2010 - 2015 Alden S. Hart, Jr.
- Copyright (c) 2012 - 2015 Rob Giseburt
- All rights reserved.
-
- 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/>.
-
- The software is distributed in the hope that it will be useful, but
- WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
- Lesser General Public License for more details.
-
- You should have received a copy of the GNU Lesser General Public
- License along with the software. If not, see
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-#include "line.h"
-
-#include "axis.h"
-#include "planner.h"
-#include "exec.h"
-#include "buffer.h"
-
-
-/* Sonny's algorithm - simple
- *
- * Computes the maximum allowable junction speed by finding the velocity that
- * will yield the centripetal acceleration in the corner_acceleration value. The
- * value of delta sets the effective radius of curvature. Here's Sonny's
- * (Sungeun K. Jeon's) explanation of what's going on:
- *
- * "First let's assume that at a junction we only look a centripetal
- * acceleration to simplify things. At a junction of two lines, let's place a
- * circle such that both lines are tangent to the circle. The circular segment
- * joining the lines represents the path for constant centripetal acceleration.
- * This creates a deviation from the path (let's call this delta),
- * which is the distance from the junction to the edge of the circular
- * segment. Delta needs to be defined, so let's replace the term max_jerk (see
- * note 1) with max_junction_deviation, or "delta". This indirectly sets the
- * radius of the circle, and hence limits the velocity by the centripetal
- * acceleration. Think of the this as widening the race track. If a race car is
- * driving on a track only as wide as a car, it'll have to slow down a lot to
- * turn corners. If we widen the track a bit, the car can start to use the
- * track to go into the turn. The wider it is, the faster through the corner
- * it can go.
- *
- * Note 1: "max_jerk" refers to the old grbl/marlin "max_jerk" approximation
- * term, not the TinyG jerk terms.
- *
- * If you do the geometry in terms of the known variables, you get:
- *
- * sin(theta/2) = R / (R + delta)
- *
- * Re-arranging in terms of circle radius (R)
- *
- * R = delta * sin(theta/2) / (1 - sin(theta/2))
- *
- * Theta is the angle between line segments given by:
- *
- * cos(theta) = dot(a, b) / (norm(a) * norm(b))
- *
- * Most of these calculations are already done in the planner.
- * To remove the acos() and sin() computations, use the trig half
- * angle identity:
- *
- * sin(theta/2) = +/-sqrt((1 - cos(theta)) / 2)
- *
- * For our applications, this should always be positive. Now just plug the
- * equations into the centripetal acceleration equation:
- *
- * v_c = sqrt(a_max * R)
- *
- * You'll see that there are only two sqrt computations and no sine/cosines.
- *
- * How to compute the radius using brute-force trig:
- *
- * float theta = acos(costheta);
- * float radius = delta * sin(theta/2) / (1 - sin(theta/2));
- *
- * This version extends Chamnit's algorithm by computing a value for delta that
- * takes the contributions of the individual axes in the move into account.
- * This allows the control radius to vary by axis. This is necessary to
- * support axes that have different dynamics; such as a Z axis that doesn't
- * move as fast as X and Y (such as a screw driven Z axis on machine with a belt
- * driven XY - like a Shapeoko), or rotary axes ABC that have completely
- * different dynamics than their linear counterparts.
- *
- * The function takes the absolute values of the sum of the unit vector
- * components as a measure of contribution to the move, then scales the delta
- * values from the non-zero axes into a composite delta to be used for the
- * move. Shown for an XY vector:
- *
- * U[i] Unit sum of i'th axis fabs(unit_a[i]) + fabs(unit_b[i])
- * Usum Length of sums Ux + Uy
- * d Delta of sums (Dx * Ux + DY * UY) / Usum
- */
-static float _get_junction_vmax(const float a_unit[], const float b_unit[]) {
- float costheta = 0;
- for (int axis = 0; axis < AXES; axis++)
- costheta -= a_unit[axis] * b_unit[axis];
-
- if (costheta < -0.99) return 10000000; // straight line cases
- if (0.99 < costheta) return 0; // reversal cases
-
- // Fuse the junction deviations into a vector sum
- float a_delta = 0;
- float b_delta = 0;
-
- for (int axis = 0; axis < AXES; axis++) {
- a_delta += square(a_unit[axis] * axis_get_junction_dev(axis));
- b_delta += square(b_unit[axis] * axis_get_junction_dev(axis));
- }
-
- if (!a_delta || !b_delta) return 0; // One or both unit vectors are null
-
- float delta = (sqrt(a_delta) + sqrt(b_delta)) / 2;
- float sintheta_over2 = sqrt((1 - costheta) / 2);
- float radius = delta * sintheta_over2 / (1 - sintheta_over2);
- float velocity = sqrt(radius * JUNCTION_ACCELERATION);
-
- return velocity;
-}
-
-
-/* Find the axis for which the jerk cannot be exceeded - the 'jerk_axis'.
- * This is the axis for which the time to decelerate from the target velocity
- * to zero would be the longest.
- *
- * We can determine the "longest" deceleration in terms of time or distance.
- *
- * The math for time-to-decelerate T from speed S to speed E with constant
- * jerk J is:
- *
- * T = 2 * sqrt((S - E) / J)
- *
- * Since E is always zero in this calculation, we can simplify:
- *
- * T = 2 * sqrt(S / J)
- *
- * The math for distance-to-decelerate l from speed S to speed E with
- * constant jerk J is:
- *
- * l = (S + E) * sqrt((S - E) / J)
- *
- * Since E is always zero in this calculation, we can simplify:
- *
- * l = S * sqrt(S / J)
- *
- * The final value we want to know is which one is *longest*, compared to the
- * others, so we don't need the actual value. This means that the scale of
- * the value doesn't matter, so for T we can remove the "2 *" and for L we can
- * remove the "S *". This leaves both to be simply "sqrt(S / J)". Since we
- * don't need the scale, it doesn't matter what speed we're coming from, so S
- * can be replaced with 1.
- *
- * However, we *do* need to compensate for the fact that each axis contributes
- * differently to the move, so we will scale each contribution C[n] by the
- * proportion of the axis movement length D[n]. Using that, we construct the
- * following, with these definitions:
- *
- * J[n] = max_jerk for axis n
- * D[n] = distance traveled for this move for axis n
- * C[n] = "axis contribution" of axis n
- *
- * For each axis n:
- *
- * C[n] = sqrt(1 / J[n]) * D[n]
- *
- * Keeping in mind that we only need a rank compared to the other axes, we can
- * further optimize the calculations:
- *
- * Square the expression to remove the square root:
- *
- * C[n]^2 = 1 / J[n] * D[n]^2
- *
- * We don't care that C is squared, so we'll use it that way. Also note that
- * we already have 1 / J[n] calculated for each axis.
- */
-int mp_find_jerk_axis(const float axis_square[]) {
- float C;
- float maxC = 0;
- int jerk_axis = 0;
-
- for (int axis = 0; axis < AXES; axis++)
- if (axis_square[axis]) { // Do not use fp_ZERO here
- // Squaring axis_length ensures it's positive
- C = axis_square[axis] * axis_get_recip_jerk(axis);
-
- if (maxC < C) {
- maxC = C;
- jerk_axis = axis;
- }
- }
-
- return jerk_axis;
-}
-
-
-/// Determine jerk value to use for the block.
-static float _calc_jerk(const float axis_square[], const float unit[]) {
- int axis = mp_find_jerk_axis(axis_square);
-
- // Finally, the selected jerk term needs to be scaled by the
- // reciprocal of the absolute value of the axis's unit
- // vector term. This way when the move is finally decomposed into
- // its constituent axes for execution the jerk for that axis will be
- // at it's maximum value.
- return axis_get_jerk_max(axis) * JERK_MULTIPLIER / fabs(unit[axis]);
-}
-
-
-/// Compute cached jerk terms used by planning
-static void _calc_and_cache_jerk_values(mp_buffer_t *bf) {
- static float jerk = 0;
- static float cbrt_jerk = 0;
-
- if (JERK_MATCH_PRECISION < fabs(bf->jerk - jerk)) { // Tolerance comparison
- jerk = bf->jerk;
- cbrt_jerk = cbrt(bf->jerk);
- }
-
- bf->cbrt_jerk = cbrt_jerk;
-}
-
-
-static void _calc_max_velocities(mp_buffer_t *bf, float move_time,
- bool exact_stop) {
- float junction_velocity =
- _get_junction_vmax(mp_buffer_prev(bf)->unit, bf->unit);
-
- bf->cruise_vmax = bf->length / move_time; // target velocity requested
- bf->entry_vmax = min(bf->cruise_vmax, junction_velocity);
- bf->delta_vmax = mp_get_target_velocity(0, bf->length, bf);
- bf->exit_vmax = min(bf->cruise_vmax, (bf->entry_vmax + bf->delta_vmax));
- bf->braking_velocity = bf->delta_vmax;
-
- // Zero out exact stop cases
- if (exact_stop) bf->entry_vmax = bf->exit_vmax = 0;
- else bf->replannable = true;
-}
-
-
-/* Compute optimal and minimum move times
- *
- * "Minimum time" is the fastest the move can be performed given the velocity
- * constraints on each participating axis - regardless of the feed rate
- * requested. The minimum time is the time limited by the rate-limiting
- * axis. The minimum time is needed to compute the optimal time and is recorded
- * for possible feed override computation.
- *
- * "Optimal time" is either the time resulting from the requested feed rate or
- * the minimum time if the requested feed rate is not achievable. Optimal times
- * for rapids are always the minimum time.
- *
- * The following times are compared and the longest is returned:
- * - G93 inverse time (if G93 is active)
- * - time for coordinated move at requested feed rate
- * - time that the slowest axis would require for the move
- *
- * NIST RS274NGC_v3 Guidance
- *
- * The following is verbatim text from NIST RS274NGC_v3. As I interpret A for
- * moves that combine both linear and rotational movement, the feed rate should
- * apply to the XYZ movement, with the rotational axis (or axes) timed to start
- * and end at the same time the linear move is performed. It is possible under
- * this case for the rotational move to rate-limit the linear move.
- *
- * 2.1.2.5 Feed Rate
- *
- * The rate at which the controlled point or the axes move is nominally a steady
- * rate which may be set by the user. In the Interpreter, the interpretation of
- * the feed rate is as follows unless inverse time feed rate mode is being used
- * in the RS274/NGC view (see Section 3.5.19). The machining functions view of
- * feed rate, as described in Section 4.3.5.1, has conditions under which the
- * set feed rate is applied differently, but none of these is used in the
- * Interpreter.
- *
- * A. For motion involving one or more of the X, Y, and Z axes (with or without
- * simultaneous rotational axis motion), the feed rate means length units
- * per minute along the programmed XYZ path, as if the rotational axes were
- * not moving.
- *
- * B. For motion of one rotational axis with X, Y, and Z axes not moving, the
- * feed rate means degrees per minute rotation of the rotational axis.
- *
- * C. For motion of two or three rotational axes with X, Y, and Z axes not
- * moving, the rate is applied as follows. Let dA, dB, and dC be the angles
- * in degrees through which the A, B, and C axes, respectively, must move.
- * Let D = sqrt(dA^2 + dB^2 + dC^2). Conceptually, D is a measure of total
- * angular motion, using the usual Euclidean metric. Let T be the amount of
- * time required to move through D degrees at the current feed rate in
- * degrees per minute. The rotational axes should be moved in coordinated
- * linear motion so that the elapsed time from the start to the end of the
- * motion is T plus any time required for acceleration or deceleration.
- */
-static float _calc_move_time(const float axis_length[],
- const float axis_square[], bool rapid,
- bool inverse_time, float feed_rate,
- float feed_override) {
- float max_time = 0;
-
- // Compute times for feed motion
- if (!rapid) {
- if (inverse_time) max_time = feed_rate;
- else {
- // Compute length of linear move in millimeters. Feed rate in mm/min.
- max_time = sqrt(axis_square[AXIS_X] + axis_square[AXIS_Y] +
- axis_square[AXIS_Z]) / feed_rate;
-
- // If no linear axes, compute length of multi-axis rotary move in degrees.
- // Feed rate is provided as degrees/min
- if (fp_ZERO(max_time))
- max_time = sqrt(axis_square[AXIS_A] + axis_square[AXIS_B] +
- axis_square[AXIS_C]) / feed_rate;
- }
- }
-
- // Apply feed override
- max_time /= feed_override;
-
- // Compute time required for rate-limiting axis
- for (int axis = 0; axis < AXES; axis++) {
- float time = fabs(axis_length[axis]) /
- (rapid ? axis_get_velocity_max(axis) : axis_get_feedrate_max(axis));
-
- if (max_time < time) max_time = time;
- }
-
- return max_time < MIN_SEGMENT_TIME ? MIN_SEGMENT_TIME : max_time;
-}
-
-
-/*** Plan line acceleration / deceleration
- *
- * This function uses constant jerk motion equations to plan acceleration and
- * deceleration. Jerk is the rate of change of acceleration; it's the 1st
- * derivative of acceleration, and the 3rd derivative of position. Jerk is a
- * measure of impact to the machine. Controlling jerk smooths transitions
- * between moves and allows for faster feeds while controlling machine
- * oscillations and other undesirable side-effects.
- *
- * Notes:
- * [1] All math is done in absolute coordinates using single precision floats.
- *
- * [2] 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.
- *
- * @param reed_rate is in minutes when @param inverse_time is true.
- * See mach_set_feed_rate()
- */
-stat_t mp_aline(const float target[], bool rapid, bool inverse_time,
- bool exact_stop, float feed_rate, float feed_override,
- int32_t line) {
- // Compute axis and move lengths
- float axis_length[AXES];
- float axis_square[AXES];
- float length_square = 0;
-
- for (int axis = 0; axis < AXES; axis++) {
- axis_length[axis] = target[axis] - mp_get_axis_position(axis);
- axis_square[axis] = square(axis_length[axis]);
- length_square += axis_square[axis];
- }
-
- float length = sqrt(length_square);
- if (fp_ZERO(length)) return STAT_OK;
-
- // Get a buffer. Note, new buffers are initialized to zero.
- mp_buffer_t *bf = mp_queue_get_tail(); // current move pointer
-
- // Set buffer values
- bf->length = length;
- copy_vector(bf->target, target);
-
- // Compute unit vector
- for (int axis = 0; axis < AXES; axis++)
- bf->unit[axis] = axis_length[axis] / length;
-
- // Compute and cache jerk values
- bf->jerk = _calc_jerk(axis_square, bf->unit);
- _calc_and_cache_jerk_values(bf);
-
- // Compute move time and velocities
- float time = _calc_move_time(axis_length, axis_square, rapid, inverse_time,
- feed_rate, feed_override);
- _calc_max_velocities(bf, time, exact_stop);
-
- // Note, the following lines must remain in order.
- bf->line = line; // Planner needs then when planning steps
- mp_plan(bf); // Plan block list
- mp_set_position(target); // Set planner position before committing buffer
- mp_queue_push(mp_exec_aline, line); // After position update
-
- return STAT_OK;
-}
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2016 Buildbotics LLC
- All rights reserved.
-
- 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/>.
-
- The software is distributed in the hope that it will be useful, but
- WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
- Lesser General Public License for more details.
-
- You should have received a copy of the GNU Lesser General Public
- License along with the software. If not, see
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-#pragma once
-
-#include "status.h"
-
-#include <stdbool.h>
-
-
-stat_t mp_aline(const float target[], bool rapid, bool inverse_time,
- bool exact_stop, float feed_rate, float feed_override,
- int32_t line);
-int mp_find_jerk_axis(const float axis_square[]);
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2016 Buildbotics LLC
- Copyright (c) 2010 - 2015 Alden S. Hart, Jr.
- Copyright (c) 2012 - 2015 Rob Giseburt
- All rights reserved.
-
- 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/>.
-
- The software is distributed in the hope that it will be useful, but
- WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
- Lesser General Public License for more details.
-
- You should have received a copy of the GNU Lesser General Public
- License along with the software. If not, see
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-/* Planner Notes
- *
- * The planner works below the machine and above the
- * motor mapping and stepper execution layers. A rudimentary
- * multitasking capability is implemented for long-running commands
- * such as lines, arcs, and dwells. These functions are coded as
- * non-blocking continuations - which are simple state machines
- * that are re-entered multiple times until a particular operation
- * is complete. These functions have 2 parts - the initial call,
- * which sets up the local context, and callbacks (continuations)
- * that are called from the main loop.
- *
- * One important concept is isolation of the three layers of the
- * data model - the Gcode model (gm), planner model (bf queue & mm),
- * and runtime model (exec.c).
- *
- * The Gcode model is owned by the machine and should only be
- * accessed by mach_xxxx() functions. Data from the Gcode model is
- * transferred to the planner by the mp_xxx() functions called by
- * the machine.
- *
- * The planner should only use data in the planner model. When a
- * move (block) is ready for execution the planner data is
- * transferred to the runtime model, which should also be isolated.
- *
- * Lower-level models should never use data from upper-level models
- * as the data may have changed and lead to unpredictable results.
- */
-
-#include "planner.h"
-
-#include "axis.h"
-#include "buffer.h"
-#include "machine.h"
-#include "stepper.h"
-#include "motor.h"
-#include "state.h"
-#include "usart.h"
-
-#include <string.h>
-#include <stdbool.h>
-#include <stdio.h>
-
-
-typedef struct {
- float position[AXES]; // final move position for planning purposes
- bool plan_steps; // if true plan one GCode line at a time
-} planner_t;
-
-
-static planner_t mp = {{0}};
-
-
-void mp_init() {mp_queue_init();}
-
-
-/// Set planner position for a single axis
-void mp_set_axis_position(int axis, float p) {mp.position[axis] = p;}
-float mp_get_axis_position(int axis) {return mp.position[axis];}
-void mp_set_position(const float p[]) {copy_vector(mp.position, p);}
-void mp_set_plan_steps(bool plan_steps) {mp.plan_steps = plan_steps;}
-
-
-/*** Flush all moves in the planner
- *
- * Does not affect the move currently running. 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 usually
- * be directly called. Call mp_request_flush() instead.
- */
-void mp_flush_planner() {mp_queue_init();}
-
-
-/*** Performs axis mapping & conversion of length units to steps.
- *
- * The reason steps are returned as floats (as opposed to, say,
- * uint32_t) is to accommodate fractional steps. stepper.c deals
- * with fractional step values as fixed-point binary in order to get
- * the smoothest possible operation. Steps are passed to the move prep
- * routine as floats and converted to fixed-point binary during queue
- * loading. See stepper.c for details.
- */
-void mp_kinematics(const float travel[], float steps[]) {
- // You could insert inverse kinematics transformations here
- // or just use travel directly for Cartesian machines.
-
- // Map motors to axes and convert length units to steps
- // Most of the conversion math has already been done during config in
- // steps_per_unit() which takes axis travel, step angle and microsteps into
- // account.
- for (int motor = 0; motor < MOTORS; motor++)
- steps[motor] =
- travel[motor_get_axis(motor)] * motor_get_steps_per_unit(motor);
-}
-
-
-// The minimum lengths are dynamic and depend on the velocity. These
-// expressions evaluate to the minimum lengths for the current velocity
-// settings. Note: The head and tail lengths are 2 minimum segments, the body
-// is 1 min segment.
-#define MIN_HEAD_LENGTH \
- (MIN_SEGMENT_TIME_PLUS_MARGIN * (bf->cruise_velocity + bf->entry_velocity))
-#define MIN_TAIL_LENGTH \
- (MIN_SEGMENT_TIME_PLUS_MARGIN * (bf->cruise_velocity + bf->exit_velocity))
-#define MIN_BODY_LENGTH (MIN_SEGMENT_TIME_PLUS_MARGIN * bf->cruise_velocity)
-
-
-/*** Calculate move acceleration / deceleration
- *
- * This rather brute-force and long-ish function sets section lengths and
- * velocities based on the line length and velocities requested. It modifies
- * the incoming bf buffer and returns accurate head, body and tail lengths, and
- * accurate or reasonably approximate velocities. We care about accuracy on
- * lengths, less so for velocity, as long as velocity errs on the side of too
- * slow.
- *
- * Note: We need the velocities to be set even for zero-length sections (Note:
- * sections, not moves) so we can compute entry and exits for adjacent sections.
- *
- * Inputs used are:
- *
- * bf->length - actual block length, length is never changed
- * bf->entry_velocity - requested Ve, entry velocity is never changed
- * bf->cruise_velocity - requested Vt, is often changed
- * bf->exit_velocity - requested Vx, may change for degenerate cases
- * bf->cruise_vmax - used in some comparisons
- * bf->delta_vmax - used to degrade velocity of short blocks
- *
- * Variables that may be set/updated are:
- *
- * bf->entry_velocity - requested Ve
- * bf->cruise_velocity - requested Vt
- * bf->exit_velocity - requested Vx
- * bf->head_length - bf->length allocated to head
- * bf->body_length - bf->length allocated to body
- * bf->tail_length - bf->length allocated to tail
- *
- * Note: The following conditions must be met on entry:
- *
- * bf->length must be non-zero (filter these out upstream)
- * bf->entry_velocity <= bf->cruise_velocity >= bf->exit_velocity
- *
- * Classes of moves:
- *
- * Requested-Fit - The move has sufficient length to achieve the target
- * velocity (cruise velocity). I.e it will accommodate the acceleration /
- * deceleration profile in the given length.
- *
- * Rate-Limited-Fit - The move does not have sufficient length to achieve
- * target velocity. In this case the cruise velocity will be set lower than
- * the requested velocity (incoming bf->cruise_velocity). The entry and
- * exit velocities are satisfied.
- *
- * Degraded-Fit - The move does not have sufficient length to transition from
- * the entry velocity to the exit velocity in the available length. These
- * velocities are not negotiable, so a degraded solution is found.
- *
- * In worst cases, the move cannot be executed as the required execution
- * time is less than the minimum segment time. The first degradation is to
- * reduce the move to a body-only segment with an average velocity. If that
- * still doesn't fit then the move velocity is reduced so it fits into a
- * minimum segment. This will reduce the velocities in that region of the
- * planner buffer as the moves are replanned to that worst-case move.
- *
- * Various cases handled (H=head, B=body, T=tail)
- *
- * Requested-Fit cases:
- *
- * HBT Ve<Vt>Vx sufficient length exists for all parts (corner
- * case: HBT')
- * HB Ve<Vt=Vx head accelerates to cruise - exits at full speed
- * (corner case: H')
- * BT Ve=Vt>Vx enter at full speed & decelerate (corner case: T')
- * HT Ve & Vx perfect fit HT (very rare). May be symmetric or
- * asymmetric
- * H Ve<Vx perfect fit H (common, results from planning)
- * T Ve>Vx perfect fit T (common, results from planning)
- * B Ve=Vt=Vx Velocities are close to each other and within
- * matching tolerance
- *
- * Rate-Limited cases - Ve and Vx can be satisfied but Vt cannot:
- *
- * HT (Ve=Vx)<Vt symmetric case. Split the length and compute Vt.
- * HT' (Ve!=Vx)<Vt asymmetric case. Find H and T by successive
- * approximation.
- * HBT' body length < min body length - treated as an HT case
- * H' body length < min body length - subsume body into head length
- * T' body length < min body length - subsume body into tail length
- *
- * Degraded fit cases - line is too short to satisfy both Ve and Vx:
- *
- * H" Ve<Vx Ve is degraded (velocity step). Vx is met
- * T" Ve>Vx Ve is degraded (velocity step). Vx is met
- * B" <short> line is very short but drawable; is treated as a
- * body only
- * F <too short> force fit: This block is slowed down until it can
- * be executed
- *
- * Note: The order of the cases/tests in the code is important. Start with
- * the shortest cases first and work up. Not only does this simplify the order
- * of the tests, but it reduces execution time when you need it most - when
- * tons of pathologically short Gcode blocks are being thrown at you.
- */
-void mp_calculate_trapezoid(mp_buffer_t *bf) {
- // RULE #1 of mp_calculate_trapezoid(): Don't change bf->length
-
- if (!bf->length) return;
-
- // F case: Block is too short - run time < minimum segment time
- // Force block into a single segment body with limited velocities
- // Accept the entry velocity, limit the cruise, and go for the best exit
- // velocity you can get given the delta_vmax (maximum velocity slew).
-
- float naive_move_time =
- 2 * bf->length / (bf->entry_velocity + bf->exit_velocity); // average
-
- if (naive_move_time < MIN_SEGMENT_TIME_PLUS_MARGIN) {
- bf->cruise_velocity = bf->length / MIN_SEGMENT_TIME_PLUS_MARGIN;
- bf->exit_velocity =
- max(0, min(bf->cruise_velocity, (bf->entry_velocity - bf->delta_vmax)));
- bf->body_length = bf->length;
- bf->head_length = 0;
- bf->tail_length = 0;
-
- // Violating jerk but since it's a single segment move we don't use it.
- return;
- }
-
- // B" case: Block is short, but fits into a single body segment
- if (naive_move_time <= NOM_SEGMENT_TIME) {
- bf->entry_velocity = mp_buffer_prev(bf)->exit_velocity;
-
- if (fp_NOT_ZERO(bf->entry_velocity)) {
- bf->cruise_velocity = bf->entry_velocity;
- bf->exit_velocity = bf->entry_velocity;
-
- } else {
- bf->cruise_velocity = bf->delta_vmax / 2;
- bf->exit_velocity = bf->delta_vmax;
- }
-
- bf->body_length = bf->length;
- bf->head_length = 0;
- bf->tail_length = 0;
-
- // Violating jerk but since it's a single segment move we don't use it.
- return;
- }
-
- // B case: Velocities all match (or close enough)
- // This occurs frequently in normal gcode files with lots of short lines.
- // This case is not really necessary, but saves lots of processing time.
- if (((bf->cruise_velocity - bf->entry_velocity) <
- TRAPEZOID_VELOCITY_TOLERANCE) &&
- ((bf->cruise_velocity - bf->exit_velocity) <
- TRAPEZOID_VELOCITY_TOLERANCE)) {
- bf->body_length = bf->length;
- bf->head_length = 0;
- bf->tail_length = 0;
-
- return;
- }
-
- // Head-only and tail-only short-line cases
- // H" and T" degraded-fit cases
- // H' and T' requested-fit cases where the body residual is less than
- // MIN_BODY_LENGTH
- bf->body_length = 0;
- float minimum_length =
- mp_get_target_length(bf->entry_velocity, bf->exit_velocity, bf->jerk);
-
- // head-only & tail-only cases
- if (bf->length <= (minimum_length + MIN_BODY_LENGTH)) {
- // tail-only cases (short decelerations)
- if (bf->entry_velocity > bf->exit_velocity) {
- // T" (degraded case)
- if (bf->length < minimum_length)
- bf->entry_velocity =
- mp_get_target_velocity(bf->exit_velocity, bf->length, bf);
-
- bf->cruise_velocity = bf->entry_velocity;
- bf->tail_length = bf->length;
- bf->head_length = 0;
-
- return;
- }
-
- // head-only cases (short accelerations)
- if (bf->entry_velocity < bf->exit_velocity) {
- // H" (degraded case)
- if (bf->length < minimum_length)
- bf->exit_velocity =
- mp_get_target_velocity(bf->entry_velocity, bf->length, bf);
-
- bf->cruise_velocity = bf->exit_velocity;
- bf->head_length = bf->length;
- bf->tail_length = 0;
-
- return;
- }
- }
-
- // Set head and tail lengths for evaluating the next cases
- bf->head_length =
- mp_get_target_length(bf->entry_velocity, bf->cruise_velocity, bf->jerk);
- bf->tail_length =
- mp_get_target_length(bf->exit_velocity, bf->cruise_velocity, bf->jerk);
- if (bf->head_length < MIN_HEAD_LENGTH) { bf->head_length = 0;}
- if (bf->tail_length < MIN_TAIL_LENGTH) { bf->tail_length = 0;}
-
- // Rate-limited HT and HT' cases
- if (bf->length < (bf->head_length + bf->tail_length)) { // it's rate limited
-
- // Symmetric rate-limited case (HT)
- if (fabs(bf->entry_velocity - bf->exit_velocity) <
- TRAPEZOID_VELOCITY_TOLERANCE) {
- bf->head_length = bf->length/2;
- bf->tail_length = bf->head_length;
- bf->cruise_velocity =
- min(bf->cruise_vmax,
- mp_get_target_velocity(bf->entry_velocity, bf->head_length, bf));
-
- if (bf->head_length < MIN_HEAD_LENGTH) {
- // Convert this to a body-only move
- bf->body_length = bf->length;
- bf->head_length = 0;
- bf->tail_length = 0;
-
- // Average the entry speed and computed best cruise-speed
- bf->cruise_velocity = (bf->entry_velocity + bf->cruise_velocity)/2;
- bf->entry_velocity = bf->cruise_velocity;
- bf->exit_velocity = bf->cruise_velocity;
- }
-
- return;
- }
-
- // Asymmetric HT' rate-limited case. This is relatively expensive but it's
- // not called very often
- float computed_velocity = bf->cruise_vmax;
- do {
- // initialize from previous iteration
- bf->cruise_velocity = computed_velocity;
- bf->head_length =
- mp_get_target_length(bf->entry_velocity, bf->cruise_velocity, bf->jerk);
- bf->tail_length =
- mp_get_target_length(bf->exit_velocity, bf->cruise_velocity, bf->jerk);
-
- if (bf->head_length > bf->tail_length) {
- bf->head_length =
- (bf->head_length / (bf->head_length + bf->tail_length)) * bf->length;
- computed_velocity =
- mp_get_target_velocity(bf->entry_velocity, bf->head_length, bf);
-
- } else {
- bf->tail_length =
- (bf->tail_length / (bf->head_length + bf->tail_length)) * bf->length;
- computed_velocity =
- mp_get_target_velocity(bf->exit_velocity, bf->tail_length, bf);
- }
-
- } while ((fabs(bf->cruise_velocity - computed_velocity) /
- computed_velocity) > TRAPEZOID_ITERATION_ERROR_PERCENT);
-
- // set velocity and clean up any parts that are too short
- bf->cruise_velocity = computed_velocity;
- bf->head_length =
- mp_get_target_length(bf->entry_velocity, bf->cruise_velocity, bf->jerk);
- bf->tail_length = bf->length - bf->head_length;
-
- if (bf->head_length < MIN_HEAD_LENGTH) {
- // adjust the move to be all tail...
- bf->tail_length = bf->length;
- bf->head_length = 0;
- }
-
- if (bf->tail_length < MIN_TAIL_LENGTH) {
- bf->head_length = bf->length; //...or all head
- bf->tail_length = 0;
- }
-
- return;
- }
-
- // Requested-fit cases: remaining of: HBT, HB, BT, BT, H, T, B, cases
- bf->body_length = bf->length - bf->head_length - bf->tail_length;
-
- // If a non-zero body is < minimum length distribute it to the head and/or
- // tail. This will generate small (acceptable) velocity errors in runtime
- // execution but preserve correct distance, which is more important.
- if ((bf->body_length < MIN_BODY_LENGTH) && (fp_NOT_ZERO(bf->body_length))) {
- if (fp_NOT_ZERO(bf->head_length)) {
- if (fp_NOT_ZERO(bf->tail_length)) { // HBT reduces to HT
- bf->head_length += bf->body_length / 2;
- bf->tail_length += bf->body_length / 2;
-
- } else bf->head_length += bf->body_length; // HB reduces to H
- } else bf->tail_length += bf->body_length; // BT reduces to T
-
- bf->body_length = 0;
-
- // If the body is a standalone make the cruise velocity match the entry
- // velocity. This removes a potential velocity discontinuity at the expense
- // of top speed
- } else if ((fp_ZERO(bf->head_length)) && (fp_ZERO(bf->tail_length)))
- bf->cruise_velocity = bf->entry_velocity;
-}
-
-
-#if 0
-/// Prints the entire planning queue as comma separated values embedded in
-/// JSON ``msg`` entries. Used for debugging.
-void mp_print_queue(mp_buffer_t *bf) {
- printf_P(PSTR("{\"msg\":\"id,replannable,callback,"
- "length,head_length,body_length,tail_length,"
- "entry_velocity,cruise_velocity,exit_velocity,braking_velocity,"
- "entry_vmax,cruise_vmax,exit_vmax\"}\n"));
-
- int i = 0;
- mp_buffer_t *bp = bf;
- while (bp) {
- printf_P(PSTR("{\"msg\":\"%d,%d,0x%04x,"
- "%0.2f,%0.2f,%0.2f,%0.2f,"
- "%0.2f,%0.2f,%0.2f,%0.2f,"
- "%0.2f,%0.2f,%0.2f\"}\n"),
- i++, bp->replannable, bp->cb,
- bp->length, bp->head_length, bp->body_length, bp->tail_length,
- bp->entry_velocity, bp->cruise_velocity, bp->exit_velocity,
- bp->braking_velocity,
- bp->entry_vmax, bp->cruise_vmax, bp->exit_vmax);
-
- bp = mp_buffer_prev(bp);
- if (bp == bf || bp->state == BUFFER_OFF) break;
- }
-
- while (!usart_tx_empty()) continue;
-}
-#endif
-
-
-/*** Plans the entire queue
- *
- * The block list is the circular buffer of planner buffers (bl's). The block
- * currently being planned is the "bl" block. The "first block" is the next
- * block to execute; queued immediately behind the currently executing block,
- * aka the "running" block. In some cases, there is no first block because the
- * list is empty or there is only one block and it is already running.
- *
- * If blocks following the first block are already optimally planned (non
- * replannable) the first block that is not optimally planned becomes the
- * effective first block.
- *
- * mp_plan() plans all blocks between and including the (effective)
- * first block and the bl. It sets entry, exit and cruise v's from vmax's then
- * calls trapezoid generation.
- *
- * Variables that must be provided in the mp_buffer_t that will be processed:
- *
- * bl (function arg) - end of block list (last block in time)
- * bl->replannable - start of block list set by last FALSE value
- * [Note 1]
- * bl->move_type - typically MOVE_TYPE_ALINE. Other move_types should
- * be set to length=0, entry_vmax=0 and exit_vmax=0
- * and are treated as a momentary stop (plan to zero
- * and from zero).
- * bl->length - provides block length
- * bl->entry_vmax - used during forward planning to set entry velocity
- * bl->cruise_vmax - used during forward planning to set cruise velocity
- * bl->exit_vmax - used during forward planning to set exit velocity
- * bl->delta_vmax - used during forward planning to set exit velocity
- * bl->cbrt_jerk - used during trapezoid generation
- *
- * Variables that will be set during processing:
- *
- * bl->replannable - set if the block becomes optimally planned
- * bl->braking_velocity - set during backward planning
- * bl->entry_velocity - set during forward planning
- * bl->cruise_velocity - set during forward planning
- * bl->exit_velocity - set during forward planning
- * bl->head_length - set during trapezoid generation
- * bl->body_length - set during trapezoid generation
- * bl->tail_length - set during trapezoid generation
- *
- * Variables that are ignored but here's what you would expect them to be:
- *
- * bl->state - BUFFER_NEW for all blocks but the earliest
- * bl->target[] - block target position
- * bl->unit[] - block unit vector
- * bl->jerk - source of the other jerk variables.
- *
- * Notes:
- *
- * [1] Whether or not a block is planned is controlled by the bl->replannable
- * setting. Replan flags are checked during the backwards pass. They prune
- * the replan list to include only the latest blocks that require planning.
- *
- * In normal operation, the first block (currently running block) is not
- * replanned, but may be for feedholds and feed overrides. In these cases,
- * the prep routines modify the contents of the (ex) buffer and re-shuffle
- * the block list, re-enlisting the current bl buffer with new parameters.
- * These routines also set all blocks in the list to be replannable so the
- * list can be recomputed regardless of exact stops and previous replanning
- * optimizations.
- */
-void mp_plan(mp_buffer_t *bl) {
- mp_buffer_t *bp = bl;
-
- // Backward planning pass. Find first block and update braking velocities.
- // By the end bp points to the buffer before the first block.
- mp_buffer_t *next = bp;
- while ((bp = mp_buffer_prev(bp)) != bl) {
- if (!bp->replannable) break;
-
- bp->braking_velocity =
- min(next->entry_vmax, next->braking_velocity) + bp->delta_vmax;
-
- next = bp;
- }
-
- // Forward planning pass. Recompute trapezoids from the first block to bl.
- mp_buffer_t *prev = bp;
- while ((bp = mp_buffer_next(bp)) != bl) {
- mp_buffer_t *next = mp_buffer_next(bp);
-
- bp->entry_velocity = prev == bl ? bp->entry_vmax : prev->exit_velocity;
- bp->cruise_velocity = bp->cruise_vmax;
- bp->exit_velocity = min4(bp->exit_vmax, next->entry_vmax,
- next->braking_velocity,
- bp->entry_velocity + bp->delta_vmax);
-
- if (mp.plan_steps && bp->line != next->line) {
- bp->exit_velocity = 0;
- bp->hold = true;
-
- } else bp->hold = false;
-
- mp_calculate_trapezoid(bp);
-
- // Test for optimally planned trapezoids by checking exit conditions
- if ((fp_EQ(bp->exit_velocity, bp->exit_vmax) ||
- fp_EQ(bp->exit_velocity, next->entry_vmax)) ||
- (!prev->replannable &&
- fp_EQ(bp->exit_velocity, (bp->entry_velocity + bp->delta_vmax))))
- bp->replannable = false;
-
- prev = bp;
- }
-
- // Finish last block
- bl->entry_velocity = prev->exit_velocity;
- bl->cruise_velocity = bl->cruise_vmax;
- bl->exit_velocity = 0;
-
- mp_calculate_trapezoid(bl);
-}
-
-
-void mp_replan_all() {
- ASSERT(mp_get_state() == STATE_READY || mp_get_state() == STATE_HOLDING);
-
- // Get next buffer
- mp_buffer_t *bf = mp_queue_get_head();
- if (!bf) return;
-
- mp_buffer_t *bp = bf;
-
- // Mark all blocks replanable
- while (true) {
- bp->replannable = true;
- mp_buffer_t *next = mp_buffer_next(bp);
- if (next->state == BUFFER_OFF || next == bf) break; // Avoid wrap around
- bp = next;
- }
-
- // Plan blocks
- mp_plan(bp);
-}
-
-
-/// Push a non-stop command to the queue. I.e. one that does not cause the
-/// planner to plan to zero.
-void mp_queue_push_nonstop(buffer_cb_t cb, uint32_t line) {
- mp_buffer_t *bp = mp_queue_get_tail();
-
- bp->entry_vmax = bp->cruise_vmax = bp->exit_vmax = INFINITY;
- copy_vector(bp->unit, bp->prev->unit);
- bp->replannable = true;
-
- mp_queue_push(cb, line);
-}
-
-
-/*** Derive accel/decel length from delta V and jerk
- *
- * A convenient function for determining the optimal_length (L) of a line
- * given the initial velocity (Vi), final velocity (Vf) and maximum jerk (Jm).
- *
- * Definitions:
- *
- * Jm = the given maximum jerk
- * T = time of the entire move
- * Vi = initial velocity
- * Vf = final velocity
- * T = time
- * As = accel at inflection point between convex & concave portions of S-curve
- * Ar = ramp acceleration
- *
- * Formulas:
- *
- * T = 2 * sqrt((Vt - Vi) / Jm)
- * As = (Jm * T) / 2
- * Ar = As / 2 = (Jm * T) / 4
- *
- * Then the length (distance) equation is:
- *
- * a) L = (Vf - Vi) * T - (Ar * T^2) / 2
- *
- * Substituting for Ar and T:
- *
- * b) L = (Vf - Vi) * 2 * sqrt((Vf - Vi) / Jm) -
- * (2 * sqrt((Vf - Vi) / Jm) * (Vf - Vi)) / 2
- *
- * Reducing b). See Wolfram Alpha:
- *
- * c) L = (Vf - Vi)^(3/2) / sqrt(Jm)
- *
- * Assuming Vf >= Vi [Note 2]:
- *
- * d) L = (Vf - Vi) * sqrt((Vf - Vi) / Jm)
- *
- * Notes:
- *
- * [1] Assuming Vi, Vf and L are positive or zero.
- * [2] Cannot assume Vf >= Vi due to rounding errors and use of
- * PLANNER_VELOCITY_TOLERANCE necessitating the introduction of fabs()
- */
-float mp_get_target_length(float Vi, float Vf, float jerk) {
- return fp_EQ(Vi, Vf) ? 0 : fabs(Vi - Vf) * invsqrt(jerk / fabs(Vi - Vf));
-}
-
-
-#define GET_VELOCITY_ITERATIONS 0 // must be zero or more
-
-/*** Derive velocity achievable from delta V and length
- *
- * A convenient function for determining Vf target velocity for a given
- * initial velocity (Vi), length (L), and maximum jerk (Jm). Equation e) is
- * b) solved for Vf. Equation f) is c) solved for Vf. Use f) (obviously)
- *
- * e) Vf = (sqrt(L) * (L / sqrt(1 / Jm))^(1/6) +
- * (1 / Jm)^(1/4) * Vi) / (1 / Jm)^(1/4)
- *
- * f) Vf = L^(2/3) * Jm^(1/3) + Vi
- *
- * FYI: Here's an expression that returns the jerk for a given deltaV and L:
- *
- * cube(deltaV / (pow(L, 0.66666666)));
- *
- * We do some Newton-Raphson iterations to narrow it down.
- * We need a formula that includes known variables except the one we want to
- * find, and has a root [Z(x) = 0] at the value (x) we are looking for.
- *
- * Z(x) = zero at x
- *
- * We calculate the value from the knowns and the estimate (see below) and then
- * subtract the known value to get zero (root) if x is the correct value.
- *
- * Vi = initial velocity (known)
- * Vf = estimated final velocity
- * J = jerk (known)
- * L = length (know)
- *
- * There are (at least) two such functions we can use:
- *
- * L from J, Vi, and Vf:
- *
- * L = sqrt((Vf - Vi) / J) * (Vi + Vf)
- *
- * Replacing Vf with x, and subtracting the known L we get:
- *
- * 0 = sqrt((x - Vi) / J) * (Vi + x) - L
- * Z(x) = sqrt((x - Vi) / J) * (Vi + x) - L
- *
- * Or J from L, Vi, and Vf:
- *
- * J = ((Vf - Vi) * (Vi + Vf)^2) / L^2
- *
- * Replacing Vf with x, and subtracting the known J we get:
- *
- * 0 = ((x - Vi) * (Vi + x)^2) / L^2 - J
- * Z(x) = ((x - Vi) * (Vi + x)^2) / L^2 - J
- *
- * L doesn't resolve to the value very quickly (its graph is nearly vertical).
- * So, we'll use J, which resolves in < 10 iterations, often in only two or
- * three with a good estimate.
- *
- * In order to do a Newton-Raphson iteration, we need the derivative. Here
- * they are for both the (unused) L and the (used) J formulas above:
- *
- * J > 0, Vi > 0, Vf > 0
- * A = sqrt((x - Vi) * J)
- * B = sqrt((x - Vi) / J)
- *
- * L'(x) = B + (Vi + x) / (2 * J) + (Vi + x) / (2 * A)
- *
- * J'(x) = (2 * Vi * x - Vi^2 + 3 * x^2) / L^2
- */
-float mp_get_target_velocity(float Vi, float L, const mp_buffer_t *bf) {
- // 0 iterations (a reasonable estimate)
- float x = pow(L, 0.66666666) * bf->cbrt_jerk + Vi; // First estimate
-
-#if (GET_VELOCITY_ITERATIONS > 0)
- const float L2 = L * L;
- const float Vi2 = Vi * Vi;
-
- for (int i = 0; i < GET_VELOCITY_ITERATIONS; i++)
- // x' = x - Z(x) / J'(x)
- x = x - ((x - Vi) * square(Vi + x) / L2 - bf->jerk) /
- ((2 * Vi * x - Vi2 + 3 * x * x) / L2);
-#endif
-
- return x;
-}
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2016 Buildbotics LLC
- Copyright (c) 2013 - 2015 Alden S. Hart, Jr.
- Copyright (c) 2013 - 2015 Robert Giseburt
- All rights reserved.
-
- 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/>.
-
- The software is distributed in the hope that it will be useful, but
- WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
- Lesser General Public License for more details.
-
- You should have received a copy of the GNU Lesser General Public
- License along with the software. If not, see
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-#pragma once
-
-
-#include "machine.h" // used for gcode_state_t
-#include "buffer.h"
-#include "util.h"
-#include "config.h"
-
-#include <stdbool.h>
-
-
-// Most of these factors are the result of a lot of tweaking.
-// Change with caution.
-#define JERK_MULTIPLIER 1000000.0
-#define JERK_MATCH_PRECISION 1000.0 // jerk precision to be considered same
-
-#define NOM_SEGMENT_TIME (NOM_SEGMENT_USEC / MICROSECONDS_PER_MINUTE)
-#define MIN_SEGMENT_TIME (MIN_SEGMENT_USEC / MICROSECONDS_PER_MINUTE)
-
-#define MIN_SEGMENT_TIME_PLUS_MARGIN \
- ((MIN_SEGMENT_USEC + 1) / MICROSECONDS_PER_MINUTE)
-
-/// Error percentage for iteration convergence. As percent - 0.01 = 1%
-#define TRAPEZOID_ITERATION_ERROR_PERCENT 0.1
-
-/// Adaptive velocity tolerance term
-#define TRAPEZOID_VELOCITY_TOLERANCE (max(2, bf->entry_velocity / 100))
-
-/*** If the absolute value of the remaining deceleration length would be less
- * than this value then finish the deceleration in the current move. This is
- * used to avoid creating segements before or after the hold which are too
- * short to process correctly.
- */
-#define HOLD_DECELERATION_TOLERANCE 1 // In mm
-
-
-typedef enum {
- SECTION_HEAD, // acceleration
- SECTION_BODY, // cruise
- SECTION_TAIL, // deceleration
-} move_section_t;
-
-
-void mp_init();
-
-void mp_set_axis_position(int axis, float position);
-float mp_get_axis_position(int axis);
-void mp_set_position(const float p[]);
-void mp_set_plan_steps(bool plan_steps);
-
-void mp_flush_planner();
-void mp_kinematics(const float travel[], float steps[]);
-
-void mp_plan(mp_buffer_t *bf);
-void mp_replan_all();
-
-void mp_queue_push_nonstop(buffer_cb_t cb, uint32_t line);
-
-float mp_get_target_length(float Vi, float Vf, float jerk);
-float mp_get_target_velocity(float Vi, float L, const mp_buffer_t *bf);
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2016 Buildbotics LLC
- Copyright (c) 2010 - 2015 Alden S. Hart, Jr.
- Copyright (c) 2012 - 2015 Rob Giseburt
- All rights reserved.
-
- 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/>.
-
- The software is distributed in the hope that it will be useful, but
- WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
- Lesser General Public License for more details.
-
- You should have received a copy of the GNU Lesser General Public
- License along with the software. If not, see
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-#include "planner.h"
-#include "buffer.h"
-#include "stepper.h"
-#include "motor.h"
-#include "util.h"
-#include "report.h"
-#include "state.h"
-#include "config.h"
-
-#include <string.h>
-#include <stdbool.h>
-#include <math.h>
-#include <stdio.h>
-
-
-typedef struct {
- bool busy; // True if a move is running
- float position[AXES]; // Current move position
- float work_offset[AXES]; // Current move work offset
- float velocity; // Current move velocity
-
- int32_t line; // Current move GCode line number
- uint8_t tool; // Active tool
-
- float feed;
- feed_mode_t feed_mode;
- float feed_override;
- float spindle_override;
-
- plane_t plane;
- units_t units;
- coord_system_t coord_system;
- bool absolute_mode;
- path_mode_t path_mode;
- distance_mode_t distance_mode;
- distance_mode_t arc_distance_mode;
-
- float previous_error[MOTORS];
-} mp_runtime_t;
-
-static mp_runtime_t rt = {0};
-
-
-bool mp_runtime_is_busy() {return rt.busy;}
-void mp_runtime_set_busy(bool busy) {rt.busy = busy;}
-int32_t mp_runtime_get_line() {return rt.line;}
-void mp_runtime_set_line(int32_t line) {rt.line = line; report_request();}
-uint8_t mp_runtime_get_tool() {return rt.tool;}
-void mp_runtime_set_tool(uint8_t tool) {rt.tool = tool; report_request();}
-
-
-/// Returns current segment velocity
-float mp_runtime_get_velocity() {return rt.velocity;}
-
-
-void mp_runtime_set_velocity(float velocity) {
- rt.velocity = velocity;
- report_request();
-}
-
-
-/// Set encoder counts to the runtime position
-void mp_runtime_set_steps_from_position() {
- // Convert lengths to steps in floating point
- float steps[MOTORS];
- mp_kinematics(rt.position, steps);
-
- for (int motor = 0; motor < MOTORS; motor++)
- // Write steps to encoder register
- motor_set_encoder(motor, steps[motor]);
-}
-
-
-/* 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.
- *
- * Keeping track of position is complicated by the fact that moves
- * exist in several reference frames. The scheme to keep this
- * straight is:
- *
- * - mp_position - start and end position for planning
- * - rt.position - current position of runtime segment
- * - rt.steps.* - position in steps
- *
- * Note that position is set immediately when called and may not be
- * 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.
- */
-
-
-/// Set runtime position for a single axis
-void mp_runtime_set_axis_position(uint8_t axis, const float position) {
- rt.position[axis] = position;
- report_request();
-}
-
-
-/// Returns current axis position in machine coordinates
-float mp_runtime_get_axis_position(uint8_t axis) {return rt.position[axis];}
-float *mp_runtime_get_position() {return rt.position;}
-
-
-void mp_runtime_set_position(float position[]) {
- copy_vector(rt.position, position);
- report_request();
-}
-
-
-/// Returns axis position in work coordinates that were in effect at plan time
-float mp_runtime_get_work_position(uint8_t axis) {
- return rt.position[axis] - rt.work_offset[axis];
-}
-
-
-/// Set offsets
-void mp_runtime_set_work_offsets(float offset[]) {
- copy_vector(rt.work_offset, offset);
-}
-
-
-/// Segment runner
-stat_t mp_runtime_move_to_target(float target[], float time) {
- // Convert target position to steps.
- float steps[MOTORS];
- mp_kinematics(target, steps);
-
-#ifdef STEP_CORRECTION
- int32_t error[MOTORS];
- st_get_error(error);
-
- float travel[MOTORS];
- float new_length_sqr = 0;
- float old_length_sqr = 0;
-
- for (int motor = 0; motor < MOTORS; motor++) {
- travel[motor] = steps[motor] - motor_get_position(motor);
-
- if (fp_ZERO(travel[motor])) {
- motor[travel] = 0;
- motor[error] = 0;
- }
-
- error[motor] = 0.5 * (error[motor] - rt.previous_error[motor]);
- rt.previous_error[motor] = error[motor];
-
- if (error[motor] < -MAX_STEP_CORRECTION)
- error[motor] = -MAX_STEP_CORRECTION;
- else if (MAX_STEP_CORRECTION < error[motor])
- error[motor] = MAX_STEP_CORRECTION;
-
- old_length_sqr += square(travel[motor]);
- new_length_sqr += square(travel[motor] - error[motor]);
- }
-
- bool use_error = false;
- if (!fp_ZERO(new_length_sqr)) {
- float new_time = time * invsqrt(old_length_sqr / new_length_sqr);
-
- if (!isnan(new_time) && !isinf(new_time) &&
- EPSILON <= new_time && new_time <= MAX_SEGMENT_TIME) {
- time = new_time;
- use_error = true;
- }
- }
-
- if (!use_error) memset(error, 0, sizeof(error));
-
-#else
- const int32_t error[MOTORS] = {0};
-#endif
-
- // Call the stepper prep function
- RITORNO(st_prep_line(time, steps, error));
-
- // Update positions
- mp_runtime_set_position(target);
-
- return STAT_OK;
-}
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2016 Buildbotics LLC
- All rights reserved.
-
- 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/>.
-
- The software is distributed in the hope that it will be useful, but
- WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
- Lesser General Public License for more details.
-
- You should have received a copy of the GNU Lesser General Public
- License along with the software. If not, see
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-#pragma once
-
-#include "status.h"
-
-#include <stdbool.h>
-
-
-bool mp_runtime_is_busy();
-void mp_runtime_set_busy(bool busy);
-
-int32_t mp_runtime_get_line();
-void mp_runtime_set_line(int32_t line);
-
-uint8_t mp_runtime_get_tool();
-void mp_runtime_set_tool(uint8_t tool);
-
-float mp_runtime_get_velocity();
-void mp_runtime_set_velocity(float velocity);
-
-void mp_runtime_set_steps_from_position();
-
-void mp_runtime_set_axis_position(uint8_t axis, const float position);
-float mp_runtime_get_axis_position(uint8_t axis);
-
-float *mp_runtime_get_position();
-void mp_runtime_set_position(float position[]);
-
-float mp_runtime_get_work_position(uint8_t axis);
-void mp_runtime_set_work_offsets(float offset[]);
-
-stat_t mp_runtime_move_to_target(float target[], float time);
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2016 Buildbotics LLC
- Copyright (c) 2013 - 2015 Alden S. Hart, Jr.
- Copyright (c) 2013 - 2015 Robert Giseburt
- All rights reserved.
-
- 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/>.
-
- The software is distributed in the hope that it will be useful, but
- WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
- Lesser General Public License for more details.
-
- You should have received a copy of the GNU Lesser General Public
- License along with the software. If not, see
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-#include "state.h"
-#include "machine.h"
-#include "planner.h"
-#include "runtime.h"
-#include "buffer.h"
-#include "arc.h"
-#include "stepper.h"
-
-#include "report.h"
-
-#include <stdbool.h>
-
-
-typedef struct {
- mp_state_t state;
- mp_cycle_t cycle;
- mp_hold_reason_t hold_reason;
-
- bool hold_requested;
- bool flush_requested;
- bool start_requested;
- bool resume_requested;
- bool optional_pause_requested;
-} planner_state_t;
-
-
-static planner_state_t ps = {
- .flush_requested = true, // Start out flushing
-};
-
-
-PGM_P mp_get_state_pgmstr(mp_state_t state) {
- switch (state) {
- case STATE_READY: return PSTR("READY");
- case STATE_ESTOPPED: return PSTR("ESTOPPED");
- case STATE_RUNNING: return PSTR("RUNNING");
- case STATE_STOPPING: return PSTR("STOPPING");
- case STATE_HOLDING: return PSTR("HOLDING");
- }
-
- return PSTR("INVALID");
-}
-
-
-PGM_P mp_get_cycle_pgmstr(mp_cycle_t cycle) {
- switch (cycle) {
- case CYCLE_MACHINING: return PSTR("MACHINING");
- case CYCLE_HOMING: return PSTR("HOMING");
- case CYCLE_PROBING: return PSTR("PROBING");
- case CYCLE_CALIBRATING: return PSTR("CALIBRATING");
- case CYCLE_JOGGING: return PSTR("JOGGING");
- }
-
- return PSTR("INVALID");
-}
-
-
-PGM_P mp_get_hold_reason_pgmstr(mp_hold_reason_t reason) {
- switch (reason) {
- case HOLD_REASON_USER_PAUSE: return PSTR("USER");
- case HOLD_REASON_PROGRAM_PAUSE: return PSTR("PROGRAM");
- case HOLD_REASON_PROGRAM_END: return PSTR("END");
- case HOLD_REASON_PALLET_CHANGE: return PSTR("PALLET");
- case HOLD_REASON_TOOL_CHANGE: return PSTR("TOOL");
- }
-
- return PSTR("INVALID");
-}
-
-
-mp_state_t mp_get_state() {return ps.state;}
-mp_cycle_t mp_get_cycle() {return ps.cycle;}
-
-
-static void _set_state(mp_state_t state) {
- if (ps.state == state) return; // No change
- if (ps.state == STATE_ESTOPPED) return; // Can't leave EStop state
- if (state == STATE_READY) mp_runtime_set_line(0);
- ps.state = state;
- report_request();
-}
-
-
-void mp_set_cycle(mp_cycle_t cycle) {
- if (ps.cycle == cycle) return; // No change
-
- if (ps.state != STATE_READY && cycle != CYCLE_MACHINING) {
- STATUS_ERROR(STAT_INTERNAL_ERROR, "Cannot transition to %S while %S",
- mp_get_cycle_pgmstr(cycle),
- mp_get_state_pgmstr(ps.state));
- return;
- }
-
- if (ps.cycle != CYCLE_MACHINING && cycle != CYCLE_MACHINING) {
- STATUS_ERROR(STAT_INTERNAL_ERROR,
- "Cannot transition to cycle %S while in %S",
- mp_get_cycle_pgmstr(cycle),
- mp_get_cycle_pgmstr(ps.cycle));
- return;
- }
-
- ps.cycle = cycle;
- report_request();
-}
-
-
-mp_hold_reason_t mp_get_hold_reason() {return ps.hold_reason;}
-
-
-void mp_set_hold_reason(mp_hold_reason_t reason) {
- if (ps.hold_reason == reason) return; // No change
- ps.hold_reason = reason;
- report_request();
-}
-
-
-bool mp_is_flushing() {return ps.flush_requested && !ps.resume_requested;}
-bool mp_is_resuming() {return ps.resume_requested;}
-
-
-bool mp_is_quiescent() {
- return (mp_get_state() == STATE_READY || mp_get_state() == STATE_HOLDING) &&
- !st_is_busy() && !mp_runtime_is_busy();
-}
-
-
-void mp_state_optional_pause() {
- if (ps.optional_pause_requested) {
- mp_set_hold_reason(HOLD_REASON_USER_PAUSE);
- mp_state_holding();
- }
-}
-
-
-void mp_state_holding() {
- _set_state(STATE_HOLDING);
- mp_set_plan_steps(false);
-}
-
-
-void mp_state_running() {
- if (mp_get_state() == STATE_READY) _set_state(STATE_RUNNING);
-}
-
-
-void mp_state_idle() {
- if (mp_get_state() == STATE_RUNNING) _set_state(STATE_READY);
-}
-
-
-void mp_state_estop() {_set_state(STATE_ESTOPPED);}
-
-
-void mp_request_hold() {ps.hold_requested = true;}
-void mp_request_start() {ps.start_requested = true;}
-void mp_request_flush() {ps.flush_requested = true;}
-void mp_request_resume() {if (ps.flush_requested) ps.resume_requested = true;}
-void mp_request_optional_pause() {ps.optional_pause_requested = true;}
-
-
-void mp_request_step() {
- mp_set_plan_steps(true);
- ps.start_requested = true;
-}
-
-
-/*** Feedholds, queue flushes and starts are all related. Request functions
- * set flags. The callback interprets the flags according to these rules:
- *
- * A hold request received:
- * - during motion is honored
- * - during a feedhold is ignored and reset
- * - when already stopped is ignored and reset
- *
- * A flush request received:
- * - during motion is ignored but not reset
- * - during a feedhold is deferred until the feedhold enters HOLDING state.
- * I.e. until deceleration is complete.
- * - when stopped or holding and the planner is not busy, is honored
- *
- * A start request received:
- * - during motion is ignored and reset
- * - during a feedhold is deferred until the feedhold enters HOLDING state.
- * I.e. until deceleration is complete. If a queue flush request is also
- * present the queue flush is done first
- * - when stopped is honored and starts to run anything in the planner queue
- */
-void mp_state_callback() {
- if (ps.hold_requested || ps.flush_requested) {
- ps.hold_requested = false;
- mp_set_hold_reason(HOLD_REASON_USER_PAUSE);
-
- if (mp_get_state() == STATE_RUNNING) _set_state(STATE_STOPPING);
- }
-
- // Only flush queue when idle or holding.
- if (ps.flush_requested && mp_is_quiescent()) {
- mach_abort_arc();
-
- if (!mp_queue_is_empty()) {
- mp_flush_planner();
-
- // NOTE The following uses low-level mp calls for absolute position.
- // Reset to actual machine position. Otherwise machine is set to the
- // position of the last queued move.
- for (int axis = 0; axis < AXES; axis++)
- mach_set_axis_position(axis, mp_runtime_get_axis_position(axis));
- }
-
- // Resume
- if (ps.resume_requested) {
- ps.flush_requested = ps.resume_requested = false;
- _set_state(STATE_READY);
- }
- }
-
- // Don't start while flushing or stopping
- if (ps.start_requested && !ps.flush_requested &&
- mp_get_state() != STATE_STOPPING) {
- ps.start_requested = false;
- ps.optional_pause_requested = false;
-
- if (mp_get_state() == STATE_HOLDING) {
- // Check if any moves are buffered
- if (!mp_queue_is_empty()) {
- // Always replan when coming out of a hold
- mp_replan_all();
- _set_state(STATE_RUNNING);
-
- } else _set_state(STATE_READY);
- }
- }
-}
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2016 Buildbotics LLC
- Copyright (c) 2013 - 2015 Alden S. Hart, Jr.
- Copyright (c) 2013 - 2015 Robert Giseburt
- All rights reserved.
-
- 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/>.
-
- The software is distributed in the hope that it will be useful, but
- WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
- Lesser General Public License for more details.
-
- You should have received a copy of the GNU Lesser General Public
- License along with the software. If not, see
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-#pragma once
-
-#include <avr/pgmspace.h>
-
-#include <stdbool.h>
-
-
-typedef enum {
- STATE_READY,
- STATE_ESTOPPED,
- STATE_RUNNING,
- STATE_STOPPING,
- STATE_HOLDING,
-} mp_state_t;
-
-
-typedef enum {
- CYCLE_MACHINING,
- CYCLE_HOMING,
- CYCLE_PROBING,
- CYCLE_CALIBRATING,
- CYCLE_JOGGING,
-} mp_cycle_t;
-
-
-typedef enum {
- HOLD_REASON_USER_PAUSE,
- HOLD_REASON_PROGRAM_PAUSE,
- HOLD_REASON_PROGRAM_END,
- HOLD_REASON_PALLET_CHANGE,
- HOLD_REASON_TOOL_CHANGE,
-} mp_hold_reason_t;
-
-
-PGM_P mp_get_state_pgmstr(mp_state_t state);
-PGM_P mp_get_cycle_pgmstr(mp_cycle_t cycle);
-PGM_P mp_get_hold_reason_pgmstr(mp_hold_reason_t reason);
-
-mp_state_t mp_get_state();
-
-mp_cycle_t mp_get_cycle();
-void mp_set_cycle(mp_cycle_t cycle);
-
-mp_hold_reason_t mp_get_hold_reason();
-void mp_set_hold_reason(mp_hold_reason_t reason);
-
-bool mp_is_flushing();
-bool mp_is_resuming();
-bool mp_is_quiescent();
-
-void mp_state_optional_pause();
-void mp_state_holding();
-void mp_state_running();
-void mp_state_idle();
-void mp_state_estop();
-
-void mp_request_hold();
-void mp_request_start();
-void mp_request_flush();
-void mp_request_resume();
-void mp_request_optional_pause();
-void mp_request_step();
-
-void mp_state_callback();
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2016 Buildbotics LLC
- Copyright (c) 2010 - 2015 Alden S Hart, Jr.
- All rights reserved.
-
- 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/>.
-
- The software is distributed in the hope that it will be useful, but
- WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
- Lesser General Public License for more details.
-
- You should have received a copy of the GNU Lesser General Public
- License along with the software. If not, see
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-#include "machine.h"
-#include "axis.h"
-#include "spindle.h"
-#include "switch.h"
-#include "util.h"
-
-#include "plan/planner.h"
-#include "plan/runtime.h"
-#include "plan/state.h"
-
-#include <avr/pgmspace.h>
-
-#include <math.h>
-#include <string.h>
-#include <stdbool.h>
-#include <stdio.h>
-
-
-#define MINIMUM_PROBE_TRAVEL 0.254
-
-
-typedef enum {
- PROBE_FAILED, // probe reached endpoint without triggering
- PROBE_SUCCEEDED, // probe was triggered, pb.results has position
- PROBE_WAITING, // probe is waiting to be started
-} probe_state_t;
-
-
-typedef struct {
- probe_state_t state;
- float results[AXES]; // probing results
-
- void (*func)(); // binding for callback function state machine
-
- // state saved from gcode model
- uint8_t saved_distance_mode; // G90, G91 global setting
- uint8_t saved_coord_system; // G54 - G59 setting
-
- // probe destination
- float start_position[AXES];
- float target[AXES];
- bool flags[AXES];
-} probing_t;
-
-
-static probing_t pb = {0};
-
-
-/* Note: When coding a cycle (like this one) you get to perform one
- * queued move per entry into the continuation, then you must exit.
- *
- * Another Note: When coding a cycle (like this one) you must wait
- * until the last move has actually been queued (or has finished)
- * before declaring the cycle to be done. Otherwise there is a nasty
- * race condition that will accept the next command before the position
- * of the final move has been recorded in the Gcode model.
- */
-
-
-static void _probe_restore_settings() {
- // we should be stopped now, but in case of switch closure
- mp_flush_planner();
-
- // restore coordinate system and distance mode
- mach_set_coord_system(pb.saved_coord_system);
- mach_set_distance_mode(pb.saved_distance_mode);
-
- // update the model with actual position
- mach_set_motion_mode(MOTION_MODE_CANCEL_MOTION_MODE);
-
- mp_set_cycle(CYCLE_MACHINING); // Default cycle
-}
-
-
-static void _probing_error_exit(stat_t status) {
- _probe_restore_settings(); // clean up and exit
- status_error(status);
-}
-
-
-static void _probing_finish() {
- bool closed = switch_is_active(SW_PROBE);
- pb.state = closed ? PROBE_SUCCEEDED : PROBE_FAILED;
-
- for (int axis = 0; axis < AXES; axis++) {
- // if we got here because of a feed hold keep the model position correct
- mach_set_axis_position(axis, mp_runtime_get_work_position(axis));
-
- // store the probe results
- pb.results[axis] = mach_get_axis_position(axis);
- }
-
- _probe_restore_settings();
-}
-
-
-static void _probing_start() {
- // initial probe state, don't probe if we're already contacted!
- bool closed = switch_is_active(SW_PROBE);
-
- if (!closed) {
- stat_t status = mach_feed(pb.target, pb.flags);
- if (status) return _probing_error_exit(status);
- }
-
- pb.func = _probing_finish;
-}
-
-
-/* G38.2 homing cycle using limit switches
- *
- * These initializations are required before starting the probing cycle.
- * They must be done after the planner has exhasted all current CYCLE moves as
- * they affect the runtime (specifically the switch modes). Side effects would
- * include limit switches initiating probe actions instead of just killing
- * movement
- */
-static void _probing_init() {
- // NOTE: it is *not* an error condition for the probe not to trigger.
- // it is an error for the limit or homing switches to fire, or for some other
- // configuration error.
- pb.state = PROBE_FAILED;
- mp_set_cycle(CYCLE_PROBING);
-
- // initialize the axes
- for (int axis = 0; axis < AXES; axis++)
- pb.start_position[axis] = mach_get_axis_position(axis);
-
- // error if the probe target is too close to the current position
- if (axis_get_vector_length(pb.start_position, pb.target) <
- MINIMUM_PROBE_TRAVEL)
- _probing_error_exit(STAT_PROBE_INVALID_DEST);
-
- // error if the probe target requires a move along the A/B/C axes
- for (int axis = 0; axis < AXES; axis++)
- if (fp_NE(pb.start_position[axis], pb.target[axis]))
- _probing_error_exit(STAT_MOVE_DURING_PROBE);
-
- // probe in absolute machine coords
- pb.saved_coord_system = mach_get_coord_system();
- pb.saved_distance_mode = mach_get_distance_mode();
- mach_set_distance_mode(ABSOLUTE_MODE);
- mach_set_coord_system(ABSOLUTE_COORDS);
-
- mach_set_spindle_mode(SPINDLE_OFF);
-
- // start the move
- pb.func = _probing_start;
-}
-
-
-bool mach_is_probing() {
- return mp_get_cycle() == CYCLE_PROBING || pb.state == PROBE_WAITING;
-}
-
-
-/// G38.2 homing cycle using limit switches
-stat_t mach_probe(float target[], bool flags[]) {
- // trap zero feed rate condition
- if (mach_get_feed_mode() != INVERSE_TIME_MODE &&
- fp_ZERO(mach_get_feed_rate()))
- return STAT_GCODE_FEEDRATE_NOT_SPECIFIED;
-
- // trap no axes specified
- if (!flags[AXIS_X] && !flags[AXIS_Y] && !flags[AXIS_Z])
- return STAT_GCODE_AXIS_IS_MISSING;
-
- // set probe move endpoint
- copy_vector(pb.target, target); // set probe move endpoint
- copy_vector(pb.flags, flags); // set axes involved on the move
- clear_vector(pb.results); // clear the old probe position.
- // NOTE: relying on pb.results will not detect a probe to (0, 0, 0).
-
- // wait until planner queue empties before completing initialization
- pb.state = PROBE_WAITING;
- pb.func = _probing_init; // bind probing initialization function
-
- return STAT_OK;
-}
-
-
-/// main loop callback for running the homing cycle
-void mach_probe_callback() {
- // sync to planner move ends
- if (!mach_is_probing() || mp_get_state() != STATE_READY) return;
-
- pb.func(); // execute the current homing move
-}
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2016 Buildbotics LLC
- All rights reserved.
-
- 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/>.
-
- The software is distributed in the hope that it will be useful, but
- WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
- Lesser General Public License for more details.
-
- You should have received a copy of the GNU Lesser General Public
- License along with the software. If not, see
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-#pragma once
-
-#include "status.h"
-
-#include <stdbool.h>
-
-
-bool mach_is_probing();
-stat_t mach_probe(float target[], bool flags[]);
-void mach_probe_callback();
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2016 Buildbotics LLC
- All rights reserved.
-
- 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/>.
-
- The software is distributed in the hope that it will be useful, but
- WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
- Lesser General Public License for more details.
-
- You should have received a copy of the GNU Lesser General Public
- License along with the software. If not, see
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-
-#include "pwm_spindle.h"
-
-#include "config.h"
-
-
-typedef struct {
- uint16_t freq; // base frequency for PWM driver, in Hz
- float min_rpm;
- float max_rpm;
- float min_duty;
- float max_duty;
- bool reverse;
- bool enable_invert;
- bool estop;
-} pwm_spindle_t;
-
-
-static pwm_spindle_t spindle = {
- .freq = SPINDLE_PWM_FREQUENCY,
- .min_rpm = SPINDLE_MIN_RPM,
- .max_rpm = SPINDLE_MAX_RPM,
- .min_duty = SPINDLE_MIN_DUTY,
- .max_duty = SPINDLE_MAX_DUTY,
- .reverse = SPINDLE_REVERSE,
- .enable_invert = false,
- .estop = false,
-};
-
-
-static void _spindle_set_pwm(spindle_mode_t mode, float speed) {
- if (mode == SPINDLE_OFF || speed < spindle.min_rpm || spindle.estop) {
- TIMER_PWM.CTRLA = 0;
- return;
- }
-
- // Clamp speed
- if (spindle.max_rpm < speed) speed = spindle.max_rpm;
-
- // Set clock period and optimal prescaler value
- float prescale = F_CPU / 65536.0 / spindle.freq;
- if (prescale <= 1) {
- TIMER_PWM.PER = F_CPU / spindle.freq;
- TIMER_PWM.CTRLA = TC_CLKSEL_DIV1_gc;
-
- } else if (prescale <= 2) {
- TIMER_PWM.PER = F_CPU / 2 / spindle.freq;
- TIMER_PWM.CTRLA = TC_CLKSEL_DIV2_gc;
-
- } else if (prescale <= 4) {
- TIMER_PWM.PER = F_CPU / 4 / spindle.freq;
- TIMER_PWM.CTRLA = TC_CLKSEL_DIV4_gc;
-
- } else if (prescale <= 8) {
- TIMER_PWM.PER = F_CPU / 8 / spindle.freq;
- TIMER_PWM.CTRLA = TC_CLKSEL_DIV8_gc;
-
- } else if (prescale <= 64) {
- TIMER_PWM.PER = F_CPU / 64 / spindle.freq;
- TIMER_PWM.CTRLA = TC_CLKSEL_DIV64_gc;
-
- } else TIMER_PWM.CTRLA = 0;
-
- // Map RPM to duty cycle
- float duty = (speed - spindle.min_rpm) / (spindle.max_rpm - spindle.min_rpm) *
- (spindle.max_duty - spindle.min_duty) + spindle.min_duty;
-
- TIMER_PWM.CCB = TIMER_PWM.PER * duty;
-}
-
-
-static void _spindle_set_enable(bool enable) {
- if (enable ^ spindle.enable_invert)
- PORT(SPIN_ENABLE_PIN)->OUTSET = BM(SPIN_ENABLE_PIN);
- else PORT(SPIN_ENABLE_PIN)->OUTCLR = BM(SPIN_ENABLE_PIN);
-}
-
-
-static void _spindle_set_dir(bool forward) {
- if (forward ^ spindle.reverse) PORT(SPIN_DIR_PIN)->OUTCLR = BM(SPIN_DIR_PIN);
- else PORT(SPIN_DIR_PIN)->OUTSET = BM(SPIN_DIR_PIN);
-}
-
-
-void pwm_spindle_init() {
- // Configure IO
- PORT(SPIN_PWM_PIN)->DIRSET = BM(SPIN_PWM_PIN); // PWM Output
- _spindle_set_dir(true);
- PORT(SPIN_DIR_PIN)->DIRSET = BM(SPIN_DIR_PIN); // Dir Output
- _spindle_set_enable(false);
- PORT(SPIN_ENABLE_PIN)->DIRSET = BM(SPIN_ENABLE_PIN); // Enable output
-
- // Configure clock
- TIMER_PWM.CTRLB = TC1_CCBEN_bm | TC_WGMODE_SINGLESLOPE_gc;
-}
-
-
-void pwm_spindle_set(spindle_mode_t mode, float speed) {
- _spindle_set_dir(mode == SPINDLE_CW);
- _spindle_set_pwm(mode, speed);
- _spindle_set_enable(mode != SPINDLE_OFF && TIMER_PWM.CTRLA);
-}
-
-
-void pwm_spindle_estop() {
- spindle.estop = true;
- _spindle_set_pwm(SPINDLE_OFF, 0);
-}
-
-
-// TODO these need more effort and should work with the huanyang spindle too
-float get_max_spin(int index) {return spindle.max_rpm;}
-void set_max_spin(int axis, float value) {spindle.max_rpm = value;}
-float get_min_spin(int index) {return spindle.min_rpm;}
-void set_min_spin(int axis, float value) {spindle.min_rpm = value;}
-float get_spin_min_pulse(int index) {return spindle.min_duty;}
-void set_spin_min_pulse(int axis, float value) {spindle.min_duty = value;}
-float get_spin_max_pulse(int index) {return spindle.max_duty;}
-void set_spin_max_pulse(int axis, float value) {spindle.max_duty = value;}
-uint8_t get_spin_reverse(int index) {return spindle.reverse;}
-void set_spin_reverse(int axis, uint8_t value) {spindle.reverse = value;}
-float get_spin_up(int index) {return 0;}
-void set_spin_up(int axis, float value) {}
-float get_spin_down(int index) {return 0;}
-void set_spin_down(int axis, float value) {}
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2016 Buildbotics LLC
- All rights reserved.
-
- 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/>.
-
- The software is distributed in the hope that it will be useful, but
- WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
- Lesser General Public License for more details.
-
- You should have received a copy of the GNU Lesser General Public
- License along with the software. If not, see
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-#pragma once
-
-#include "machine.h"
-
-
-void pwm_spindle_init();
-void pwm_spindle_set(spindle_mode_t mode, float speed);
-void pwm_spindle_estop();
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2016 Buildbotics LLC
- All rights reserved.
-
- 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/>.
-
- The software is distributed in the hope that it will be useful, but
- WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
- Lesser General Public License for more details.
-
- You should have received a copy of the GNU Lesser General Public
- License along with the software. If not, see
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-#include "report.h"
-#include "config.h"
-#include "usart.h"
-#include "rtc.h"
-#include "vars.h"
-
-#include <avr/pgmspace.h>
-
-#include <stdio.h>
-#include <stdbool.h>
-
-
-static bool report_requested = false;
-static bool report_full = false;
-static uint32_t last_report = 0;
-
-
-void report_request() {
- report_requested = true;
-}
-
-
-void report_request_full() {
- report_requested = report_full = true;
-}
-
-
-void report_callback() {
- if (usart_tx_full()) return; // Wait for buffer space
-
- if (report_requested && usart_tx_empty()) {
- uint32_t now = rtc_get_time();
- if (now - last_report < 100) return;
- last_report = now;
-
- vars_report(report_full);
- report_requested = report_full = false;
- }
-}
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2016 Buildbotics LLC
- All rights reserved.
-
- 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/>.
-
- The software is distributed in the hope that it will be useful, but
- WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
- Lesser General Public License for more details.
-
- You should have received a copy of the GNU Lesser General Public
- License along with the software. If not, see
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-#pragma once
-
-
-#include "status.h"
-
-void report_request();
-void report_request_full();
-void report_callback();
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2016 Buildbotics LLC
- All rights reserved.
-
- 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/>.
-
- The software is distributed in the hope that it will be useful, but
- WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
- Lesser General Public License for more details.
-
- You should have received a copy of the GNU Lesser General Public
- License along with the software. If not, see
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-/* This file defines an X-Macro ring buffer. It can be used like this:
- *
- * #define RING_BUF_NAME tx_buf
- * #define RING_BUF_SIZE 256
- * #include "ringbuf.def"
- *
- * This will define the following functions:
- *
- * void <name>_init();
- * int <name>_empty();
- * int <name>_full();
- * <type> <name>_peek();
- * void <name>_pop();
- * void <name>_push(<type> data);
- *
- * Where <name> is defined by RING_BUF_NAME and <type> by RING_BUF_TYPE.
- * RING_BUF_SIZE defines the length of the ring buffer and must be a power of 2.
- *
- * The data type and index type both default to uint8_t but can be changed by
- * defining RING_BUF_TYPE and RING_BUF_INDEX_TYPE respectively.
- *
- * By default these functions are declared static inline but this can be changed
- * by defining RING_BUF_FUNC.
- */
-
-#include <stdint.h>
-
-#ifndef RING_BUF_NAME
-#error Must define RING_BUF_NAME
-#endif
-
-#ifndef RING_BUF_SIZE
-#error Must define RING_BUF_SIZE
-#endif
-
-#ifndef RING_BUF_TYPE
-#define RING_BUF_TYPE uint8_t
-#endif
-
-#ifndef RING_BUF_INDEX_TYPE
-#define RING_BUF_INDEX_TYPE uint8_t
-#endif
-
-#ifndef RING_BUF_FUNC
-#define RING_BUF_FUNC static inline
-#endif
-
-#define RING_BUF_MASK (RING_BUF_SIZE - 1)
-#if (RING_BUF_SIZE & RING_BUF_MASK)
-#error RING_BUF_SIZE is not a power of 2
-#endif
-
-#ifndef CONCAT
-#define _CONCAT(prefix, name) prefix##name
-#define CONCAT(prefix, name) _CONCAT(prefix, name)
-#endif
-
-#define RING_BUF_STRUCT CONCAT(RING_BUF_NAME, _ring_buf_t)
-#define RING_BUF CONCAT(RING_BUF_NAME, _ring_buf)
-
-typedef struct {
- RING_BUF_TYPE buf[RING_BUF_SIZE];
- volatile RING_BUF_INDEX_TYPE head;
- volatile RING_BUF_INDEX_TYPE tail;
-} RING_BUF_STRUCT;
-
-static RING_BUF_STRUCT RING_BUF;
-
-
-RING_BUF_FUNC void CONCAT(RING_BUF_NAME, _init)() {
- RING_BUF.head = RING_BUF.tail = 0;
-}
-
-
-#define RING_BUF_INC CONCAT(RING_BUF_NAME, _inc)
-RING_BUF_FUNC RING_BUF_INDEX_TYPE RING_BUF_INC(RING_BUF_INDEX_TYPE x) {
- return (x + 1) & RING_BUF_MASK;
-}
-
-
-RING_BUF_FUNC int CONCAT(RING_BUF_NAME, _empty)() {
- return RING_BUF.head == RING_BUF.tail;
-}
-
-
-RING_BUF_FUNC int CONCAT(RING_BUF_NAME, _full)() {
- return RING_BUF.head == RING_BUF_INC(RING_BUF.tail);
-}
-
-
-RING_BUF_FUNC int CONCAT(RING_BUF_NAME, _fill)() {
- return (RING_BUF.tail - RING_BUF.head) & RING_BUF_MASK;
-}
-
-
-RING_BUF_FUNC int CONCAT(RING_BUF_NAME, _space)() {
- return RING_BUF_SIZE - CONCAT(RING_BUF_NAME, _fill)();
-}
-
-
-RING_BUF_FUNC RING_BUF_TYPE CONCAT(RING_BUF_NAME, _peek)() {
- return RING_BUF.buf[RING_BUF.head];
-}
-
-
-RING_BUF_FUNC RING_BUF_TYPE CONCAT(RING_BUF_NAME, _get)(int offset) {
- return RING_BUF.buf[(RING_BUF.head + offset) & RING_BUF_MASK];
-}
-
-
-RING_BUF_FUNC void CONCAT(RING_BUF_NAME, _pop)() {
- RING_BUF.head = RING_BUF_INC(RING_BUF.head);
-}
-
-
-RING_BUF_FUNC void CONCAT(RING_BUF_NAME, _push)(RING_BUF_TYPE data) {
- RING_BUF.buf[RING_BUF.tail] = data;
- RING_BUF.tail = RING_BUF_INC(RING_BUF.tail);
-}
-
-
-#undef RING_BUF
-#undef RING_BUF_STRUCT
-#undef RING_BUF_INC
-#undef RING_BUF_MASK
-
-#undef RING_BUF_NAME
-#undef RING_BUF_SIZE
-#undef RING_BUF_TYPE
-#undef RING_BUF_INDEX_TYPE
-#undef RING_BUF_FUNC
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2016 Buildbotics LLC
- Copyright (c) 2010 - 2013 Alden S. Hart Jr.
- All rights reserved.
-
- 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/>.
-
- The software is distributed in the hope that it will be useful, but
- WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
- Lesser General Public License for more details.
-
- You should have received a copy of the GNU Lesser General Public
- License along with the software. If not, see
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-#include "rtc.h"
-
-#include "switch.h"
-#include "huanyang.h"
-#include "motor.h"
-
-#include <avr/io.h>
-#include <avr/interrupt.h>
-
-#include <string.h>
-
-
-static uint32_t ticks;
-
-
-ISR(RTC_OVF_vect) {
- ticks++;
-
- switch_rtc_callback();
- huanyang_rtc_callback();
- if (!(ticks & 255)) motor_rtc_callback();
-}
-
-
-/// Initialize and start the clock
-/// This routine follows the code in app note AVR1314.
-void rtc_init() {
- ticks = 0;
-
- OSC.CTRL |= OSC_RC32KEN_bm; // enable internal 32kHz.
- while (!(OSC.STATUS & OSC_RC32KRDY_bm)); // 32kHz osc stabilize
- while (RTC.STATUS & RTC_SYNCBUSY_bm); // wait RTC not busy
-
- CLK.RTCCTRL = CLK_RTCSRC_RCOSC32_gc | CLK_RTCEN_bm; // 32kHz clock as RTC src
- while (RTC.STATUS & RTC_SYNCBUSY_bm); // wait RTC not busy
-
- // the following must be in this order or it doesn't work
- RTC.PER = 33; // overflow period ~1ms
- RTC.INTCTRL = RTC_OVFINTLVL_LO_gc; // overflow LO interrupt
- RTC.CTRL = RTC_PRESCALER_DIV1_gc; // no prescale
-}
-
-
-uint32_t rtc_get_time() {return ticks;}
-
-
-bool rtc_expired(uint32_t t) {
- return 0 <= (int32_t)(ticks - t);
-}
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2016 Buildbotics LLC
- Copyright (c) 2010 - 2013 Alden S. Hart Jr.
- All rights reserved.
-
- 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/>.
-
- The software is distributed in the hope that it will be useful, but
- WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
- Lesser General Public License for more details.
-
- You should have received a copy of the GNU Lesser General Public
- License along with the software. If not, see
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-#pragma once
-
-
-#include <stdint.h>
-#include <stdbool.h>
-
-void rtc_init();
-uint32_t rtc_get_time();
-int32_t rtc_diff(uint32_t t);
-bool rtc_expired(uint32_t t);
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2016 Buildbotics LLC
- Copyright (c) 2010 - 2015 Alden S. Hart, Jr.
- All rights reserved.
-
- 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/>.
-
- The software is distributed in the hope that it will be useful, but
- WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
- Lesser General Public License for more details.
-
- You should have received a copy of the GNU Lesser General Public
- License along with the software. If not, see
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-#include "spindle.h"
-
-#include "config.h"
-#include "pwm_spindle.h"
-#include "huanyang.h"
-
-
-typedef enum {
- SPINDLE_TYPE_PWM,
- SPINDLE_TYPE_HUANYANG,
-} spindle_type_t;
-
-
-typedef struct {
- spindle_type_t type;
- spindle_mode_t mode;
- float speed;
-} spindle_t;
-
-
-static spindle_t spindle = {.type = SPINDLE_TYPE};
-
-
-void spindle_init() {
- pwm_spindle_init();
- huanyang_init();
-}
-
-
-void _spindle_set(spindle_mode_t mode, float speed) {
- spindle.mode = mode;
- spindle.speed = speed;
-
- switch (spindle.type) {
- case SPINDLE_TYPE_PWM: pwm_spindle_set(mode, speed); break;
- case SPINDLE_TYPE_HUANYANG: huanyang_set(mode, speed); break;
- }
-}
-
-
-void spindle_set_mode(spindle_mode_t mode) {
- spindle.mode = mode;
-
- switch (spindle.type) {
- case SPINDLE_TYPE_PWM: pwm_spindle_set(mode, spindle.speed); break;
- case SPINDLE_TYPE_HUANYANG: huanyang_set(mode, spindle.speed); break;
- }
-}
-
-
-void spindle_set_speed(float speed) {
- spindle.speed = speed;
-
- switch (spindle.type) {
- case SPINDLE_TYPE_PWM: pwm_spindle_set(spindle.mode, speed); break;
- case SPINDLE_TYPE_HUANYANG: huanyang_set(spindle.mode, speed); break;
- }
-}
-
-
-spindle_mode_t spindle_get_mode() {return spindle.mode;}
-float spindle_get_speed() {return spindle.speed;}
-
-
-void spindle_estop() {
- switch (spindle.type) {
- case SPINDLE_TYPE_PWM: pwm_spindle_estop(); break;
- case SPINDLE_TYPE_HUANYANG: huanyang_estop(); break;
- }
-}
-
-
-uint8_t get_spindle_type() {return spindle.type;}
-
-
-void set_spindle_type(uint8_t value) {
- if (value != spindle.type) {
- spindle_mode_t mode = spindle.mode;
- float speed = spindle.speed;
-
- _spindle_set(SPINDLE_OFF, 0);
- spindle.type = value;
- _spindle_set(mode, speed);
- }
-}
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2016 Buildbotics LLC
- Copyright (c) 2010 - 2015 Alden S. Hart, Jr.
- All rights reserved.
-
- 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/>.
-
- The software is distributed in the hope that it will be useful, but
- WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
- Lesser General Public License for more details.
-
- You should have received a copy of the GNU Lesser General Public
- License along with the software. If not, see
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-#pragma once
-
-#include "machine.h"
-
-
-void spindle_init();
-void spindle_set_mode(spindle_mode_t mode);
-void spindle_set_speed(float speed);
-spindle_mode_t spindle_get_mode();
-float spindle_get_speed();
-void spindle_estop();
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2016 Buildbotics LLC
- All rights reserved.
-
- 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/>.
-
- The software is distributed in the hope that it will be useful, but
- WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
- Lesser General Public License for more details.
-
- You should have received a copy of the GNU Lesser General Public
- License along with the software. If not, see
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-#include "status.h"
-#include "estop.h"
-#include "usart.h"
-
-#include <stdio.h>
-#include <stdarg.h>
-
-
-stat_t status_code; // allocate a variable for the RITORNO macro
-
-#define STAT_MSG(NAME, TEXT) static const char stat_##NAME[] PROGMEM = TEXT;
-#include "messages.def"
-#undef STAT_MSG
-
-static const char *const stat_msg[] PROGMEM = {
-#define STAT_MSG(NAME, TEXT) stat_##NAME,
-#include "messages.def"
-#undef STAT_MSG
-};
-
-
-const char *status_to_pgmstr(stat_t code) {
- return pgm_read_ptr(&stat_msg[code]);
-}
-
-
-const char *status_level_pgmstr(status_level_t level) {
- switch (level) {
- case STAT_LEVEL_INFO: return PSTR("info");
- case STAT_LEVEL_DEBUG: return PSTR("debug");
- case STAT_LEVEL_WARNING: return PSTR("warning");
- default: return PSTR("error");
- }
-}
-
-
-stat_t status_error(stat_t code) {
- return status_message_P(0, STAT_LEVEL_ERROR, code, 0);
-}
-
-
-stat_t status_message_P(const char *location, status_level_t level,
- stat_t code, const char *msg, ...) {
- va_list args;
-
- // Type
- printf_P(PSTR("\n{\"level\":\"%S\",\"msg\":\""), status_level_pgmstr(level));
-
- // Message
- if (msg) {
- va_start(args, msg);
- vfprintf_P(stdout, msg, args);
- va_end(args);
-
- } else printf_P(status_to_pgmstr(code));
-
- putchar('"');
-
- // Code
- if (code) printf_P(PSTR(", \"code\": %d"), code);
-
- // Location
- if (location) printf_P(PSTR(", \"where\": \"%S\""), location);
-
- putchar('}');
- putchar('\n');
-
- return code;
-}
-
-
-void status_help() {
- putchar('{');
-
- for (int i = 0; i < STAT_MAX; i++) {
- if (i) putchar(',');
- putchar('\n');
- printf_P(PSTR(" \"%d\": \"%S\""), i, status_to_pgmstr(i));
- }
-
- putchar('\n');
- putchar('}');
- putchar('\n');
-}
-
-
-/// Alarm state; send an exception report and stop processing input
-stat_t status_alarm(const char *location, stat_t code) {
- status_message_P(location, STAT_LEVEL_ERROR, code, 0);
- estop_trigger(code);
- while (!usart_tx_empty()) continue;
- return code;
-}
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2016 Buildbotics LLC
- All rights reserved.
-
- 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/>.
-
- The software is distributed in the hope that it will be useful, but
- WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
- Lesser General Public License for more details.
-
- You should have received a copy of the GNU Lesser General Public
- License along with the software. If not, see
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-#pragma once
-
-#include <avr/pgmspace.h>
-
-
-// RITORNO is a handy way to provide exception returns
-// It returns only if an error occurred. (ritorno is Italian for return)
-#define RITORNO(a) if ((status_code = a) != STAT_OK) {return status_code;}
-
-typedef enum {
-#define STAT_MSG(NAME, TEXT) STAT_##NAME,
-#include "messages.def"
-#undef STAT_MSG
-
- STAT_DO_NOT_EXCEED = 255 // Do not exceed 255
-} stat_t;
-
-
-typedef enum {
- STAT_LEVEL_INFO,
- STAT_LEVEL_DEBUG,
- STAT_LEVEL_WARNING,
- STAT_LEVEL_ERROR,
-} status_level_t;
-
-
-extern stat_t status_code;
-
-const char *status_to_pgmstr(stat_t code);
-const char *status_level_pgmstr(status_level_t level);
-stat_t status_error(stat_t code);
-stat_t status_message_P(const char *location, status_level_t level,
- stat_t code, const char *msg, ...);
-void status_help();
-
-/// Enter alarm state. returns same status code
-stat_t status_alarm(const char *location, stat_t status);
-
-#define TO_STRING(x) _TO_STRING(x)
-#define _TO_STRING(x) #x
-
-#define STATUS_LOCATION PSTR(__FILE__ ":" TO_STRING(__LINE__))
-
-#define STATUS_MESSAGE(LEVEL, CODE, MSG, ...) \
- status_message_P(STATUS_LOCATION, LEVEL, CODE, PSTR(MSG), ##__VA_ARGS__)
-
-#define STATUS_INFO(MSG, ...) \
- STATUS_MESSAGE(STAT_LEVEL_INFO, STAT_OK, MSG, ##__VA_ARGS__)
-
-#define STATUS_DEBUG(MSG, ...) \
- STATUS_MESSAGE(STAT_LEVEL_WARNING, STAT_OK, MSG, ##__VA_ARGS__)
-
-#define STATUS_WARNING(MSG, ...) \
- STATUS_MESSAGE(STAT_LEVEL_WARNING, STAT_OK, MSG, ##__VA_ARGS__)
-
-#define STATUS_ERROR(CODE, MSG, ...) \
- STATUS_MESSAGE(STAT_LEVEL_ERROR, CODE, MSG, ##__VA_ARGS__)
-
-#define ALARM(CODE) status_alarm(STATUS_LOCATION, CODE)
-#define ASSERT(COND) do {if (!(COND)) ALARM(STAT_INTERNAL_ERROR);} while (0)
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2016 Buildbotics LLC
- Copyright (c) 2010 - 2015 Alden S. Hart, Jr.
- Copyright (c) 2013 - 2015 Robert Giseburt
- All rights reserved.
-
- 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/>.
-
- The software is distributed in the hope that it will be useful, but
- WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
- Lesser General Public License for more details.
-
- You should have received a copy of the GNU Lesser General Public
- License along with the software. If not, see
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-#include "stepper.h"
-
-#include "config.h"
-#include "machine.h"
-#include "plan/runtime.h"
-#include "plan/exec.h"
-#include "motor.h"
-#include "hardware.h"
-#include "estop.h"
-#include "util.h"
-#include "cpp_magic.h"
-
-#include <string.h>
-#include <stdio.h>
-
-
-typedef enum {
- MOVE_TYPE_NULL, // null move - does a no-op
- MOVE_TYPE_ALINE, // acceleration planned line
- MOVE_TYPE_DWELL, // delay with no movement
-} move_type_t;
-
-
-typedef struct {
- // Runtime
- bool busy;
- bool requesting;
- uint16_t dwell;
-
- // Move prep
- bool move_ready; // prepped move ready for loader
- bool move_queued; // prepped move queued
- move_type_t move_type;
- uint16_t seg_period;
- uint32_t prep_dwell;
-} stepper_t;
-
-
-static stepper_t st = {0};
-
-
-void stepper_init() {
- // Setup step timer
- TIMER_STEP.CTRLB = STEP_TIMER_WGMODE; // waveform mode
- TIMER_STEP.INTCTRLA = STEP_TIMER_INTLVL; // interrupt mode
- TIMER_STEP.PER = STEP_TIMER_POLL; // timer idle rate
- TIMER_STEP.CTRLA = STEP_TIMER_ENABLE; // start step timer
-}
-
-
-void st_shutdown() {
- for (int motor = 0; motor < MOTORS; motor++)
- motor_enable(motor, false);
-
- st.dwell = 0;
- st.move_type = MOVE_TYPE_NULL;
-}
-
-
-/// Return true if motors or dwell are running
-bool st_is_busy() {return st.busy;}
-
-
-/// Interrupt handler for calling move exec function.
-/// ADC channel 0 triggered by load ISR as a "software" interrupt.
-ISR(ADCB_CH0_vect) {
- while (true) {
- stat_t status = mp_exec_move();
-
- switch (status) {
- case STAT_NOOP: st.busy = false; break; // No command executed
- case STAT_EAGAIN: continue; // No command executed, try again
-
- case STAT_OK: // Move executed
- if (!st.move_queued) ALARM(STAT_EXPECTED_MOVE); // No move was queued
- st.move_queued = false;
- st.move_ready = true;
- break;
-
- default: ALARM(status); break;
- }
-
- break;
- }
-
- ADCB_CH0_INTCTRL = 0;
- st.requesting = false;
-}
-
-
-static void _request_exec_move() {
- if (st.requesting) return;
- st.requesting = true;
-
- // Use ADC as a "software" interrupt to trigger next move exec
- ADCB_CH0_INTCTRL = ADC_CH_INTLVL_LO_gc; // LO level interrupt
- ADCB_CTRLA = ADC_ENABLE_bm | ADC_CH0START_bm;
-}
-
-
-/// Step timer interrupt routine
-/// Dequeue move and load into stepper struct
-ISR(STEP_TIMER_ISR) {
- // Dwell
- if (st.dwell && --st.dwell) return;
-
- // End last move
- TIMER_STEP.PER = STEP_TIMER_POLL;
-
- DMA.INTFLAGS = 0xff; // clear all interrups
- for (int motor = 0; motor < MOTORS; motor++)
- motor_end_move(motor);
-
- if (estop_triggered()) {
- st.move_type = MOVE_TYPE_NULL;
- return;
- }
-
- // If the next move is not ready try to load it
- if (!st.move_ready) {
- _request_exec_move();
- return;
- }
-
- // Wait until all motors have energized
- if (motor_energizing()) return;
-
- // Start move
- if (st.seg_period) {
- for (int motor = 0; motor < MOTORS; motor++)
- motor_load_move(motor);
-
- TIMER_STEP.PER = st.seg_period;
- st.busy = true;
-
- // Start dwell
- st.dwell = st.prep_dwell;
- }
-
- // We are done with this move
- st.move_type = MOVE_TYPE_NULL;
- st.seg_period = 0; // clear timer
- st.prep_dwell = 0; // clear dwell
- st.move_ready = false; // flip the flag back
-
- // Request next move if not currently in a dwell. Requesting the next move
- // may power up motors and the motors should not be powered up during a dwell.
- if (!st.dwell) _request_exec_move();
-}
-
-
-/* Prepare the next move
- *
- * This function precomputes the next pulse segment (move) so it can
- * be executed quickly in the ISR. It works in steps, rather than
- * length units. All args are provided as floats which converted here
- * to integer values.
- *
- * Args:
- * @param target signed position in steps for each motor.
- * Steps are fractional. Their sign indicates direction. Motors not in the
- * move have 0 steps.
- *
- * @param time is segment run time in minutes. If timing is not 100%
- * accurate this will affect the move velocity but not travel distance.
- */
-stat_t st_prep_line(float time, const float target[], const int32_t error[]) {
- // Trap conditions that would prevent queueing the line
- if (st.move_ready) return ALARM(STAT_INTERNAL_ERROR);
- if (isinf(time)) return ALARM(STAT_PREP_LINE_MOVE_TIME_INFINITE);
- if (isnan(time)) return ALARM(STAT_PREP_LINE_MOVE_TIME_NAN);
- if (time < EPSILON) return ALARM(STAT_MINIMUM_TIME_MOVE);
- if (MAX_SEGMENT_TIME < time) return ALARM(STAT_MAXIMUM_TIME_MOVE);
-
- // Setup segment parameters
- st.move_type = MOVE_TYPE_ALINE;
- st.seg_period = round(time * 60 * STEP_TIMER_FREQ); // Must fit 16-bit
- int32_t seg_clocks = (int32_t)st.seg_period * STEP_TIMER_DIV;
-
- // Prepare motor moves
- for (int motor = 0; motor < MOTORS; motor++)
- RITORNO
- (motor_prep_move(motor, seg_clocks, target[motor], error[motor], time));
-
- st.move_queued = true; // signal prep buffer ready (do this last)
-
- return STAT_OK;
-}
-
-
-/// Add a dwell to the move buffer
-void st_prep_dwell(float seconds) {
- if (st.move_ready) ALARM(STAT_INTERNAL_ERROR);
- st.move_type = MOVE_TYPE_DWELL;
- st.seg_period = STEP_TIMER_FREQ * 0.001; // 1 ms
- st.prep_dwell = seconds * 1000; // convert to ms
- st.move_queued = true; // signal prep buffer ready
-}
-
-
-void st_get_error(int32_t error[]) {
- for (int motor = 0; motor < MOTORS; motor++)
- error[motor] = motor_get_error(motor);
-}
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2016 Buildbotics LLC
- Copyright (c) 2010 - 2015 Alden S. Hart, Jr.
- Copyright (c) 2012 - 2015 Rob Giseburt
- All rights reserved.
-
- 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/>.
-
- The software is distributed in the hope that it will be useful, but
- WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
- Lesser General Public License for more details.
-
- You should have received a copy of the GNU Lesser General Public
- License along with the software. If not, see
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-#pragma once
-
-#include "status.h"
-
-#include <stdbool.h>
-#include <stdint.h>
-
-
-void stepper_init();
-void st_shutdown();
-bool st_is_busy();
-stat_t st_prep_line(float time, const float target[], const int32_t error[]);
-void st_prep_dwell(float seconds);
-void st_get_error(int32_t error[]);
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2016 Buildbotics LLC
- Copyright (c) 2010 - 2015 Alden S. Hart, Jr.
- All rights reserved.
-
- 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/>.
-
- The software is distributed in the hope that it will be useful, but
- WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
- Lesser General Public License for more details.
-
- You should have received a copy of the GNU Lesser General Public
- License along with the software. If not, see
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-/* Normally open switches (NO) trigger an interrupt on the
- * falling edge and lockout subsequent interrupts for the defined
- * lockout period. This approach beats doing debouncing as an
- * integration as switches fire immediately.
- *
- * Normally closed switches (NC) trigger an interrupt on the
- * rising edge and lockout subsequent interrupts for the defined
- * lockout period.
- *
- * These functions interact with each other to process switch closures
- * and firing. Each switch has a counter which is initially set to
- * negative SW_DEGLITCH_TICKS. When a switch closure is DETECTED the
- * count increments for each RTC tick. When the count reaches zero
- * the switch is tripped and action occurs. The counter continues to
- * increment positive until the lockout is exceeded.
- */
-
-#include "switch.h"
-#include "config.h"
-
-#include <avr/interrupt.h>
-
-#include <stdbool.h>
-
-
-typedef enum {
- SW_IDLE,
- SW_DEGLITCHING,
- SW_LOCKOUT
-} switch_debounce_t;
-
-
-typedef struct {
- uint8_t pin;
- switch_type_t type;
-
- switch_callback_t cb;
- bool state;
- switch_debounce_t debounce;
- int16_t count;
-} switch_t;
-
-
-
-static switch_t switches[SWITCHES] = {
- {.pin = MIN_X_PIN, .type = SW_NORMALLY_OPEN},
- {.pin = MAX_X_PIN, .type = SW_NORMALLY_OPEN},
- {.pin = MIN_Y_PIN, .type = SW_NORMALLY_OPEN},
- {.pin = MAX_X_PIN, .type = SW_NORMALLY_OPEN},
- {.pin = MIN_Z_PIN, .type = SW_NORMALLY_OPEN},
- {.pin = MAX_Z_PIN, .type = SW_NORMALLY_OPEN},
- {.pin = MIN_A_PIN, .type = SW_NORMALLY_OPEN},
- {.pin = MAX_A_PIN, .type = SW_NORMALLY_OPEN},
- {.pin = ESTOP_PIN, .type = SW_NORMALLY_OPEN},
- // {.pin = PROBE_PIN, .type = SW_NORMALLY_OPEN},
-};
-
-
-static bool _read_state(const switch_t *s) {
- // A normally open switch drives the pin low when thrown
- return (s->type == SW_NORMALLY_OPEN) ^ IN_PIN(s->pin);
-}
-
-
-static void _switch_isr() {
- for (int i = 0; i < SWITCHES; i++) {
- switch_t *s = &switches[i];
- bool state = _read_state(s);
-
- if (state == s->state || s->type == SW_DISABLED) continue;
-
- s->debounce = SW_DEGLITCHING;
- s->count = -SW_DEGLITCH_TICKS; // reset deglitch count
- s->state = state;
- PORT(s->pin)->INT0MASK &= ~BM(s->pin); // Disable INT0
- }
-}
-
-
-// Switch interrupt handler vectors
-ISR(PORTA_INT0_vect) {_switch_isr();}
-ISR(PORTB_INT0_vect) {_switch_isr();}
-ISR(PORTC_INT0_vect) {_switch_isr();}
-ISR(PORTD_INT0_vect) {_switch_isr();}
-ISR(PORTE_INT0_vect) {_switch_isr();}
-ISR(PORTF_INT0_vect) {_switch_isr();}
-
-
-void _switch_enable(switch_t *s, bool enable) {
- if (enable) {
- PORT(s->pin)->INT0MASK |= BM(s->pin); // Enable INT0
- s->state = _read_state(s); // Initialize state
- s->debounce = SW_IDLE;
-
- } else PORT(s->pin)->INT0MASK &= ~BM(s->pin); // Disable INT0
-}
-
-
-void switch_init() {
- for (int i = 0; i < SWITCHES; i++) {
- switch_t *s = &switches[i];
-
- // Pull up and trigger on both edges
- PINCTRL_PIN(s->pin) = PORT_OPC_PULLUP_gc | PORT_ISC_BOTHEDGES_gc;
- DIRCLR_PIN(s->pin); // Input
- PORT(s->pin)->INTCTRL |= SWITCH_INTLVL; // Set interrupt level
-
- _switch_enable(s, s->type != SW_DISABLED);
- }
-}
-
-
-/// Called from RTC on each tick
-void switch_rtc_callback() {
- for (int i = 0; i < SWITCHES; i++) {
- switch_t *s = &switches[i];
-
- if (s->type == SW_DISABLED || s->debounce == SW_IDLE) continue;
-
- // state is either lockout or deglitching
- if (++s->count == SW_LOCKOUT_TICKS) {
- PORT(s->pin)->INT0MASK |= BM(s->pin); // Renable INT0
- bool state = _read_state(s);
- s->debounce = SW_IDLE;
-
- // check if the state has changed while we were in lockout
- if (s->state != state) {
- s->state = state;
- s->debounce = SW_DEGLITCHING;
- s->count = -SW_DEGLITCH_TICKS;
- }
-
- continue;
- }
-
- if (!s->count) { // switch triggered
- s->debounce = SW_LOCKOUT;
- if (s->cb) s->cb(i, s->state);
- }
- }
-}
-
-
-bool switch_is_active(int index) {
- return switches[index].state;
-}
-
-
-bool switch_is_enabled(int index) {
- return switch_get_type(index) != SW_DISABLED;
-}
-
-
-switch_type_t switch_get_type(int index) {
- return switches[index].type;
-}
-
-
-void switch_set_type(int index, switch_type_t type) {
- switch_t *s = &switches[index];
-
- if (s->type != type) {
- s->type = type;
- _switch_enable(s, type != SW_DISABLED);
- }
-}
-
-
-void switch_set_callback(int index, switch_callback_t cb) {
- switches[index].cb = cb;
-}
-
-
-// Var callbacks
-uint8_t get_switch_type(int index) {return switch_get_type(index);}
-void set_switch_type(int index, uint8_t value) {switch_set_type(index, value);}
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2016 Buildbotics LLC
- Copyright (c) 2010 - 2015 Alden S. Hart, Jr.
- All rights reserved.
-
- 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/>.
-
- The software is distributed in the hope that it will be useful, but
- WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
- Lesser General Public License for more details.
-
- You should have received a copy of the GNU Lesser General Public
- License along with the software. If not, see
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-#pragma once
-
-
-#include "config.h"
-
-#include <stdint.h>
-#include <stdbool.h>
-
-
-// macros for finding the index into the switch table give the axis number
-#define MIN_SWITCH(axis) (axis * 2)
-#define MAX_SWITCH(axis) (axis * 2 + 1)
-
-
-typedef enum {
- SW_DISABLED,
- SW_NORMALLY_OPEN,
- SW_NORMALLY_CLOSED
-} switch_type_t;
-
-
-/// Switch IDs
-typedef enum {
- SW_MIN_X, SW_MAX_X,
- SW_MIN_Y, SW_MAX_Y,
- SW_MIN_Z, SW_MAX_Z,
- SW_MIN_A, SW_MAX_A,
- SW_ESTOP, SW_PROBE
-} switch_id_t;
-
-
-typedef void (*switch_callback_t)(switch_id_t sw, bool active);
-
-
-void switch_init();
-void switch_rtc_callback();
-bool switch_is_active(int index);
-bool switch_is_enabled(int index);
-switch_type_t switch_get_type(int index);
-void switch_set_type(int index, switch_type_t type);
-void switch_set_callback(int index, switch_callback_t cb);
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2016 Buildbotics LLC
- All rights reserved.
-
- 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/>.
-
- The software is distributed in the hope that it will be useful, but
- WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
- Lesser General Public License for more details.
-
- You should have received a copy of the GNU Lesser General Public
- License along with the software. If not, see
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-#include "usart.h"
-#include "cpp_magic.h"
-#include "config.h"
-
-#include <avr/io.h>
-#include <avr/interrupt.h>
-
-#include <stdio.h>
-#include <stdbool.h>
-
-// Ring buffers
-#define RING_BUF_NAME tx_buf
-#define RING_BUF_SIZE USART_TX_RING_BUF_SIZE
-#include "ringbuf.def"
-
-#define RING_BUF_NAME rx_buf
-#define RING_BUF_SIZE USART_RX_RING_BUF_SIZE
-#include "ringbuf.def"
-
-static int usart_flags = USART_CRLF;
-
-
-static void _set_dre_interrupt(bool enable) {
- if (enable) SERIAL_PORT.CTRLA |= USART_DREINTLVL_HI_gc;
- else SERIAL_PORT.CTRLA &= ~USART_DREINTLVL_HI_gc;
-}
-
-
-static void _set_rxc_interrupt(bool enable) {
- if (enable) {
- SERIAL_PORT.CTRLA |= USART_RXCINTLVL_HI_gc;
- if (4 <= rx_buf_space()) OUTCLR_PIN(SERIAL_CTS_PIN); // CTS Lo (enable)
-
- } else SERIAL_PORT.CTRLA &= ~USART_RXCINTLVL_HI_gc;
-}
-
-
-// Data register empty interrupt vector
-ISR(SERIAL_DRE_vect) {
- if (tx_buf_empty()) _set_dre_interrupt(false); // Disable interrupt
-
- else {
- SERIAL_PORT.DATA = tx_buf_peek();
- tx_buf_pop();
- }
-}
-
-
-// Data received interrupt vector
-ISR(SERIAL_RXC_vect) {
- if (rx_buf_full()) _set_rxc_interrupt(false); // Disable interrupt
-
- else {
- uint8_t data = SERIAL_PORT.DATA;
- rx_buf_push(data);
- if (rx_buf_space() < 4) OUTSET_PIN(SERIAL_CTS_PIN); // CTS Hi (disable)
- }
-}
-
-
-static int _usart_putchar(char c, FILE *f) {
- usart_putc(c);
- return 0;
-}
-
-static FILE _stdout = FDEV_SETUP_STREAM(_usart_putchar, 0, _FDEV_SETUP_WRITE);
-
-
-void usart_init(void) {
- // Setup ring buffer
- tx_buf_init();
- rx_buf_init();
-
- PR.PRPC &= ~PR_USART0_bm; // Disable power reduction
-
- // Setup pins
- OUTSET_PIN(SERIAL_CTS_PIN); // CTS Hi (disable)
- DIRSET_PIN(SERIAL_CTS_PIN); // CTS Output
- OUTSET_PIN(SERIAL_TX_PIN); // Tx High
- DIRSET_PIN(SERIAL_TX_PIN); // Tx Output
- DIRCLR_PIN(SERIAL_RX_PIN); // Rx Input
-
- // Set baud rate
- usart_set_baud(SERIAL_BAUD);
-
- // No parity, 8 data bits, 1 stop bit
- SERIAL_PORT.CTRLC = USART_CMODE_ASYNCHRONOUS_gc | USART_PMODE_DISABLED_gc |
- USART_CHSIZE_8BIT_gc;
-
- // Configure receiver and transmitter
- SERIAL_PORT.CTRLB = USART_RXEN_bm | USART_TXEN_bm | USART_CLK2X_bm;
-
- PMIC.CTRL |= PMIC_HILVLEN_bm; // Interrupt level on
-
- // Connect IO
- stdout = &_stdout;
- stderr = &_stdout;
-
- // Enable Rx
- _set_rxc_interrupt(true);
-}
-
-
-static void _set_baud(uint16_t bsel, uint8_t bscale) {
- SERIAL_PORT.BAUDCTRLB = (uint8_t)((bscale << 4) | (bsel >> 8));
- SERIAL_PORT.BAUDCTRLA = bsel;
-}
-
-
-void usart_set_baud(int baud) {
- // The BSEL / BSCALE values provided below assume a 32 Mhz clock
- // Assumes CTRLB CLK2X bit (0x04) is set
- // See http://www.avrcalc.elektronik-projekt.de/xmega/baud_rate_calculator
-
- switch (baud) {
- case USART_BAUD_9600: _set_baud(3325, 0b1101); break;
- case USART_BAUD_19200: _set_baud(3317, 0b1100); break;
- case USART_BAUD_38400: _set_baud(3301, 0b1011); break;
- case USART_BAUD_57600: _set_baud(1095, 0b1100); break;
- case USART_BAUD_115200: _set_baud(1079, 0b1011); break;
- case USART_BAUD_230400: _set_baud(1047, 0b1010); break;
- case USART_BAUD_460800: _set_baud(983, 0b1001); break;
- case USART_BAUD_921600: _set_baud(107, 0b1011); break;
- case USART_BAUD_500000: _set_baud(1, 0b0010); break;
- case USART_BAUD_1000000: _set_baud(1, 0b0001); break;
- }
-}
-
-
-void usart_set(int flag, bool enable) {
- if (enable) usart_flags |= flag;
- else usart_flags &= ~flag;
-}
-
-
-bool usart_is_set(int flags) {
- return (usart_flags & flags) == flags;
-}
-
-
-void usart_putc(char c) {
- while (tx_buf_full() || (usart_flags & USART_FLUSH)) continue;
-
- tx_buf_push(c);
-
- _set_dre_interrupt(true); // Enable interrupt
-
- if ((usart_flags & USART_CRLF) && c == '\n') usart_putc('\r');
-}
-
-
-void usart_puts(const char *s) {
- while (*s) usart_putc(*s++);
-}
-
-
-int8_t usart_getc() {
- while (rx_buf_empty()) continue;
-
- uint8_t data = rx_buf_peek();
- rx_buf_pop();
-
- _set_rxc_interrupt(true); // Enable interrupt
-
- return data;
-}
-
-
-char *usart_readline() {
- static char line[INPUT_BUFFER_LEN];
- static int i = 0;
- bool eol = false;
-
- while (!rx_buf_empty()) {
- char data = usart_getc();
-
- if (usart_flags & USART_ECHO) usart_putc(data);
-
- switch (data) {
- case '\r': case '\n': eol = true; break;
-
- case '\b': // BS - backspace
- if (usart_flags & USART_ECHO) {
- usart_putc(' ');
- usart_putc('\b');
- }
- if (i) i--;
- break;
-
- case 0x18: // CAN - Cancel or CTRL-X
- if (usart_flags & USART_ECHO)
- while (i) {
- usart_putc('\b');
- usart_putc(' ');
- usart_putc('\b');
- i--;
- }
-
- i = 0;
- break;
-
- default:
- line[i++] = data;
- if (i == INPUT_BUFFER_LEN - 1) eol = true;
- break;
- }
-
- if (eol) {
- line[i] = 0;
- i = 0;
- return line;
- }
- }
-
- return 0;
-}
-
-
-int16_t usart_peek() {
- return rx_buf_empty() ? -1 : rx_buf_peek();
-}
-
-
-void usart_flush() {
- usart_set(USART_FLUSH, true);
-
- while (!tx_buf_empty() || !(SERIAL_PORT.STATUS & USART_DREIF_bm) ||
- !(SERIAL_PORT.STATUS & USART_TXCIF_bm))
- continue;
-}
-
-
-void usart_rx_flush() {
- rx_buf_init();
-}
-
-
-int16_t usart_rx_space() {
- return rx_buf_space();
-}
-
-
-int16_t usart_rx_fill() {
- return rx_buf_fill();
-}
-
-
-int16_t usart_tx_space() {
- return tx_buf_space();
-}
-
-
-int16_t usart_tx_fill() {
- return tx_buf_fill();
-}
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2016 Buildbotics LLC
- All rights reserved.
-
- 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/>.
-
- The software is distributed in the hope that it will be useful, but
- WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
- Lesser General Public License for more details.
-
- You should have received a copy of the GNU Lesser General Public
- License along with the software. If not, see
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-#pragma once
-
-
-#include <stdint.h>
-#include <stdbool.h>
-
-#define USART_TX_RING_BUF_SIZE 256
-#define USART_RX_RING_BUF_SIZE 256
-
-enum {
- USART_BAUD_9600,
- USART_BAUD_19200,
- USART_BAUD_38400,
- USART_BAUD_57600,
- USART_BAUD_115200,
- USART_BAUD_230400,
- USART_BAUD_460800,
- USART_BAUD_921600,
- USART_BAUD_500000,
- USART_BAUD_1000000
-};
-
-enum {
- USART_CRLF = 1 << 0,
- USART_ECHO = 1 << 1,
- USART_XOFF = 1 << 2,
- USART_FLUSH = 1 << 3,
-};
-
-void usart_init();
-void usart_set_baud(int baud);
-void usart_set(int flag, bool enable);
-bool usart_is_set(int flags);
-void usart_putc(char c);
-void usart_puts(const char *s);
-int8_t usart_getc();
-char *usart_readline();
-int16_t usart_peek();
-void usart_flush();
-
-void usart_rx_flush();
-int16_t usart_rx_fill();
-int16_t usart_rx_space();
-inline bool usart_rx_empty() {return !usart_rx_fill();}
-inline bool usart_rx_full() {return !usart_rx_space();}
-
-int16_t usart_tx_fill();
-int16_t usart_tx_space();
-inline bool usart_tx_empty() {return !usart_tx_fill();}
-inline bool usart_tx_full() {return !usart_tx_space();}
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2016 Buildbotics LLC
- Copyright (c) 2010 - 2015 Alden S. Hart, Jr.
- All rights reserved.
-
- 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/>.
-
- The software is distributed in the hope that it will be useful, but
- WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
- Lesser General Public License for more details.
-
- You should have received a copy of the GNU Lesser General Public
- License along with the software. If not, see
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-#include "util.h"
-
-
-
-
-/// Fast inverse square root originally from Quake III Arena code. Original
-/// comments left intact.
-/// See: https://en.wikipedia.org/wiki/Fast_inverse_square_root
-float invsqrt(float number) {
- const float threehalfs = 1.5F;
-
- float x2 = number * 0.5;
- float y = number;
- long i = *(long *)&y; // evil floating point bit level hacking
- i = 0x5f3759df - (i >> 1); // what the fuck?
- y = *(float *)&i;
- y = y * (threehalfs - x2 * y * y); // 1st iteration
- y = y * (threehalfs - x2 * y * y); // 2nd iteration, this can be removed
-
- return y; //isnan(y) ? 1.0 / sqrt(number) : y;
-}
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2016 Buildbotics LLC
- Copyright (c) 2010 - 2014 Alden S. Hart, Jr.
- All rights reserved.
-
- 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/>.
-
- The software is distributed in the hope that it will be useful, but
- WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
- Lesser General Public License for more details.
-
- You should have received a copy of the GNU Lesser General Public
- License along with the software. If not, see
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-#pragma once
-
-
-#include "config.h"
-
-#include <stdint.h>
-#include <math.h>
-#include <string.h>
-#include <stdbool.h>
-
-
-// Vector utilities
-#define clear_vector(a) memset(a, 0, sizeof(a))
-#define copy_vector(d, s) memcpy(d, s, sizeof(d))
-
-// Math utilities
-inline float min(float a, float b) {return a < b ? a : b;}
-inline float max(float a, float b) {return a < b ? b : a;}
-inline float min3(float a, float b, float c) {return min(min(a, b), c);}
-inline float max3(float a, float b, float c) {return max(max(a, b), c);}
-inline float min4(float a, float b, float c, float d)
-{return min(min(a, b), min(c, d));}
-inline float max4(float a, float b, float c, float d)
-{return max(max(a, b), max(c, d));}
-
-float invsqrt(float number);
-
-// Floating-point utilities
-#define EPSILON 0.00001 // allowable rounding error for floats
-inline bool fp_EQ(float a, float b) {return fabs(a - b) < EPSILON;}
-inline bool fp_NE(float a, float b) {return fabs(a - b) > EPSILON;}
-inline bool fp_ZERO(float a) {return fabs(a) < EPSILON;}
-inline bool fp_NOT_ZERO(float a) {return !fp_ZERO(a);}
-inline bool fp_FALSE(float a) {return fp_ZERO(a);}
-inline bool fp_TRUE(float a) {return !fp_ZERO(a);}
-
-// Constants
-#define MM_PER_INCH 25.4
-#define INCHES_PER_MM (1 / 25.4)
-#define MICROSECONDS_PER_MINUTE 60000000.0
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2016 Buildbotics LLC
- Copyright (c) 2010 - 2015 Alden S. Hart, Jr.
- All rights reserved.
-
- 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/>.
-
- The software is distributed in the hope that it will be useful, but
- WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
- Lesser General Public License for more details.
-
- You should have received a copy of the GNU Lesser General Public
- License along with the software. If not, see
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-#include "usart.h"
-#include "machine.h"
-#include "spindle.h"
-#include "coolant.h"
-#include "plan/runtime.h"
-#include "plan/state.h"
-
-// Axis
-float get_position(int index) {return mp_runtime_get_axis_position(index);}
-
-// GCode
-int32_t get_line() {return mp_runtime_get_line();}
-PGM_P get_unit() {return gs_get_units_pgmstr(mach_get_units());}
-float get_speed() {return spindle_get_speed();}
-float get_feed() {return mach_get_feed_rate();} // TODO get runtime value
-uint8_t get_tool() {return mp_runtime_get_tool();}
-
-
-PGM_P get_feed_mode() {
- return gs_get_feed_mode_pgmstr(mach_get_feed_mode());
-}
-
-
-PGM_P get_plane() {return gs_get_plane_pgmstr(mach_get_plane());}
-
-
-PGM_P get_coord_system() {
- return gs_get_coord_system_pgmstr(mach_get_coord_system());
-}
-
-
-bool get_abs_override() {return mach_get_absolute_mode();}
-PGM_P get_path_mode() {return gs_get_path_mode_pgmstr(mach_get_path_mode());}
-
-
-PGM_P get_distance_mode() {
- return gs_get_distance_mode_pgmstr(mach_get_distance_mode());
-}
-
-
-PGM_P get_arc_dist_mode() {
- return gs_get_distance_mode_pgmstr(mach_get_arc_distance_mode());
-}
-
-
-float get_feed_override() {return mach_get_feed_override();}
-float get_speed_override() {return mach_get_spindle_override();}
-bool get_mist_coolant() {return coolant_get_mist();}
-bool get_flood_coolant() {return coolant_get_flood();}
-
-// System
-float get_velocity() {return mp_runtime_get_velocity();}
-bool get_echo() {return usart_is_set(USART_ECHO);}
-void set_echo(bool value) {return usart_set(USART_ECHO, value);}
-PGM_P get_state() {return mp_get_state_pgmstr(mp_get_state());}
-PGM_P get_cycle() {return mp_get_cycle_pgmstr(mp_get_cycle());}
-
-
-PGM_P get_hold_reason() {
- return mp_get_hold_reason_pgmstr(mp_get_hold_reason());
-}
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2016 Buildbotics LLC
- All rights reserved.
-
- 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/>.
-
- The software is distributed in the hope that it will be useful, but
- WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
- Lesser General Public License for more details.
-
- You should have received a copy of the GNU Lesser General Public
- License along with the software. If not, see
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-#include "vars.h"
-
-#include "cpp_magic.h"
-#include "status.h"
-#include "hardware.h"
-#include "config.h"
-
-#include <stdint.h>
-#include <stdbool.h>
-#include <stdio.h>
-#include <string.h>
-#include <stdlib.h>
-#include <ctype.h>
-#include <math.h>
-
-#include <avr/pgmspace.h>
-#include <avr/eeprom.h>
-
-
-typedef uint8_t flags_t;
-typedef const char *string;
-typedef PGM_P pstring;
-
-
-// Format strings
-static const char code_fmt[] PROGMEM = "\"%s\":";
-static const char indexed_code_fmt[] PROGMEM = "\"%c%s\":";
-
-
-// Type names
-static const char bool_name [] PROGMEM = "<bool>";
-#define TYPE_NAME(TYPE) static const char TYPE##_name [] PROGMEM = "<" #TYPE ">"
-MAP(TYPE_NAME, SEMI, flags_t, string, pstring, float, uint8_t, uint16_t,
- int32_t, char);
-
-
-// String
-static void var_print_string(string s) {
- printf_P(PSTR("\"%s\""), s);
-}
-
-// Program string
-static void var_print_pstring(pstring s) {
- printf_P(PSTR("\"%S\""), s);
-}
-
-
-// Flags
-extern void print_status_flags(uint8_t x);
-
-static void var_print_flags_t(uint8_t x) {
- print_status_flags(x);
-}
-
-
-// Float
-static void var_print_float(float x) {
- char buf[20];
-
- int len = sprintf_P(buf, PSTR("%.6f"), x);
-
- // Remove trailing zeros
- for (int i = len; 0 < i; i--) {
- if (buf[i - 1] == '.') buf[i - 1] = 0;
- else if (buf[i - 1] == '0') {
- buf[i - 1] = 0;
- continue;
- }
-
- break;
- }
-
- printf(buf);
-}
-
-
-static float var_parse_float(const char *value) {
- return strtod(value, 0);
-}
-
-
-// Bool
-static void var_print_bool(bool x) {
- printf_P(x ? PSTR("true") : PSTR("false"));
-}
-
-
-static bool var_parse_bool(const char *value) {
- return !strcasecmp(value, "true") || var_parse_float(value);
-}
-
-
-static uint8_t eeprom_read_bool(bool *addr) {
- return eeprom_read_byte((uint8_t *)addr);
-}
-
-
-static void eeprom_update_bool(bool *addr, bool value) {
- eeprom_update_byte((uint8_t *)addr, value);
-}
-
-
-// Char
-static void var_print_char(char x) {putchar('"'); putchar(x); putchar('"');}
-static char var_parse_char(const char *value) {return value[0];}
-
-
-static uint8_t eeprom_read_char(char *addr) {
- return eeprom_read_byte((uint8_t *)addr);
-}
-
-
-static void eeprom_update_char(char *addr, char value) {
- eeprom_update_byte((uint8_t *)addr, value);
-}
-
-
-// int8
-#if 0
-static void var_print_int8_t(int8_t x) {
- printf_P(PSTR("%"PRIi8), x);
-}
-
-
-static int8_t var_parse_int8_t(const char *value) {
- return strtol(value, 0, 0);
-}
-
-
-static int8_t eeprom_read_int8_t(int8_t *addr) {
- return eeprom_read_byte((uint8_t *)addr);
-}
-
-
-static void eeprom_update_int8_t(int8_t *addr, int8_t value) {
- eeprom_update_byte((uint8_t *)addr, value);
-}
-#endif
-
-// uint8
-static void var_print_uint8_t(uint8_t x) {
- printf_P(PSTR("%"PRIu8), x);
-}
-
-
-static uint8_t var_parse_uint8_t(const char *value) {
- return strtol(value, 0, 0);
-}
-
-
-static uint8_t eeprom_read_uint8_t(uint8_t *addr) {
- return eeprom_read_byte(addr);
-}
-
-
-static void eeprom_update_uint8_t(uint8_t *addr, uint8_t value) {
- eeprom_update_byte(addr, value);
-}
-
-
-// unit16
-static void var_print_uint16_t(uint16_t x) {
- printf_P(PSTR("%"PRIu16), x);
-}
-
-
-static uint16_t var_parse_uint16_t(const char *value) {
- return strtoul(value, 0, 0);
-}
-
-
-static uint16_t eeprom_read_uint16_t(uint16_t *addr) {
- return eeprom_read_word(addr);
-}
-
-
-static void eeprom_update_uint16_t(uint16_t *addr, uint16_t value) {
- eeprom_update_word(addr, value);
-}
-
-
-// int32
-static void var_print_int32_t(uint32_t x) {
- printf_P(PSTR("%"PRIi32), x);
-}
-
-
-// Var forward declarations
-#define VAR(NAME, CODE, TYPE, INDEX, SET, ...) \
- TYPE get_##NAME(IF(INDEX)(int index)); \
- IF(SET) \
- (void set_##NAME(IF(INDEX)(int index,) TYPE value);)
-
-#include "vars.def"
-#undef VAR
-
-// Var names, count & help
-#define VAR(NAME, CODE, TYPE, INDEX, SET, SAVE, HELP) \
- static const char NAME##_name[] PROGMEM = #NAME; \
- static const char NAME##_help[] PROGMEM = HELP;
-
-#include "vars.def"
-#undef VAR
-
-// EEPROM storage
-#define VAR(NAME, CODE, TYPE, INDEX, SET, SAVE, HELP) \
- IF(SAVE) \
- (static TYPE NAME##_eeprom IF(INDEX)([INDEX]) EEMEM;)
-
-#include "vars.def"
-#undef VAR
-
-static uint16_t eeprom_crc EEMEM;
-
-// Last value
-#define VAR(NAME, CODE, TYPE, INDEX, ...) \
- static TYPE NAME##_state IF(INDEX)([INDEX]);
-
-#include "vars.def"
-#undef VAR
-
-
-
-void vars_init() {
- vars_restore();
-
- // Initialize var state
-#define VAR(NAME, CODE, TYPE, INDEX, SET, SAVE, ...) \
- IF(INDEX)(for (int i = 0; i < INDEX; i++)) \
- (NAME##_state)IF(INDEX)([i]) = get_##NAME(IF(INDEX)(i));
-
-#include "vars.def"
-#undef VAR
-}
-
-
-void vars_report(bool full) {
- // Save and disable watchdog
- uint8_t wd_state = hw_disable_watchdog();
-
- bool reported = false;
-
-#define VAR(NAME, CODE, TYPE, INDEX, ...) \
- IF(INDEX)(for (int i = 0; i < (INDEX ? INDEX : 1); i++)) { \
- TYPE value = get_##NAME(IF(INDEX)(i)); \
- TYPE last = (NAME##_state)IF(INDEX)([i]); \
- \
- if (value != last || full) { \
- (NAME##_state)IF(INDEX)([i]) = value; \
- \
- if (!reported) { \
- reported = true; \
- putchar('{'); \
- } else putchar(','); \
- \
- printf_P \
- (IF_ELSE(INDEX)(indexed_code_fmt, code_fmt), \
- IF(INDEX)(INDEX##_LABEL[i],) #CODE); \
- \
- var_print_##TYPE(value); \
- } \
- }
-
-#include "vars.def"
-#undef VAR
-
- if (reported) printf("}\n");
-
- // Restore watchdog
- hw_restore_watchdog(wd_state);
-}
-
-
-int vars_find(const char *name) {
- uint8_t i = 0;
- uint8_t n = 0;
- unsigned len = strlen(name);
-
- if (!len) return -1;
-
-#define VAR(NAME, CODE, TYPE, INDEX, ...) \
- if (!strcmp(IF_ELSE(INDEX)(name + 1, name), #CODE)) { \
- IF(INDEX) \
- (i = strchr(INDEX##_LABEL, name[0]) - INDEX##_LABEL; \
- if (INDEX <= i) return -1); \
- return n; \
- } \
- n++;
-
-#include "vars.def"
-#undef VAR
-
- return -1;
-}
-
-
-bool vars_print(const char *name) {
- uint8_t i;
- unsigned len = strlen(name);
-
- if (!len) return false;
-
-#define VAR(NAME, CODE, TYPE, INDEX, ...) \
- if (!strcmp(IF_ELSE(INDEX)(name + 1, name), #CODE)) { \
- IF(INDEX) \
- (i = strchr(INDEX##_LABEL, name[0]) - INDEX##_LABEL; \
- if (INDEX <= i) return false); \
- \
- putchar('{'); \
- printf_P \
- (IF_ELSE(INDEX)(indexed_code_fmt, code_fmt), \
- IF(INDEX)(INDEX##_LABEL[i],) #CODE); \
- var_print_##TYPE(get_##NAME(IF(INDEX)(i))); \
- putchar('}'); \
- \
- return true; \
- }
-
-#include "vars.def"
-#undef VAR
-
- return false;
-}
-
-
-bool vars_set(const char *name, const char *value) {
- uint8_t i;
- unsigned len = strlen(name);
-
- if (!len) return false;
-
-#define VAR(NAME, CODE, TYPE, INDEX, SET, ...) \
- IF(SET) \
- (if (!strcmp(IF_ELSE(INDEX)(name + 1, name), #CODE)) { \
- IF(INDEX) \
- (i = strchr(INDEX##_LABEL, name[0]) - INDEX##_LABEL; \
- if (INDEX <= i) return false); \
- \
- TYPE x = var_parse_##TYPE(value); \
- set_##NAME(IF(INDEX)(i,) x); \
- \
- return true; \
- }) \
-
-#include "vars.def"
-#undef VAR
-
- return false;
-}
-
-
-int vars_parser(char *vars) {
- if (!*vars || !*vars++ == '{') return STAT_OK;
-
- while (*vars) {
- while (isspace(*vars)) vars++;
- if (*vars == '}') return STAT_OK;
- if (*vars != '"') return STAT_COMMAND_NOT_ACCEPTED;
-
- // Parse name
- vars++; // Skip "
- const char *name = vars;
- while (*vars && *vars != '"') vars++;
- *vars++ = 0;
-
- while (isspace(*vars)) vars++;
- if (*vars != ':') return STAT_COMMAND_NOT_ACCEPTED;
- vars++;
- while (isspace(*vars)) vars++;
-
- // Parse value
- const char *value = vars;
- while (*vars && *vars != ',' && *vars != '}') vars++;
- if (*vars) {
- char c = *vars;
- *vars++ = 0;
- vars_set(name, value);
- if (c == '}') break;
- }
- }
-
- return STAT_OK;
-}
-
-
-void vars_print_help() {
- static const char fmt[] PROGMEM = " $%-5s %-20S %-16S %S\n";
-
- // Save and disable watchdog
- uint8_t wd_state = hw_disable_watchdog();
-
-#define VAR(NAME, CODE, TYPE, ...) \
- printf_P(fmt, #CODE, NAME##_name, TYPE##_name, NAME##_help);
-#include "vars.def"
-#undef VAR
-
- // Restore watchdog
- hw_restore_watchdog(wd_state);
-}
-
-
-uint16_t vars_crc() {
- // Save and disable watchdog
- uint8_t wd_state = hw_disable_watchdog();
-
- CRC.CTRL = CRC_RESET_RESET0_gc;
- CRC.CTRL = CRC_SOURCE_IO_gc; // Must be after reset
-
-#define VAR(NAME, CODE, TYPE, INDEX, SET, SAVE, ...) \
- IF(SAVE) \
- ({ \
- for (int j = 0; ; j++) { \
- char c = pgm_read_byte(&NAME##_name[j]); \
- if (!c) break; \
- CRC.DATAIN = c; \
- } \
- \
- CRC.DATAIN = INDEX; \
- })
-
-#include "vars.def"
-#undef VAR
-
- // Restore watchdog
- hw_restore_watchdog(wd_state);
-
- CRC.STATUS = CRC_BUSY_bm; // Done
- return CRC.CHECKSUM1 << 8 | CRC.CHECKSUM0;
-}
-
-
-void vars_save() {
- // Save and disable watchdog
- uint8_t wd_state = hw_disable_watchdog();
-
-#define VAR(NAME, CODE, TYPE, INDEX, SET, SAVE, ...) \
- IF(SAVE) \
- (IF(INDEX)(for (int i = 0; i < (INDEX ? INDEX : 1); i++)) { \
- TYPE value = get_##NAME(IF(INDEX)(i)); \
- eeprom_update_##TYPE(&NAME##_eeprom IF(INDEX)([i]), value); \
- }) \
-
-#include "vars.def"
-#undef VAR
-
- // Restore watchdog
- hw_restore_watchdog(wd_state);
-
- // Save CRC
- eeprom_update_word(&eeprom_crc, vars_crc());
-}
-
-
-
-bool vars_valid() {
- return eeprom_read_word(&eeprom_crc) == vars_crc();
-}
-
-
-stat_t vars_restore() {
- if (!vars_valid()) return STAT_EEPROM_DATA_INVALID;
-
- // Save and disable watchdog
- uint8_t wd_state = hw_disable_watchdog();
-
-#define VAR(NAME, CODE, TYPE, INDEX, SET, SAVE, ...) \
- IF(SAVE) \
- (IF(INDEX)(for (int i = 0; i < (INDEX ? INDEX : 1); i++)) { \
- TYPE value = eeprom_read_##TYPE(&NAME##_eeprom IF(INDEX)([i])); \
- set_##NAME(IF(INDEX)(i,) value); \
- NAME##_state IF(INDEX)([i]) = value; \
- }) \
-
-#include "vars.def"
-#undef VAR
-
- // Restore watchdog
- hw_restore_watchdog(wd_state);
-
- return STAT_OK;
-}
-
-
-void vars_clear() {
- eeprom_update_word(&eeprom_crc, -1);
-}
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2016 Buildbotics LLC
- All rights reserved.
-
- 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/>.
-
- The software is distributed in the hope that it will be useful, but
- WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
- Lesser General Public License for more details.
-
- You should have received a copy of the GNU Lesser General Public
- License along with the software. If not, see
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-#define AXES_LABEL "xyzabcuvw"
-#define MOTORS_LABEL "0123"
-#define SWITCHES_LABEL "0123456789"
-
-// VAR(name, code, type, index, settable, save, help)
-
-// Motor
-VAR(axis_name, an, char, MOTORS, 1, 1, "Maps motor to axis name")
-VAR(step_angle, sa, float, MOTORS, 1, 1, "In degrees per full step")
-VAR(travel, tr, float, MOTORS, 1, 1, "Travel in mm per revolution")
-VAR(microstep, mi, uint16_t, MOTORS, 1, 1, "Microsteps per full step")
-VAR(reverse, rv, uint8_t, MOTORS, 1, 1, "Reverse motor polarity")
-
-VAR(power_mode, pm, uint8_t, MOTORS, 1, 1, "Motor power mode")
-VAR(drive_power, dp, float, MOTORS, 1, 1, "Motor drive power")
-VAR(idle_power, ip, float, MOTORS, 1, 1, "Motor idle power")
-VAR(current_power, cp, float, MOTORS, 0, 0, "Motor power now")
-VAR(status_flags, mf, uint8_t, MOTORS, 0, 0, "Motor status flags")
-VAR(status_strings, ms, flags_t, MOTORS, 0, 0, "Motor status strings")
-
-VAR(motor_fault, mf, bool, 0, 0, 0, "Motor fault status")
-
-VAR(velocity_max, vm, float, MOTORS, 1, 1, "Maxium velocity in mm/min")
-VAR(feedrate_max, fr, float, MOTORS, 1, 1, "Maxium feedrate in mm/min")
-VAR(jerk_max, jm, float, MOTORS, 1, 1, "Maxium jerk in mm/min^3")
-VAR(junction_dev, jd, float, MOTORS, 1, 1, "Junction deviation")
-VAR(radius, ra, float, MOTORS, 1, 1, "Axis radius or zero")
-
-// Homing
-VAR(homing_mode, hm, uint8_t, MOTORS, 1, 1, "Homing type")
-VAR(travel_min, tn, float, MOTORS, 1, 1, "Minimum soft limit")
-VAR(travel_max, tm, float, MOTORS, 1, 1, "Maximum soft limit")
-VAR(search_velocity,sv, float, MOTORS, 1, 1, "Homing search velocity")
-VAR(latch_velocity, lv, float, MOTORS, 1, 1, "Homing latch velocity")
-VAR(latch_backoff, lb, float, MOTORS, 1, 1, "Homing latch backof")
-VAR(zero_backoff, zb, float, MOTORS, 1, 1, "Homing zero backof")
-
-// Axis
-VAR(position, p, float, AXES, 0, 0, "Current axis position")
-
-// Spindle
-VAR(spindle_type, st, uint8_t, 0, 1, 1, "PWM=0 or HUANYANG=1")
-VAR(spin_reverse, sr, bool, 0, 1, 1, "Reverse spin")
-VAR(max_spin, sx, float, 0, 1, 1, "Maximum spindle speed")
-VAR(min_spin, sm, float, 0, 1, 1, "Minimum spindle speed")
-VAR(spin_min_pulse, np, float, 0, 1, 1, "Minimum pulse width")
-VAR(spin_max_pulse, mp, float, 0, 1, 1, "Maximum pulse width")
-VAR(spin_up, su, float, 0, 1, 1, "Spin up velocity")
-VAR(spin_down, sd, float, 0, 1, 1, "Spin down velocity")
-
-// Huanyang spindle
-VAR(huanyang_id, hi, uint8_t, 0, 1, 1, "Huanyang ID")
-VAR(huanyang_freq, hz, float, 0, 0, 0, "Huanyang actual freq")
-VAR(huanyang_current, hc, float, 0, 0, 0, "Huanyang actual current")
-VAR(huanyang_rpm, hr, uint16_t, 0, 0, 0, "Huanyang actual RPM")
-//VAR(huanyang_dcv, hd, uint16_t, 0, 0, 0, "Huanyang DC voltage")
-//VAR(huanyang_acv, ha, uint16_t, 0, 0, 0, "Huanyang AC voltage")
-VAR(huanyang_temp, ht, uint16_t, 0, 0, 0, "Huanyang temperature")
-VAR(huanyang_max_freq, hx, float, 0, 0, 0, "Huanyang max freq")
-VAR(huanyang_min_freq, hm, float, 0, 0, 0, "Huanyang min freq")
-VAR(huanyang_rated_rpm, hq, uint16_t, 0, 0, 0, "Huanyang rated RPM")
-VAR(huanyang_status, hs, uint8_t, 0, 0, 0, "Huanyang status flags")
-VAR(huanyang_debug, hb, bool, 0, 1, 0, "Huanyang debugging")
-VAR(huanyang_connected, he, bool, 0, 0, 0, "Huanyang connected")
-
-// Switches
-VAR(switch_type, sw, uint8_t, SWITCHES, 1, 1, "Normally open or closed")
-
-// GCode
-VAR(line, ln, int32_t, 0, 0, 0, "Last GCode line executed")
-VAR(unit, u, pstring, 0, 0, 0, "Current unit of measure")
-VAR(speed, s, float, 0, 0, 0, "Current spindle speed")
-VAR(feed, f, float, 0, 0, 0, "Current feed rate")
-VAR(tool, t, uint8_t, 0, 0, 0, "Current tool")
-VAR(feed_mode, fm, pstring, 0, 0, 0, "Current feed rate mode")
-VAR(plane, pa, pstring, 0, 0, 0, "Current plane")
-VAR(coord_system, cs, pstring, 0, 0, 0, "Current coordinate system")
-VAR(abs_override, ao, bool, 0, 0, 0, "Absolute override enabled")
-VAR(path_mode, pc, pstring, 0, 0, 0, "Current path control mode")
-VAR(distance_mode, dm, pstring, 0, 0, 0, "Current distance mode")
-VAR(arc_dist_mode, ad, pstring, 0, 0, 0, "Current arc distance mode")
-VAR(mist_coolant, mc, bool, 0, 0, 0, "Mist coolant enabled")
-VAR(flood_coolant, fc, bool, 0, 0, 0, "Flood coolant enabled")
-VAR(feed_override, fo, float, 0, 0, 0, "Feed rate override")
-VAR(speed_override, so, float, 0, 0, 0, "Spindle speed override")
-
-// System
-VAR(velocity, v, float, 0, 0, 0, "Current velocity")
-VAR(hw_id, id, string, 0, 0, 0, "Hardware ID")
-VAR(echo, ec, bool, 0, 1, 0, "Enable or disable echo")
-VAR(estop, es, bool, 0, 1, 0, "Emergency stop")
-VAR(estop_reason, er, pstring, 0, 0, 0, "Emergency stop reason")
-VAR(state, x, pstring, 0, 0, 0, "Machine state")
-VAR(cycle, c, pstring, 0, 0, 0, "Machine cycle")
-VAR(hold_reason, pr, pstring, 0, 0, 0, "Machine pause reason")
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2016 Buildbotics LLC
- All rights reserved.
-
- 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/>.
-
- The software is distributed in the hope that it will be useful, but
- WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
- Lesser General Public License for more details.
-
- You should have received a copy of the GNU Lesser General Public
- License along with the software. If not, see
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-#pragma once
-
-#include "status.h"
-
-#include <stdbool.h>
-
-
-void vars_init();
-
-void vars_report(bool full);
-int vars_find(const char *name);
-bool vars_print(const char *name);
-bool vars_set(const char *name, const char *value);
-int vars_parser(char *vars);
-void vars_print_help();
-
-void vars_save();
-bool vars_valid();
-stat_t vars_restore();
-void vars_clear();
+++ /dev/null
-/************************************************************************/
-/* XMEGA EEPROM Driver */
-/* */
-/* eeprom.c */
-/* */
-/* Alex Forencich <alex@alexforencich.com> */
-/* */
-/* Copyright (c) 2011 Alex Forencich */
-/* */
-/* Permission is hereby granted, free of charge, to any person */
-/* obtaining a copy of this software and associated documentation */
-/* files(the "Software"), to deal in the Software without restriction, */
-/* including without limitation the rights to use, copy, modify, merge, */
-/* publish, distribute, sublicense, and/or sell copies of the Software, */
-/* and to permit persons to whom the Software is furnished to do so, */
-/* subject to the following conditions: */
-/* */
-/* The above copyright notice and this permission notice shall be */
-/* included in all copies or substantial portions of the Software. */
-/* */
-/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT 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 "eeprom_driver.h"
-
-
-static inline void NVM_EXEC(void) {
- void *z = (void *)&NVM_CTRLA;
-
- __asm__ volatile("out %[ccp], %[ioreg]" "\n\t"
- "st z, %[cmdex]"
- :
- : [ccp] "I" (_SFR_IO_ADDR(CCP)),
- [ioreg] "d" (CCP_IOREG_gc),
- [cmdex] "r" (NVM_CMDEX_bm),
- [z] "z" (z)
- );
-}
-
-
-void wait_for_nvm() {
- while (NVM.STATUS & NVM_NVMBUSY_bm) continue;
-}
-
-
-void EEPROM_erase_all() {
- wait_for_nvm();
- NVM.CMD = NVM_CMD_ERASE_EEPROM_gc;
- NVM_EXEC();
-}
+++ /dev/null
-/************************************************************************/
-/* XMEGA EEPROM Driver */
-/* */
-/* eeprom.h */
-/* */
-/* Alex Forencich <alex@alexforencich.com> */
-/* */
-/* Copyright (c) 2011 Alex Forencich */
-/* */
-/* Permission is hereby granted, free of charge, to any person */
-/* obtaining a copy of this software and associated documentation */
-/* files(the "Software"), to deal in the Software without restriction, */
-/* including without limitation the rights to use, copy, modify, merge, */
-/* publish, distribute, sublicense, and/or sell copies of the Software, */
-/* and to permit persons to whom the Software is furnished to do so, */
-/* subject to the following conditions: */
-/* */
-/* The above copyright notice and this permission notice shall be */
-/* included in all copies or substantial portions of the Software. */
-/* */
-/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT 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. */
-/* */
-/************************************************************************/
-
-#pragma once
-
-#include <avr/io.h>
-#include <avr/interrupt.h>
-#include <avr/eeprom.h>
-
-#include "xboot.h"
-
-#ifndef EEPROM_PAGE_SIZE
-#define EEPROM_PAGE_SIZE E2PAGESIZE
-#endif
-
-#define EEPROM_read_byte(addr) \
- eeprom_read_byte((const uint8_t *)((uint16_t)(addr)))
-
-#define EEPROM_write_byte(addr, value) \
- eeprom_write_byte((uint8_t *)((uint16_t)(addr)), (value))
-
-#define EEPROM_read_block(addr, dest, len) \
- eeprom_read_block((dest), (void *)((uint16_t)(addr)), (len))
-
-#define EEPROM_write_block(addr, src, len) \
- eeprom_write_block((src), (void *)((uint16_t)(addr)), (len))
-
-void EEPROM_erase_all();
+++ /dev/null
-/************************************************************************/
-/* XBoot Extensible AVR Bootloader */
-/* */
-/* XBoot Protocol Definition */
-/* */
-/* protocol.h */
-/* */
-/* Alex Forencich <alex@alexforencich.com> */
-/* */
-/* Copyright (c) 2010 Alex Forencich */
-/* */
-/* Permission is hereby granted, free of charge, to any person */
-/* obtaining a copy of this software and associated documentation */
-/* files(the "Software"), to deal in the Software without restriction, */
-/* including without limitation the rights to use, copy, modify, merge, */
-/* publish, distribute, sublicense, and/or sell copies of the Software, */
-/* and to permit persons to whom the Software is furnished to do so, */
-/* subject to the following conditions: */
-/* */
-/* The above copyright notice and this permission notice shall be */
-/* included in all copies or substantial portions of the Software. */
-/* */
-/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT 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. */
-/* */
-/************************************************************************/
-
-#pragma once
-
-#include "xboot.h"
-
-// General Commands
-#define CMD_SYNC '\x1b'
-
-// Informational Commands
-#define CMD_CHECK_AUTOINCREMENT 'a'
-#define CMD_CHECK_BLOCK_SUPPORT 'b'
-#define CMD_PROGRAMMER_TYPE 'p'
-#define CMD_DEVICE_CODE 't'
-#define CMD_PROGRAM_ID 'S'
-#define CMD_VERSION 'V'
-#define CMD_READ_SIGNATURE 's'
-
-// Addressing
-#define CMD_SET_ADDRESS 'A'
-#define CMD_SET_EXT_ADDRESS 'H'
-
-// Erase
-#define CMD_CHIP_ERASE 'e'
-
-// Block Access
-#define CMD_BLOCK_LOAD 'B'
-#define CMD_BLOCK_READ 'g'
-
-// Byte Access
-#define CMD_READ_BYTE 'R'
-#define CMD_WRITE_LOW_BYTE 'c'
-#define CMD_WRITE_HIGH_BYTE 'C'
-#define CMD_WRITE_PAGE 'm'
-#define CMD_WRITE_EEPROM_BYTE 'D'
-#define CMD_READ_EEPROM_BYTE 'd'
-
-// Lock and Fuse Bits
-#define CMD_WRITE_LOCK_BITS 'l'
-#define CMD_READ_LOCK_BITS 'r'
-#define CMD_READ_LOW_FUSE_BITS 'F'
-#define CMD_READ_HIGH_FUSE_BITS 'N'
-#define CMD_READ_EXT_FUSE_BITS 'Q'
-
-// Bootloader Commands
-#define CMD_ENTER_PROG_MODE 'P'
-#define CMD_LEAVE_PROG_MODE 'L'
-#define CMD_EXIT_BOOTLOADER 'E'
-#define CMD_SET_LED 'x'
-#define CMD_CLEAR_LED 'y'
-#define CMD_SET_TYPE 'T'
-
-#define CMD_CRC 'h'
-
-// Memory types for block access
-#define MEM_EEPROM 'E'
-#define MEM_FLASH 'F'
-#define MEM_USERSIG 'U'
-#define MEM_PRODSIG 'P'
-
-// Sections for CRC checks
-#define SECTION_FLASH 'F'
-#define SECTION_APPLICATION 'A'
-#define SECTION_BOOT 'B'
-#define SECTION_APP 'a'
-#define SECTION_APP_TEMP 't'
-
-// Command Responses
-#define REPLY_ACK '\r'
-#define REPLY_YES 'Y'
-#define REPLY_ERROR '?'
+++ /dev/null
-;******************************************************************************
-;*
-;* XMEGA Self-programming driver assembly source file.
-;*
-;* This file contains the low-level implementations for the
-;* XMEGA Self-programming driver. It is written for the GCC Assembler.
-;*
-;* If any SPM instructions are used, the linker file must define
-;* a segment named bootloader which must be located in the device Boot section.
-;* This can be done by passing "-Wl,--section-start=.BOOT=0x020000" to the
-;* linker with the correct address for the boot section.
-;*
-;* None of these routines clean up the NVM Command Register after use.
-;* It is therefore important to write NVM_CMD_NO_OPERATION_gc (0x00) to this
-;* register when you are finished using any of the functions in this driver.
-;*
-;* For all routines, it is important that any interrupt handlers do not
-;* perform any NVM operations. The user must implement a scheme for mutually
-;* exclusive access to the NVM. However, the 4-cycle timeout will work fine,
-;* since writing to the Configuration Change Protection register (CCP)
-;* automatically disables interrupts for 4 instruction cycles.
-;*
-;* Note on IAR calling convention:
-;* Scratch registers: R18-R27, R30-R31
-;* Preserved registers: R2-R17, R28-R29
-;* Parameter registers: R8-R25 (2-,4-, or 8- byte alignment)
-;* Return registers: R18-R25 (up to 64-bit)
-;*
-;* Application note:
-;* AVR1316: XMEGA Self-programming
-;*
-;* Documentation
-;* For comprehensive code documentation, supported compilers, compiler
-;* settings and supported devices see readme.html
-;*
-;* Atmel Corporation: http:;www.atmel.com \n
-;* Support email: avr@atmel.com
-;*
-;* $Revision: 1153 $
-;* $Date: 2007-12-18 09:48:23 +0100 (ti, 18 des 2007) $
-;*
-;* Copyright (c) 2007, Atmel Corporation All rights reserved.
-;*
-;* Redistribution and use in source and binary forms, with or without
-;* modification, are permitted provided that the following conditions are met:
-;*
-;* 1. Redistributions of source code must retain the above copyright notice,
-;* this list of conditions and the following disclaimer.
-;*
-;* 2. Redistributions in binary form must reproduce the above copyright notice,
-;* this list of conditions and the following disclaimer in the documentation
-;* and/or other materials provided with the distribution.
-;*
-;* 3. The name of ATMEL may not be used to endorse or promote products derived
-;* from this software without specific prior written permission.
-;*
-;* THIS SOFTWARE IS PROVIDED BY ATMEL "AS IS" AND ANY EXPRESS OR IMPLIED
-;* WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
-;* MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE EXPRESSLY AND
-;* SPECIFICALLY DISCLAIMED. IN NO EVENT SHALL ATMEL BE LIABLE FOR ANY DIRECT,
-;* INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
-;* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-;* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
-;* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
-;* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
-;* THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
-;******************************************************************************
-
-#include <avr/io.h>
-;#include "Flash_Defines.h"
-
-/* Define the size of the flash page if not defined in the header files. */
-#ifndef APP_SECTION_PAGE_SIZE
- #error APP_SECTION_PAGE_SIZE must be defined if not defined in header files.
- //#define APP_SECTION_PAGE_SIZE 512
-#endif /*APP_SECTION_PAGE_SIZE*/
-
-/* Defines not yet included in header file. */
-#define NVM_CMD_NO_OPERATION_gc (0x00<<0) // Noop/Ordinary LPM
-#define NVM_CMD_READ_USER_SIG_ROW_gc (0x01<<0) // Read user signature row
-#define NVM_CMD_READ_CALIB_ROW_gc (0x02<<0) // Read calibration row
-#define NVM_CMD_READ_EEPROM_gc (0x06<<0) // Read EEPROM
-#define NVM_CMD_READ_FUSES_gc (0x07<<0) // Read fuse byte
-#define NVM_CMD_WRITE_LOCK_BITS_gc (0x08<<0) // Write lock bits
-#define NVM_CMD_ERASE_USER_SIG_ROW_gc (0x18<<0) // Erase user signature row
-#define NVM_CMD_WRITE_USER_SIG_ROW_gc (0x1A<<0) // Write user signature row
-#define NVM_CMD_ERASE_APP_gc (0x20<<0) // Erase Application Section
-#define NVM_CMD_ERASE_APP_PAGE_gc (0x22<<0) // Erase Application Section page
-#define NVM_CMD_LOAD_FLASH_BUFFER_gc (0x23<<0) // Load Flash page buffer
-#define NVM_CMD_WRITE_APP_PAGE_gc (0x24<<0) // Write Application Section page
-#define NVM_CMD_ERASE_WRITE_APP_PAGE_gc (0x25<<0) // Erase-and-write Application Section page
-#define NVM_CMD_ERASE_FLASH_BUFFER_gc (0x26<<0) // Erase/flush Flash page buffer
-#define NVM_CMD_ERASE_BOOT_PAGE_gc (0x2A<<0) // Erase Boot Section page
-#define NVM_CMD_WRITE_BOOT_PAGE_gc (0x2C<<0) // Write Boot Section page
-#define NVM_CMD_ERASE_WRITE_BOOT_PAGE_gc (0x2D<<0) // Erase-and-write Boot Section page
-#define NVM_CMD_ERASE_EEPROM_gc (0x30<<0) // Erase EEPROM
-#define NVM_CMD_ERASE_EEPROM_PAGE_gc (0x32<<0) // Erase EEPROM page
-#define NVM_CMD_LOAD_EEPROM_BUFFER_gc (0x33<<0) // Load EEPROM page buffer
-#define NVM_CMD_WRITE_EEPROM_PAGE_gc (0x34<<0) // Write EEPROM page
-#define NVM_CMD_ERASE_WRITE_EEPROM_PAGE_gc (0x35<<0) // Erase-and-write EEPROM page
-#define NVM_CMD_ERASE_EEPROM_BUFFER_gc (0x36<<0) // Erase/flush EEPROM page buffer
-#define NVM_CMD_APP_CRC_gc (0x38<<0) // Generate Application section CRC
-#define NVM_CMD_BOOT_CRC_gc (0x39<<0) // Generate Boot Section CRC
-#define NVM_CMD_FLASH_RANGE_CRC_gc (0x3A<<0) // Generate Flash Range CRC
-#define CCP_SPM_gc (0x9D<<0) // SPM Instruction Protection
-#define CCP_IOREG_gc (0xD8<<0) // IO Register Protection
-
-
-
-; ---
-; This routine reads a byte from flash given by the address in
-; R25:R24:R23:R22.
-;
-; Input:
-; R25:R24:R23:R22.
-;
-; Returns:
-; R24 - Read byte.
-; ---
-
-.section .text
-.global SP_ReadByte
-
-SP_ReadByte:
- in r19, RAMPZ ; Save RAMPZ.
- out RAMPZ, r24 ; Load RAMPZ with the MSB of the address.
- movw ZL, r22 ; Move the low bytes to the Z pointer
- elpm r24, Z ; Extended load byte from address pointed to by Z.
- out RAMPZ, r19 ; Restore RAMPZ register.
- ret
-
-
-
-; ---
-; This routine reads a word from flash given by the address in
-; R25:R24:R23:R22.
-;
-; Input:
-; R25:R24:R23:R22.
-;
-; Returns:
-; R25:R24 - Read word.
-; ---
-
-.section .text
-.global SP_ReadWord
-
-SP_ReadWord:
- in r19, RAMPZ ; Save RAMPZ.
- out RAMPZ, r24 ; Load RAMPZ with the MSB of the address.
- movw ZL, r22 ; Move the low bytes to the Z pointer
- elpm r24, Z+ ; Extended load byte from address pointed to by Z.
- elpm r25, Z ; Extended load byte from address pointed to by Z.
- out RAMPZ, r19 ; Restore RAMPZ register.
- ret
-
-
-
-; ---
-; This routine reads the calibration byte given by the index in R24.
-;
-; Input:
-; R24 - Byte index.
-;
-; Returns:
-; R24 - Calibration byte.
-; ---
-
-.section .text
-.global SP_ReadCalibrationByte
-
-SP_ReadCalibrationByte:
- ldi r20, NVM_CMD_READ_CALIB_ROW_gc ; Prepare NVM command in R20.
- rjmp SP_CommonLPM ; Jump to common LPM code.
-
-
-
-; ---
-; This routine reads the user signature byte given by the index in R25:R24.
-;
-; Input:
-; R25:R24 - Byte index.
-;
-; Returns:
-; R24 - Signature byte.
-; ---
-
-.section .text
-.global SP_ReadUserSignatureByte
-
-SP_ReadUserSignatureByte:
- ldi r20, NVM_CMD_READ_USER_SIG_ROW_gc ; Prepare NVM command in R20.
- rjmp SP_CommonLPM ; Jump to common LPM code.
-
-
-
-; ---
-; This routine reads the fuse byte given by the index in R24.
-;
-; Input:
-; R24 - Byte index.
-;
-; Returns:
-; R24 - Fuse byte.
-; ---
-
-.section .text
-.global SP_ReadFuseByte
-
-SP_ReadFuseByte:
- sts NVM_ADDR0, r24 ; Load fuse byte index into NVM Address Register 0.
- clr r24 ; Prepare a zero.
- sts NVM_ADDR1, r24 ; Load zero into NVM Address Register 1.
- sts NVM_ADDR2, r24 ; Load zero into NVM Address Register 2.
- ldi r20, NVM_CMD_READ_FUSES_gc ; Prepare NVM command in R20.
- rcall SP_CommonCMD ; Jump to common NVM Action code.
- movw r24, r22 ; Move low byte to 1 byte return address.
- ret
-
-
-
-; ---
-; This routine sets the lock bits from R24. Note that unlocking is only
-; possible by doing a full chip erase, not available from software.
-;
-; Input:
-; R24 - Lock bits.
-;
-; Returns:
-; Nothing.
-; ---
-
-.section .text
-.global SP_WriteLockBits
-
-SP_WriteLockBits:
- sts NVM_DATA0, r24 ; Load lock bits into NVM Data Register 0.
- ldi r20, NVM_CMD_WRITE_LOCK_BITS_gc ; Prepare NVM command in R20.
- rjmp SP_CommonCMD ; Jump to common NVM Action code.
-
-
-
-; ---
-; This routine reads the lock bits.
-;
-; Input:
-; Nothing.
-;
-; Returns:
-; R24 - Lock bits.
-; ---
-
-.section .text
-.global SP_ReadLockBits
-
-SP_ReadLockBits:
- lds r24, NVM_LOCKBITS ; Read IO-mapped lock bits.
- ret
-
-
-
-; ---
-; This routine erases the user signature row.
-;
-; Input:
-; Nothing.
-;
-; Returns:
-; Nothing.
-; ---
-
-.section .text
-.global SP_EraseUserSignatureRow
-
-SP_EraseUserSignatureRow:
- in r19, RAMPZ ; Save RAMPZ, which is restored in SP_CommonSPM.
- ldi r20, NVM_CMD_ERASE_USER_SIG_ROW_gc ; Prepare NVM command in R20.
- jmp SP_CommonSPM ; Jump to common SPM code.
-
-
-
-; ---
-; This routine writes the flash buffer to the user signature row.
-;
-; Input:
-; Nothing.
-;
-; Returns:
-; Nothing.
-; ---
-
-.section .text
-.global SP_WriteUserSignatureRow
-
-SP_WriteUserSignatureRow:
- in r19, RAMPZ ; Save RAMPZ, which is restored in SP_CommonSPM.
- ldi r20, NVM_CMD_WRITE_USER_SIG_ROW_gc ; Prepare NVM command in R20.
- jmp SP_CommonSPM ; Jump to common SPM code.
-
-
-
-; ---
-; This routine erases the entire application section.
-;
-; Input:
-; Nothing.
-;
-; Returns:
-; Nothing.
-; ---
-
-.section .text
-.global SP_EraseApplicationSection
-
-SP_EraseApplicationSection:
- in r19, RAMPZ ; Save RAMPZ, which is restored in SP_CommonSPM.
- clr r24 ; Prepare a zero.
- clr r25
- out RAMPZ, r24 ; Point into Application area.
- ldi r20, NVM_CMD_ERASE_APP_gc ; Prepare NVM command in R20.
- jmp SP_CommonSPM ; Jump to common SPM code.
-
-
-
-; ---
-; This routine erases the page at address R25:R24:R23:R22 in the application
-; section. The address can point anywhere inside the page.
-;
-; Input:
-; R25:R24:R23:R22 - Byte address into Flash page.
-;
-; Returns:
-; Nothing.
-; ---
-
-.section .text
-.global SP_EraseApplicationPage
-
-SP_EraseApplicationPage:
- in r19, RAMPZ ; Save RAMPZ, which is restored in SP_CommonSPM.
- out RAMPZ, r24 ; Load RAMPZ with the MSB of the address.
- movw r24, r22 ; Move low bytes for ZH:ZL to R25:R24
- ldi r20, NVM_CMD_ERASE_APP_PAGE_gc ; Prepare NVM command in R20.
- jmp SP_CommonSPM ; Jump to common SPM code.
-
-
-
-; ---
-; This routine writes the word from R23:R22 into the Flash page buffer at
-; address R25:R24.
-;
-; Input:
-; R25:R24 - Byte address into Flash page.
-; R23:R22 - Word to write.
-;
-; Returns:
-; Nothing.
-; ---
-
-.section .text
-.global SP_LoadFlashWord
-
-SP_LoadFlashWord:
- in r19, RAMPZ ; Save RAMPZ, which is restored in SP_CommonSPM.
- movw r0, r22 ; Prepare flash word in R1:R0.
- ldi r20, NVM_CMD_LOAD_FLASH_BUFFER_gc ; Prepare NVM command in R20.
- jmp SP_CommonSPM ; Jump to common SPM code.
-
-
-
-; ---
-; This routine writes an entire page from the SRAM buffer at
-; address R25:R24 into the Flash page buffer.
-;
-; Note that you must define "-Wl,--section-start=.BOOT=0x020000" for the
-; linker to place this function in the boot section with the correct address.
-;
-; Input:
-; R25:R24 - 16-bit pointer to SRAM buffer.
-;
-; Returns:
-; Nothing.
-; ---
-
-.section .text
-.global SP_LoadFlashPage
-
-SP_LoadFlashPage:
- clr ZL ; Clear low byte of Z, to indicate start of page.
- clr ZH ; Clear high byte of Z, to indicate start of page.
-
- out RAMPX, r1 ; Clear RAMPX pointer.
- movw XL, r24 ; Load X with data buffer address.
-
- ldi r20, NVM_CMD_LOAD_FLASH_BUFFER_gc ; Prepare NVM command code in R20.
- sts NVM_CMD, r20 ; Load it into NVM command register.
-
-#if APP_SECTION_PAGE_SIZE > 512
- ldi r22, ((APP_SECTION_PAGE_SIZE/2) >> 8)
-#endif
-
- ldi r21, ((APP_SECTION_PAGE_SIZE/2)&0xFF) ; Load R21 with page word count.
- ldi r18, CCP_SPM_gc ; Prepare Protect SPM signature in R16.
-
-SP_LoadFlashPage_1:
- ld r0, X+ ; Load low byte from buffer into R0.
- ld r1, X+ ; Load high byte from buffer into R1.
- sts CCP, r18 ; Enable SPM operation (this disables interrupts for 4 cycles).
- spm ; Self-program.
- adiw ZL, 2 ; Move Z to next Flash word.
-
-#if APP_SECTION_PAGE_SIZE > 512
- subi r21, 1 ; Decrement word count.
- sbci r22, 0
-#else
- dec r21 ; Decrement word count.
-#endif
-
- brne SP_LoadFlashPage_1 ; Repeat until word cont is zero.
-
- clr r1 ; Clear R1 for GCC _zero_reg_ to function properly.
- ret
-
-
-
-; ---
-; This routine reads an entire Flash page from address R23:R22:R21:R20 into the
-; SRAM buffer at address R25:R24.
-;
-;
-; Input:
-; R23:R22:R21:R20 - Flash byte address.
-; R25:R24 - 16-bit pointer to SRAM buffer.
-;
-; Returns:
-; Nothing.
-; ---
-
-.section .text
-.global SP_ReadFlashPage
-
-SP_ReadFlashPage:
-
- in r19, RAMPZ ; Save RAMPZ during assembly.
- out RAMPZ, r22 ; Load RAMPZ with MSB of address
- movw ZL, r20 ; Load Z with Flash address.
-
- out RAMPX, r1 ; Load RAMPX with data pointer
- movw XL, r24 ; Load X with data buffer address.
-
- ldi r20, NVM_CMD_NO_OPERATION_gc ; Prepare NVM command code in R20.
- sts NVM_CMD, r20 ; Set NVM command to No Operation so that LPM reads Flash.
-
-#if APP_SECTION_PAGE_SIZE > 512
- ldi r22, ((APP_SECTION_PAGE_SIZE/2) >> 8) ; Load R22 with byte cont high if flash page is large.
-#endif
-
- ldi r21, ((APP_SECTION_PAGE_SIZE)&0xFF) ; Load R21 with byte count.
-
-SP_ReadFlashPage_1:
- elpm r24, Z+ ; Load Flash bytes into R18:r19
- elpm r25, Z+
- st X+, r24 ; Write bytes to buffer.
- st X+, r25
-
-#if APP_SECTION_PAGE_SIZE > 512
- subi r21, 1 ; Decrement word count.
- sbci r22, 0
-#else
- dec r21 ; Decrement word count.
-#endif
-
- brne SP_ReadFlashPage_1 ; Repeat until byte count is zero.
-
- out RAMPZ, r19
- ret
-
-
-
-; ---
-; This routine writes the page buffer to the Flash page at address R25:R24:R23:R22
-; in the application section. The address can point anywhere inside the page.
-;
-; Input:
-; R25:R24:R23:R22 - Byte address into Flash page.
-;
-; Returns:
-; Nothing.
-; ---
-
-.section .text
-.global SP_WriteApplicationPage
-
-SP_WriteApplicationPage:
- in r19, RAMPZ ; Save RAMPZ, which is restored in SP_CommonSPM.
- out RAMPZ, r24 ; Load RAMPZ with the MSB of the address.
- movw r24, r22 ; Move low bytes of address to ZH:ZL from R23:R22
- ldi r20, NVM_CMD_WRITE_APP_PAGE_gc ; Prepare NVM command in R20.
- jmp SP_CommonSPM ; Jump to common SPM code.
-
-
-
-; ---
-; This routine erases first and then writes the page buffer to the
-; Flash page at address R25:R24:R23:R22 in the application section. The address
-; can point anywhere inside the page.
-;
-; Input:
-; R25:R24:R23:R22 - Byte address into Flash page.
-;
-; Returns:
-; Nothing.
-; ---
-
-.section .text
-.global SP_EraseWriteApplicationPage
-
-SP_EraseWriteApplicationPage:
- in r19, RAMPZ ; Save RAMPZ, which is restored in SP_CommonSPM.
- out RAMPZ, r24 ; Load RAMPZ with the MSB of the address.
- movw r24, r22 ; Move low bytes of address to ZH:ZL from R23:R22
- ldi r20, NVM_CMD_ERASE_WRITE_APP_PAGE_gc ; Prepare NVM command in R20.
- jmp SP_CommonSPM ; Jump to common SPM code.
-
-
-
-; ---
-; This routine flushes the Flash page buffer.
-;
-; Input:
-; Nothing.
-;
-; Returns:
-; Nothing.
-; ---
-
-.section .text
-.global SP_EraseFlashBuffer
-
-SP_EraseFlashBuffer:
- in r19, RAMPZ ; Save RAMPZ, which is restored in SP_CommonSPM.
- ldi r20, NVM_CMD_ERASE_FLASH_BUFFER_gc ; Prepare NVM command in R20.
- jmp SP_CommonSPM ; Jump to common SPM code.
-
-
-
-; ---
-; This routine erases the page at address R25:R24:R23:R22 in the Boot section. The
-; address can point anywhere inside the page.
-;
-; Input:
-; R25:R24:R23:R22 - Byte address into Flash page.
-;
-; Returns:
-; Nothing.
-; ---
-
-.section .text
-.global SP_EraseBootPage
-
-SP_EraseBootPage:
- in r19, RAMPZ ; Save RAMPZ, which is restored in SP_CommonSPM.
- out RAMPZ, r24 ; Load RAMPZ with the MSB of the address.
- movw r24, r22 ; Move low bytes of address to ZH:ZL from R23:R22
- ldi r20, NVM_CMD_ERASE_BOOT_PAGE_gc ; Prepare NVM command in R20.
- jmp SP_CommonSPM ; Jump to common SPM code.
-
-
-
-; ---
-; This routine writes the page buffer to the Flash page at address R25:R24:R23:R22
-; in the BOOT section. The address can point anywhere inside the page.
-;
-; Input:
-; R25:R24:R23:R22 - Byte address into Flash page.
-;
-; Returns:
-; Nothing.
-; ---
-
-.section .text
-.global SP_WriteBootPage
-
-SP_WriteBootPage:
- in r19, RAMPZ ; Save RAMPZ, which is restored in SP_CommonSPM.
- out RAMPZ, r24 ; Load RAMPZ with the MSB of the address.
- movw r24, r22 ; Move low bytes of address to ZH:ZL from R23:R22
- ldi r20, NVM_CMD_WRITE_BOOT_PAGE_gc ; Prepare NVM command in R20.
- jmp SP_CommonSPM ; Jump to common SPM code.
-
-
-
-; ---
-; This routine erases first and then writes the page buffer to the
-; Flash page at address R25:R24:R23:R22 in the Boot section. The address
-; can point anywhere inside the page.
-;
-; Input:
-; R25:R24:R23:R22 - Byte address into Flash page.
-;
-; Returns:
-; Nothing.
-; ---
-
-.section .text
-.global SP_EraseWriteBootPage
-
-SP_EraseWriteBootPage:
- in r19, RAMPZ ; Save RAMPZ, which is restored in SP_CommonSPM.
- out RAMPZ, r24 ; Load RAMPZ with the MSB of the address.
- movw r24, r22 ; Move low bytes of address to ZH:ZL from R23:R22
- ldi r20, NVM_CMD_ERASE_WRITE_BOOT_PAGE_gc ; Prepare NVM command in R20.
- jmp SP_CommonSPM ; Jump to common SPM code.
-
-
-
-; ---
-; This routine calculates a CRC for the application section.
-;
-; Input:
-; Nothing.
-;
-; Returns:
-; R25:R24:R23:R22 - 32-bit CRC result (actually only 24-bit used).
-; ---
-
-.section .text
-.global SP_ApplicationCRC
-
-SP_ApplicationCRC:
- ldi r20, NVM_CMD_APP_CRC_gc ; Prepare NVM command in R20.
- rjmp SP_CommonCMD ; Jump to common NVM Action code.
-
-
-
-; ---
-; This routine calculates a CRC for the Boot section.
-;
-; Input:
-; Nothing.
-;
-; Returns:
-; R25:R24:R23:R22 - 32-bit CRC result (actually only 24-bit used).
-; ---
-
-.section .text
-.global SP_BootCRC
-
-SP_BootCRC:
- ldi r20, NVM_CMD_BOOT_CRC_gc ; Prepare NVM command in R20.
- rjmp SP_CommonCMD ; Jump to common NVM Action code.
-
-
-
-; ---
-; This routine locks all further access to SPM operations until next reset.
-;
-; Input:
-; Nothing.
-;
-; Returns:
-; Nothing.
-; ---
-
-.section .text
-.global SP_LockSPM
-
-SP_LockSPM:
- ldi r18, CCP_IOREG_gc ; Prepare Protect IO-register signature in R18.
- sts CCP, r18 ; Enable IO-register operation (this disables interrupts for 4 cycles).
- ldi r18, NVM_SPMLOCK_bm ; Prepare bitmask for locking SPM into R18.
- sts NVM_CTRLB, r18 ; Load bitmask into NVM Control Register B, which locks SPM.
- ret
-
-
-
-; ---
-; This routine wait for the SPM to finish and clears the command register.
-;
-; Note that this routine is blocking, and will halt any execution until the SPM
-; is finished.
-;
-; Input:
-; Nothing.
-;
-; Returns:
-; Nothing.
-; ---
-
-.section .text
-.global SP_WaitForSPM
-
-SP_WaitForSPM:
- lds r18, NVM_STATUS ; Load the NVM Status register.
- sbrc r18, NVM_NVMBUSY_bp ; Check if bit is cleared.
- rjmp SP_WaitForSPM ; Repeat check if bit is not cleared.
- clr r18
- sts NVM_CMD, r18 ; Clear up command register to NO_OPERATION.
- ret
-
-
-
-; ---
-; This routine is called by several other routines, and contains common code
-; for executing an NVM command, including the return statement itself.
-;
-; If the operation (NVM command) requires the NVM Address registers to be
-; prepared, this must be done before jumping to this routine.
-;
-; Note that R25:R24:R23:R22 is used for returning results, even if the
-; C-domain calling function only expects a single byte or even void.
-;
-; Input:
-; R20 - NVM Command code.
-;
-; Returns:
-; R25:R24:R23:R22 - 32-bit result from NVM operation.
-; ---
-
-.section .text
-
-SP_CommonCMD:
- sts NVM_CMD, r20 ; Load command into NVM Command register.
- ldi r18, CCP_IOREG_gc ; Prepare Protect IO-register signature in R18.
- ldi r19, NVM_CMDEX_bm ; Prepare bitmask for setting NVM Command Execute bit into R19.
- sts CCP, r18 ; Enable IO-register operation (this disables interrupts for 4 cycles).
- sts NVM_CTRLA, r19 ; Load bitmask into NVM Control Register A, which executes the command.
- lds r22, NVM_DATA0 ; Load NVM Data Register 0 into R22.
- lds r23, NVM_DATA1 ; Load NVM Data Register 1 into R23.
- lds r24, NVM_DATA2 ; Load NVM Data Register 2 into R24.
- clr r25 ; Clear R25 in order to return a clean 32-bit value.
- ret
-
-
-
-; ---
-; This routine is called by several other routines, and contains common code
-; for executing an LPM command, including the return statement itself.
-;
-; Note that R24 is used for returning results, even if the
-; C-domain calling function expects a void.
-;
-; Input:
-; R25:R24 - Low bytes of Z pointer.
-; R20 - NVM Command code.
-;
-; Returns:
-; R24 - Result from LPM operation.
-; ---
-
-.section .text
-
-SP_CommonLPM:
- movw ZL, r24 ; Load index into Z.
- sts NVM_CMD, r20 ; Load prepared command into NVM Command register.
- lpm r24,Z
- ret
-
-
-
-; ---
-; This routine is called by several other routines, and contains common code
-; for executing an SPM command, including the return statement itself.
-;
-; If the operation (SPM command) requires the R1:R0 registers to be
-; prepared, this must be done before jumping to this routine.
-;
-; Note that you must define "-Wl,--section-start=.BOOT=0x020000" for the
-; linker to place this function in the boot section with the correct address.
-;
-; Input:
-; R1:R0 - Optional input to SPM command.
-; R25:R24 - Low bytes of Z pointer.
-; R20 - NVM Command code.
-;
-; Returns:
-; Nothing.
-; ---
-
-.section .text
-
-SP_CommonSPM:
- movw ZL, r24 ; Load R25:R24 into Z.
- sts NVM_CMD, r20 ; Load prepared command into NVM Command register.
- ldi r18, CCP_SPM_gc ; Prepare Protect SPM signature in R18
- sts CCP, r18 ; Enable SPM operation (this disables interrupts for 4 cycles).
- spm ; Self-program.
- clr r1 ; Clear R1 for GCC _zero_reg_ to function properly.
- out RAMPZ, r19 ; Restore RAMPZ register.
- ret
-
-
-; END OF FILE
-
+++ /dev/null
-/* This file has been prepared for Doxygen automatic documentation generation.*/
-/*! \file *********************************************************************
- *
- * \brief XMEGA Self-programming driver header file.
- *
- * This file contains the function prototypes for the
- * XMEGA Self-programming driver.
- * If any SPM instructions are used, the linker file must define
- * a segment named BOOT which must be located in the device boot section.
- *
- *
- * None of these functions clean up the NVM Command Register after use.
- * It is therefore important to write NVMCMD_NO_OPERATION (0x00) to this
- * register when you are finished using any of the functions in this
- * driver.
- *
- * For all functions, it is important that no interrupt handlers perform
- * any NVM operations. The user must implement a scheme for mutually
- * exclusive access to the NVM. However, the 4-cycle timeout will work
- * fine, since writing to the Configuration Change Protection register
- * (CCP) automatically disables interrupts for 4 instruction cycles.
- *
- * \par Application note:
- * AVR1316: XMEGA Self-programming
- *
- * \par Documentation
- * For comprehensive code documentation, supported compilers, compiler
- * settings and supported devices see readme.html
- *
- * \author
- * Atmel Corporation: http://www.atmel.com \n
- * Support email: avr@atmel.com
- *
- * $Revision: 1691 $
- * $Date: 2008-07-29 13:25:40 +0200 (ti, 29 jul 2008) $ \n
- *
- * Copyright (c) 2008, Atmel Corporation All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions are met:
- *
- * 1. Redistributions of source code must retain the above copyright notice,
- * this list of conditions and the following disclaimer.
- *
- * 2. Redistributions in binary form must reproduce the above copyright notice,
- * this list of conditions and the following disclaimer in the documentation
- * and/or other materials provided with the distribution.
- *
- * 3. The name of ATMEL may not be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY ATMEL "AS IS" AND ANY EXPRESS OR IMPLIED
- * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
- * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE EXPRESSLY AND
- * SPECIFICALLY DISCLAIMED. IN NO EVENT SHALL ATMEL BE LIABLE FOR ANY DIRECT,
- * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
- * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
- * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
- * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
- * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
- *****************************************************************************/
-#ifndef SP_DRIVER_H
-#define SP_DRIVER_H
-
-//#include "avr_compiler.h"
-//#include "Flash_Defines.h"
-
-
-
-/* Define the size of the flash page if not defined in the header files. */
-#ifndef APP_SECTION_PAGE_SIZE
-#error APP_SECTION_PAGE_SIZE must be defined if not defined in header files.
-//#define APP_SECTION_PAGE_SIZE 512
-#endif /*FLASH_PAGE_SIZE*/
-
-// Define the Start of the application table if not defined in the header files.
-#ifndef APPTABLE_SECTION_START
-#error APPTABLE_SECTION_START must be defined if not defined in header files.
-//#define APPTABLE_SECTION_START 0x01E000 //APPTABLE address for ATxmega128A1
-#endif /*APPTABLE_SECTION_START*/
-
-/*! \brief Read a byte from flash.
- *
- * This function reads one byte from the flash.
- *
- * \note Both IAR and GCC have functions to do this, but
- * we include the fucntions for easier use.
- *
- * \param address Address to the location of the byte to read.
- *
- * \retval Byte read from flash.
- */
-uint8_t SP_ReadByte(uint32_t address);
-
-/*! \brief Read a word from flash.
- *
- * This function reads one word from the flash.
- *
- * \note Both IAR and GCC have functions to do this automatically, but
- * we include the fucntions for easier use.
- *
- * \param address Address to the location of the word to read.
- *
- * \retval word read from flash.
- */
-uint16_t SP_ReadWord(uint32_t address);
-
-/*! \brief Read calibration byte at given index.
- *
- * This function reads one calibration byte from the Calibration signature row.
- *
- * \param index Index of the byte in the calibration signature row.
- *
- * \retval Calibration byte
- */
-uint8_t SP_ReadCalibrationByte(uint8_t index);
-
-/*! \brief Read fuse byte from given index.
- *
- * This function reads the fuse byte at the given index.
- *
- * \param index Index of the fuse byte.
- *
- * \retval Fuse byte
- */
-uint8_t SP_ReadFuseByte(uint8_t index);
-
-/*! \brief Write lock bits.
- *
- * This function changes the lock bits.
- *
- * \note It is only possible to change the lock bits to a higher security l
- * evel.
- *
- * \param data The new value of the lock bits.
- */
-void SP_WriteLockBits(uint8_t data);
-
-/*! \brief Read lock bits.
- *
- * This function reads the lock bits.
- *
- * \retval Lock bits
- */
-uint8_t SP_ReadLockBits(void);
-
-/*! \brief Read user signature at given index.
- *
- * This function reads one byte from the user signature row.
- *
- * \param index Index of the byte in the user signature row.
- *
- * \retval User signature byte
- */
-uint8_t SP_ReadUserSignatureByte(uint16_t index);
-
-/*! \brief Erase user signature row.
- *
- * This function erase the entire user signature row.
- */
-void SP_EraseUserSignatureRow(void);
-
-/*! \brief Write user signature row.
- *
- * This function write the flash buffer in the user signature row.
- */
-void SP_WriteUserSignatureRow(void);
-
-/*! \brief Erase entire application section.
- *
- * This function erases the entire application and application table section
- *
- * \note If the lock bits is set to not allow spm in the application or
- * application table section the erase is not done.
- */
-void SP_EraseApplicationSection(void);
-
-/*! \brief Erase page at byte address in application or application table
- * section.
- *
- * This function erase one page given by the byte address.
- *
- * \param address Byte address for flash page.
- */
-void SP_EraseApplicationPage(uint32_t address);
-
-/*! \brief Erase and write page buffer to application or application table
- * section at byte address.
- *
- * This function does a combined erase and write to a flash page in the
- * application or application table section.
- *
- * \param address Byte address for flash page.
- */
-void SP_EraseWriteApplicationPage(uint32_t address);
-
-/*! \brief Write page buffer to application or application table section at
- * byte address.
- *
- * This function writes the Flash page buffer to a page in the application or
- * application table section given by the byte address.
- *
- * \note The page that is written to must be erased before it is written to.
- *
- * \param address Byte address for flash page.
- */
-void SP_WriteApplicationPage(uint32_t address);
-
-/*! \brief Load one word into Flash page buffer.
- *
- * This function Loads one word into the Flash page buffer.
- *
- * \param address Position in inside the flash page buffer.
- * \param data Value to be put into the buffer.
- */
-void SP_LoadFlashWord(uint16_t address, uint16_t data);
-
-/*! \brief Load entire page from SRAM buffer into Flash page buffer.
- *
- * This function load an entire page from SRAM.
- *
- * \param data Pointer to the data to put in buffer.
- *
- * \note The __near keyword limits the pointer to two bytes which means that
- * only data up to 64K (internal SRAM) can be used.
- */
-void SP_LoadFlashPage(const uint8_t * data);
-
-/*! \brief Read entire Flash page into SRAM buffer.
- *
- * This function reads an entire flash page and puts it to SRAM.
- *
- * \param data Pointer to where to store the data.
- * \param address Address to page to read from flash.
- */
-void SP_ReadFlashPage(const uint8_t * data, uint32_t address);
-
-/*! \brief Flush Flash page buffer.
- *
- * This function flush the Flash page buffer.
- */
-void SP_EraseFlashBuffer(void);
-
-/*! \brief Erase page at byte address in boot section.
- *
- * This function erase one page given by the byte address.
- *
- * \param address Byte address for flash page.
- */
-void SP_EraseBootPage(uint32_t address);
-
-/*! \brief Erase and write page buffer to boot section at byte address.
- *
- * This function does a combined erase and write to a flash page in the boot
- * section.
- *
- * \param address Byte address for flash page.
- */
-void SP_EraseWriteBootPage(uint32_t address);
-
-/*! \brief Write page buffer to boot section at byte address.
- *
- * This function writes the Flash page buffer to a page in the boot section
- * given by the byte address.
- *
- * \note The page that is written to must be erased before it is written to.
- *
- * \param address Byte address for flash page.
- */
-void SP_WriteBootPage(uint32_t address);
-
-/*! \brief Generate CRC from application section.
- *
- * \retval 24-bit CRC value
- */
-uint32_t SP_ApplicationCRC(void);
-
-/*! \brief Generate CRC from boot section.
- *
- * \retval 24-bit CRC value
- */
-uint32_t SP_BootCRC(void);
-
-/*! \brief Lock SPM instruction.
- *
- * This function locks the SPM instruction, and will disable the use of
- * SPM until the next reset occurs.
- */
-void SP_LockSPM(void);
-
-/*! \brief Wait for SPM to finish.
- *
- * This routine waits for the SPM to finish and clears the command register.
- */
-void SP_WaitForSPM(void);
-
-#endif /* SP_DRIVER_H */
+++ /dev/null
-/************************************************************************/
-/* XBoot Extensible AVR Bootloader */
-/* */
-/* UART Module */
-/* */
-/* uart.c */
-/* */
-/* Alex Forencich <alex@alexforencich.com> */
-/* */
-/* Copyright (c) 2010 Alex Forencich */
-/* */
-/* Permission is hereby granted, free of charge, to any person */
-/* obtaining a copy of this software and associated documentation */
-/* files(the "Software"), to deal in the Software without restriction, */
-/* including without limitation the rights to use, copy, modify, merge, */
-/* publish, distribute, sublicense, and/or sell copies of the Software, */
-/* and to permit persons to whom the Software is furnished to do so, */
-/* subject to the following conditions: */
-/* */
-/* The above copyright notice and this permission notice shall be */
-/* included in all copies or substantial portions of the Software. */
-/* */
-/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT 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 "uart.h"
-#include "xboot.h"
-
-
-bool uart_has_char() {return UART_DEVICE.STATUS & USART_RXCIF_bm;}
-uint8_t uart_recv_char() {return UART_DEVICE.DATA;}
-
-
-void uart_send_char_blocking(uint8_t c) {
- UART_DEVICE.DATA = c;
- while (!(UART_DEVICE.STATUS & USART_TXCIF_bm)) continue;
- UART_DEVICE.STATUS |= USART_TXCIF_bm;
-}
-
-
-void uart_init() {
- UART_PORT.DIRSET = 1 << UART_TX_PIN;
- UART_DEVICE.BAUDCTRLA = UART_BSEL_VALUE & USART_BSEL_gm;
- UART_DEVICE.BAUDCTRLB =
- ((UART_BSCALE_VALUE << USART_BSCALE_gp) & USART_BSCALE_gm) |
- ((UART_BSEL_VALUE >> 8) & ~USART_BSCALE_gm);
-
-#if UART_CLK2X
- UART_DEVICE.CTRLB = USART_RXEN_bm | USART_CLK2X_bm | USART_TXEN_bm;
-#else
- UART_DEVICE.CTRLB = USART_RXEN_bm | USART_TXEN_bm;
-#endif // UART_CLK2X
-
- PORTC.OUTCLR = 1 << 4; // CTS Lo (enable)
- PORTC.DIRSET = 1 << 4; // CTS Output
-}
-
-
-void uart_deinit() {
- UART_DEVICE.CTRLB = 0;
- UART_DEVICE.BAUDCTRLA = 0;
- UART_DEVICE.BAUDCTRLB = 0;
- UART_PORT.DIRCLR = 1 << UART_TX_PIN;
-}
+++ /dev/null
-/************************************************************************/
-/* XBoot Extensible AVR Bootloader */
-/* */
-/* UART Module */
-/* */
-/* uart.h */
-/* */
-/* Alex Forencich <alex@alexforencich.com> */
-/* */
-/* Copyright (c) 2010 Alex Forencich */
-/* */
-/* Permission is hereby granted, free of charge, to any person */
-/* obtaining a copy of this software and associated documentation */
-/* files(the "Software"), to deal in the Software without restriction, */
-/* including without limitation the rights to use, copy, modify, merge, */
-/* publish, distribute, sublicense, and/or sell copies of the Software, */
-/* and to permit persons to whom the Software is furnished to do so, */
-/* subject to the following conditions: */
-/* */
-/* The above copyright notice and this permission notice shall be */
-/* included in all copies or substantial portions of the Software. */
-/* */
-/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT 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. */
-/* */
-/************************************************************************/
-
-#pragma once
-
-#include <stdbool.h>
-#include <stdint.h>
-
-bool uart_has_char();
-uint8_t uart_recv_char();
-void uart_send_char_blocking(uint8_t c);
-void uart_init(void);
-void uart_deinit(void);
+++ /dev/null
-/************************************************************************/
-/* XBoot Extensible AVR Bootloader */
-/* */
-/* Watchdog Module */
-/* */
-/* watchdog.c */
-/* */
-/* Alex Forencich <alex@alexforencich.com> */
-/* */
-/* Copyright (c) 2010 Alex Forencich */
-/* */
-/* Permission is hereby granted, free of charge, to any person */
-/* obtaining a copy of this software and associated documentation */
-/* files(the "Software"), to deal in the Software without restriction, */
-/* including without limitation the rights to use, copy, modify, merge, */
-/* publish, distribute, sublicense, and/or sell copies of the Software, */
-/* and to permit persons to whom the Software is furnished to do so, */
-/* subject to the following conditions: */
-/* */
-/* The above copyright notice and this permission notice shall be */
-/* included in all copies or substantial portions of the Software. */
-/* */
-/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT 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 "watchdog.h"
-
-
-void WDT_EnableAndSetTimeout() {
- uint8_t temp = WDT_ENABLE_bm | WDT_CEN_bm | WATCHDOG_TIMEOUT;
- CCP = CCP_IOREG_gc;
- WDT.CTRL = temp;
-
- // Wait for WD to synchronize with new settings.
- while (WDT_IsSyncBusy()) continue;
-}
-
-
-void WDT_Disable() {
- uint8_t temp = (WDT.CTRL & ~WDT_ENABLE_bm) | WDT_CEN_bm;
- CCP = CCP_IOREG_gc;
- WDT.CTRL = temp;
-}
+++ /dev/null
-/************************************************************************/
-/* XBoot Extensible AVR Bootloader */
-/* */
-/* Watchdog Module */
-/* */
-/* watchdog.c */
-/* */
-/* Alex Forencich <alex@alexforencich.com> */
-/* */
-/* Copyright (c) 2010 Alex Forencich */
-/* */
-/* Permission is hereby granted, free of charge, to any person */
-/* obtaining a copy of this software and associated documentation */
-/* files(the "Software"), to deal in the Software without restriction, */
-/* including without limitation the rights to use, copy, modify, merge, */
-/* publish, distribute, sublicense, and/or sell copies of the Software, */
-/* and to permit persons to whom the Software is furnished to do so, */
-/* subject to the following conditions: */
-/* */
-/* The above copyright notice and this permission notice shall be */
-/* included in all copies or substantial portions of the Software. */
-/* */
-/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT 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. */
-/* */
-/************************************************************************/
-
-#pragma once
-
-#include "xboot.h"
-
-#define WDT_IsSyncBusy() (WDT.STATUS & WDT_SYNCBUSY_bm)
-#define WDT_Reset() asm("wdr")
-
-void WDT_EnableAndSetTimeout();
-void WDT_Disable();
+++ /dev/null
-/************************************************************************/
-/* XBoot Extensible AVR Bootloader */
-/* */
-/* xboot.c */
-/* */
-/* Alex Forencich <alex@alexforencich.com> */
-/* */
-/* Copyright (c) 2010 Alex Forencich */
-/* */
-/* Permission is hereby granted, free of charge, to any person */
-/* obtaining a copy of this software and associated documentation */
-/* files(the "Software"), to deal in the Software without restriction, */
-/* including without limitation the rights to use, copy, modify, merge, */
-/* publish, distribute, sublicense, and/or sell copies of the Software, */
-/* and to permit persons to whom the Software is furnished to do so, */
-/* subject to the following conditions: */
-/* */
-/* The above copyright notice and this permission notice shall be */
-/* included in all copies or substantial portions of the Software. */
-/* */
-/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT 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 "xboot.h"
-
-#include <util/delay.h>
-
-#include <stdbool.h>
-
-
-uint8_t buffer[SPM_PAGESIZE];
-
-
-void NVM_Wait() {
- while (NVM_STATUS & NVM_NVMBUSY_bp)
- // reset watchdog while waiting for erase completion
- WDT_Reset();
-}
-
-
-int main() {
- // Initialization section
- // Entry point and communication methods are initialized here
- // --------------------------------------------------
-#ifdef USE_32MHZ_RC
-#if (F_CPU != 32000000L)
-#error F_CPU must match oscillator setting!
-#endif // F_CPU
- OSC.CTRL |= OSC_RC32MEN_bm; // turn on 32 MHz oscillator
- while (!(OSC.STATUS & OSC_RC32MRDY_bm)) continue; // wait for it to start
- CCP = CCP_IOREG_gc;
- CLK.CTRL = CLK_SCLKSEL_RC32M_gc;
-#ifdef USE_DFLL
- OSC.CTRL |= OSC_RC32KEN_bm;
- while(!(OSC.STATUS & OSC_RC32KRDY_bm));
- DFLLRC32M.CTRL = DFLL_ENABLE_bm;
-#endif // USE_DFLL
-#else // USE_32MHZ_RC
-#if (F_CPU != 2000000L)
-#error F_CPU must match oscillator setting!
-#endif // F_CPU
-#ifdef USE_DFLL
- OSC.CTRL |= OSC_RC32KEN_bm;
- while(!(OSC.STATUS & OSC_RC32KRDY_bm));
- DFLLRC2M.CTRL = DFLL_ENABLE_bm;
-#endif // USE_DFLL
-#endif // USE_32MHZ_RC
-
- // Initialize UART
- uart_init();
-
- // End initialization section
-
- uint32_t address = 0;
- uint16_t i = 0;
-
- bool in_bootloader = false;
- uint16_t j = ENTER_WAIT;
- while (!in_bootloader && 0 < j--) {
- // Check for trigger
- if (uart_has_char()) in_bootloader = uart_recv_char() == CMD_SYNC;
-
- _delay_ms(1);
- }
-
- WDT_EnableAndSetTimeout();
-
- // Main bootloader
- while (in_bootloader) {
- uint8_t val = get_char();
- WDT_Reset();
-
- // Main bootloader parser
- switch (val) {
- case CMD_CHECK_AUTOINCREMENT: send_char(REPLY_YES); break;
-
- case CMD_SET_ADDRESS:
- address = get_word();
- send_char(REPLY_ACK);
- break;
-
- case CMD_SET_EXT_ADDRESS:
- address = ((uint32_t)get_char() << 16) | get_word();
- send_char(REPLY_ACK);
- break;
-
- case CMD_CHIP_ERASE:
- // Erase the application section
- SP_EraseApplicationSection();
-
- // Wait for completion
- NVM_Wait();
-
- // Erase EEPROM
- EEPROM_erase_all();
-
- send_char(REPLY_ACK);
- break;
-
- case CMD_CHECK_BLOCK_SUPPORT:
- send_char(REPLY_YES);
- // Send block size (page size)
- send_char((SPM_PAGESIZE >> 8) & 0xFF);
- send_char(SPM_PAGESIZE & 0xFF);
- break;
-
- case CMD_BLOCK_LOAD:
- i = get_word(); // Block size
- val = get_char(); // Memory type
- send_char(BlockLoad(i, val, &address)); // Load it
- break;
-
- case CMD_BLOCK_READ:
- i = get_word(); // Block size
- val = get_char(); // Memory type
- BlockRead(i, val, &address); // Read it
- break;
-
- case CMD_READ_BYTE: {
- unsigned w = SP_ReadWord((address << 1));
-
- send_char(w >> 8);
- send_char(w);
-
- address++;
- break;
- }
-
- case CMD_WRITE_LOW_BYTE:
- i = get_char(); // get low byte
- send_char(REPLY_ACK);
- break;
-
- case CMD_WRITE_HIGH_BYTE:
- i |= (get_char() << 8); // get high byte; combine
- SP_LoadFlashWord((address << 1), i);
- address++;
- send_char(REPLY_ACK);
- break;
-
- case CMD_WRITE_PAGE:
- if (address >= (APP_SECTION_SIZE >> 1))
- send_char(REPLY_ERROR); // don't allow bootloader overwrite
-
- else {
- SP_WriteApplicationPage(address << 1);
- send_char(REPLY_ACK);
- }
- break;
-
- case CMD_WRITE_EEPROM_BYTE:
- EEPROM_write_byte(address, get_char());
- address++;
- send_char(REPLY_ACK);
- break;
-
- case CMD_READ_EEPROM_BYTE:
- send_char(EEPROM_read_byte(address));
- address++;
- break;
-
- case CMD_READ_LOW_FUSE_BITS: send_char(SP_ReadFuseByte(0)); break;
- case CMD_READ_HIGH_FUSE_BITS: send_char(SP_ReadFuseByte(1)); break;
- case CMD_READ_EXT_FUSE_BITS: send_char(SP_ReadFuseByte(2)); break;
-
- case CMD_ENTER_PROG_MODE: case CMD_LEAVE_PROG_MODE:
- send_char(REPLY_ACK);
- break;
-
- case CMD_EXIT_BOOTLOADER:
- in_bootloader = false;
- send_char(REPLY_ACK);
- break;
-
- case CMD_PROGRAMMER_TYPE: send_char('S'); break; // serial
-
- case CMD_DEVICE_CODE:
- send_char(123); // send only this device
- send_char(0); // terminator
- break;
-
- case CMD_SET_LED: case CMD_CLEAR_LED: case CMD_SET_TYPE:
- get_char(); // discard parameter
- send_char(REPLY_ACK);
- break;
-
- case CMD_PROGRAM_ID:
- send_char('X');
- send_char('B');
- send_char('o');
- send_char('o');
- send_char('t');
- send_char('+');
- send_char('+');
- break;
-
- case CMD_VERSION:
- send_char('0' + XBOOT_VERSION_MAJOR);
- send_char('0' + XBOOT_VERSION_MINOR);
- break;
-
- case CMD_READ_SIGNATURE:
- send_char(SIGNATURE_2);
- send_char(SIGNATURE_1);
- send_char(SIGNATURE_0);
- break;
-
- case CMD_CRC: {
- uint32_t start = 0;
- uint32_t length = 0;
- uint16_t crc;
-
- val = get_char();
-
- switch (val) {
- case SECTION_FLASH: length = PROGMEM_SIZE; break;
- case SECTION_APPLICATION: length = APP_SECTION_SIZE; break;
- case SECTION_BOOT:
- start = BOOT_SECTION_START;
- length = BOOT_SECTION_SIZE;
- break;
-
- default:
- send_char(REPLY_ERROR);
- continue;
- }
-
- crc = crc16_block(start, length);
-
- send_char((crc >> 8) & 0xff);
- send_char(crc & 0xff);
- break;
- }
-
- case CMD_SYNC: break; // ESC (0x1b) to sync
-
- default: // otherwise, error
- send_char(REPLY_ERROR);
- break;
- }
-
- // Wait for any lingering SPM instructions to finish
- NVM_Wait();
- }
-
- // Bootloader exit
- // Code here runs after the bootloader has exited,
- // but before the application code has started
-
- // Shut down UART
- uart_deinit();
-
- WDT_Disable();
-
- // Jump into main code
- asm("jmp 0");
-}
-
-
-uint8_t get_char() {
- while (!uart_has_char()) continue;
- return uart_recv_char();
-}
-
-
-void send_char(uint8_t c) {uart_send_char_blocking(c);}
-uint16_t get_word() {return (get_char() << 8) | get_char();}
-
-
-uint8_t BlockLoad(unsigned size, uint8_t mem, uint32_t *address) {
- WDT_Reset();
-
- // fill up buffer
- for (int i = 0; i < SPM_PAGESIZE; i++) {
- char c = 0xff;
- if (i < size) c = get_char();
- buffer[i] = c;
- }
-
- switch (mem) {
- case MEM_EEPROM:
- EEPROM_write_block(*address, buffer, size);
- *address += size;
- break;
-
- case MEM_FLASH:
- // NOTE: Flash programming, address is given in words.
- SP_LoadFlashPage(buffer);
- SP_EraseWriteApplicationPage(*address << 1);
- *address += size >> 1;
- NVM_Wait();
- break;
-
- case MEM_USERSIG:
- SP_LoadFlashPage(buffer);
- SP_EraseUserSignatureRow();
- NVM_Wait();
- SP_WriteUserSignatureRow();
- NVM_Wait();
- break;
-
- default: return REPLY_ERROR;
- }
-
- return REPLY_ACK;
-}
-
-
-
-void BlockRead(unsigned size, uint8_t mem, uint32_t *address) {
- int offset = 0;
- int size2 = size;
-
- switch (mem) {
- case MEM_EEPROM:
- EEPROM_read_block(*address, buffer, size);
- (*address) += size;
- break;
-
- case MEM_FLASH: case MEM_USERSIG: case MEM_PRODSIG: {
- *address <<= 1; // Convert address to bytes temporarily
-
- do {
- switch (mem) {
- case MEM_FLASH: buffer[offset++] = SP_ReadByte(*address); break;
-
- case MEM_USERSIG:
- buffer[offset++] = SP_ReadUserSignatureByte(*address);
- break;
-
- case MEM_PRODSIG:
- buffer[offset++] = SP_ReadCalibrationByte(*address);
- break;
- }
-
- NVM_Wait();
-
- (*address)++; // Select next word in memory.
- size--; // Subtract two bytes from number of bytes to read
- } while (size); // Repeat until all block has been read
-
- *address >>= 1; // Convert address back to Flash words again.
- break;
- }
-
- default: send_char(REPLY_ERROR); break;
- }
-
- // send bytes
- for (int i = 0; i < size2; i++)
- send_char(buffer[i]);
-}
-
-
-uint16_t crc16_block(uint32_t start, uint32_t length) {
- uint16_t crc = 0;
-
- int bc = SPM_PAGESIZE;
-
- for (; length > 0; length--) {
- if (bc == SPM_PAGESIZE) {
- SP_ReadFlashPage(buffer, start);
- start += SPM_PAGESIZE;
- bc = 0;
- }
-
- crc = _crc16_update(crc, buffer[bc]);
-
- bc++;
- }
-
- return crc;
-}
+++ /dev/null
-/************************************************************************/
-/* XBoot Extensible AVR Bootloader */
-/* */
-/* xboot.h */
-/* */
-/* Alex Forencich <alex@alexforencich.com> */
-/* */
-/* Copyright (c) 2010 Alex Forencich */
-/* */
-/* Permission is hereby granted, free of charge, to any person */
-/* obtaining a copy of this software and associated documentation */
-/* files(the "Software"), to deal in the Software without restriction, */
-/* including without limitation the rights to use, copy, modify, merge, */
-/* publish, distribute, sublicense, and/or sell copies of the Software, */
-/* and to permit persons to whom the Software is furnished to do so, */
-/* subject to the following conditions: */
-/* */
-/* The above copyright notice and this permission notice shall be */
-/* included in all copies or substantial portions of the Software. */
-/* */
-/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT 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. */
-/* */
-/************************************************************************/
-
-#pragma once
-
-#include <avr/io.h>
-#include <avr/interrupt.h>
-#include <util/crc16.h>
-
-// token pasting
-#define CAT2_int(x, y) x ## y
-#define CAT2(x, y) CAT2_int(x, y)
-#define CAT3_int(x, y, z) x ## y ## z
-#define CAT3(x, y, z) CAT3_int(x, y, z)
-
-// Version
-#define XBOOT_VERSION_MAJOR 1
-#define XBOOT_VERSION_MINOR 7
-
-// Configuration
-#define ENTER_WAIT 1000 // In ms
-#define UART_BAUD_RATE 115200
-#define UART_NUMBER 0
-#define UART_PORT_NAME C
-#define USE_AVR1008_EEPROM
-#define USE_DFLL
-#define WATCHDOG_TIMEOUT WDT_PER_1KCLK_gc
-
-// clock config
-// use 32MHz osc if makefile calls for it
-#if (F_CPU == 32000000L)
-// defaults to 2MHz RC oscillator
-// define USE_32MHZ_RC to override
-#define USE_32MHZ_RC
-#endif // F_CPU
-
-// UART RS485 Enable Output
-#define UART_EN_PORT CAT2(PORT, UART_EN_PORT_NAME)
-
-#if (UART_NUMBER == 0)
-#define UART_RX_PIN 2
-#define UART_TX_PIN 3
-#else
-#define UART_RX_PIN 6
-#define UART_TX_PIN 7
-#endif
-#define UART_PORT CAT2(PORT, UART_PORT_NAME)
-#define UART_DEVICE_PORT CAT2(UART_PORT_NAME, UART_NUMBER)
-#define UART_DEVICE CAT2(USART, UART_DEVICE_PORT)
-#define UART_DEVICE_RXC_ISR CAT3(USART, UART_DEVICE_PORT, _RXC_vect)
-#define UART_DEVICE_DRE_ISR CAT3(USART, UART_DEVICE_PORT, _DRE_vect)
-#define UART_DEVICE_TXC_ISR CAT3(USART, UART_DEVICE_PORT, _TXC_vect)
-#define UART_RX_PIN_CTRL CAT3(UART_PORT.PIN, UART_RX_PIN, CTRL)
-#define UART_TX_PIN_CTRL CAT3(UART_PORT.PIN, UART_TX_PIN, CTRL)
-
-// BAUD Rate Values
-// Known good at 2MHz
-#if (F_CPU == 2000000L) && (UART_BAUD_RATE == 19200)
-#define UART_BSEL_VALUE 12
-#define UART_BSCALE_VALUE 0
-#define UART_CLK2X 1
-#elif (F_CPU == 2000000L) && (UART_BAUD_RATE == 38400)
-#define UART_BSEL_VALUE 22
-#define UART_BSCALE_VALUE -2
-#define UART_CLK2X 1
-#elif (F_CPU == 2000000L) && (UART_BAUD_RATE == 57600)
-#define UART_BSEL_VALUE 26
-#define UART_BSCALE_VALUE -3
-#define UART_CLK2X 1
-#elif (F_CPU == 2000000L) && (UART_BAUD_RATE == 115200)
-#define UART_BSEL_VALUE 19
-#define UART_BSCALE_VALUE -4
-#define UART_CLK2X 1
-
-// Known good at 32MHz
-#elif (F_CPU == 32000000L) && (UART_BAUD_RATE == 19200)
-#define UART_BSEL_VALUE 103
-#define UART_BSCALE_VALUE 0
-#define UART_CLK2X 0
-#elif (F_CPU == 32000000L) && (UART_BAUD_RATE == 38400)
-#define UART_BSEL_VALUE 51
-#define UART_BSCALE_VALUE 0
-#define UART_CLK2X 0
-#elif (F_CPU == 32000000L) && (UART_BAUD_RATE == 57600)
-#define UART_BSEL_VALUE 34
-#define UART_BSCALE_VALUE 0
-#define UART_CLK2X 0
-#elif (F_CPU == 32000000L) && (UART_BAUD_RATE == 115200)
-#define UART_BSEL_VALUE 1047
-#define UART_BSCALE_VALUE -6
-#define UART_CLK2X 0
-
-#else // None of the above, so calculate something
-#warning Not using predefined BAUD rate, possible BAUD rate error!
-#if (F_CPU == 2000000L)
-#define UART_BSEL_VALUE ((F_CPU) / ((uint32_t)UART_BAUD_RATE * 8) - 1)
-#define UART_BSCALE_VALUE 0
-#define UART_CLK2X 1
-
-#else
-#define UART_BSEL_VALUE ((F_CPU) / ((uint32_t)UART_BAUD_RATE * 16) - 1)
-#define UART_BSCALE_VALUE 0
-#define UART_CLK2X 0
-#endif
-#endif
-
-#ifndef EEPROM_PAGE_SIZE
-#define EEPROM_PAGE_SIZE E2PAGESIZE
-#endif
-
-#ifndef EEPROM_BYTE_ADDRESS_MASK
-#if EEPROM_PAGE_SIZE == 32
-#define EEPROM_BYTE_ADDRESS_MASK 0x1f
-#elif EEPROM_PAGE_SIZE == 16
-#define EEPROM_BYTE_ADDRESS_MASK = 0x0f
-#elif EEPROM_PAGE_SIZE == 8
-#define EEPROM_BYTE_ADDRESS_MASK = 0x07
-#elif EEPROM_PAGE_SIZE == 4
-#define EEPROM_BYTE_ADDRESS_MASK = 0x03
-#else
-#error Unknown EEPROM page size! Please add new byte address value!
-#endif
-#endif
-
-// Includes
-#include "protocol.h"
-#include "sp_driver.h"
-#include "eeprom_driver.h"
-#include "uart.h"
-#include "watchdog.h"
-
-// Functions
-uint8_t get_char(void);
-void send_char(uint8_t c);
-uint16_t get_word(void);
-
-uint8_t BlockLoad(unsigned size, uint8_t mem, uint32_t *address);
-void BlockRead(unsigned size, uint8_t mem, uint32_t *address);
-
-uint16_t crc16_block(uint32_t start, uint32_t length);
+++ /dev/null
-#!/usr/bin/env python3
-
-import sys
-import csv
-import json
-
-
-rdsel = 1
-
-
-def twos_comp(val, bits):
- if val & (1 << (bits - 1)): return val - (1 << bits)
- return val
-
-
-def tmc2660_decode_response(x, rdsel = 1):
- x >>= 4 # Shift right 4 bits
- d = {'_hex': '0x%05x' % x}
-
- if rdsel == 0:
- d['mstep'] = (x >> 10) & 0x3ff
-
- elif rdsel == 1:
- d['sg'] = (x >> 10) & 0x3ff
-
- elif rdsel == 2:
- d['sg'] = x >> 15 & 0x1f
- d['se'] = (x >> 10) & 0x1f
-
- flags = []
- if x & (1 << 7): flags += ['stand']
- if x & (1 << 6): flags += ['open B']
- if x & (1 << 5): flags += ['open A']
- if x & (1 << 4): flags += ['short B']
- if x & (1 << 3): flags += ['short A']
- if x & (1 << 2): flags += ['temp warn']
- if x & (1 << 1): flags += ['overtemp']
- if x & (1 << 0): flags += ['stall']
-
- d['flags'] = flags
-
- return d
-
-
-def tmc2660_decode_cmd(x, r):
- global rdsel
-
- addr = x >> 17
-
- d = {'_hex': '0x%05x' % x}
-
- if addr == 0 or addr == 1:
- cmd = 'DRVCTRL'
- d['mstep'] = (256, 128, 64, 32, 16, 8, 4, 2, 1)[x & 0xf]
- d['dedge'] = bool(x & (1 << 8))
- d['intpol'] = bool(x & (1 << 9))
-
- elif addr == 4:
- chm = bool(x & (1 << 14))
-
- cmd = 'CHOPCONF'
- d['blank'] = (16, 24, 36, 54)[(x >> 15) & 3]
- d['chm'] = 'standard' if chm else 'constant'
- d['random toff'] = bool(x & (1 << 13))
-
- if chm: d['fast decay mode'] = ('current', 'timer')[x >> 12]
- else: d['hdec'] = (16, 32, 48, 64)[(x >> 11) & 3]
-
- if chm: d['SWO'] = ((x >> 7) & 0xf) - 3
- else: d['HEND'] = ((x >> 7) & 0xf) - 3
-
- if chm: d['fast decay'] = (((x >> 4) & 7) + ((x >> 7) & 8)) * 32
- else: d['HSTART'] = ((x >> 4) & 7) + 1
-
- # TODO this isn't quite right
- toff = x & 0xf
- if toff == 0: d['TOFF'] = 'disabled'
- else: d['TOFF'] = 12 + (32 * toff)
-
- elif addr == 5:
- cmd = 'SMARTEN'
- d['SEIMIN'] = '1/4 CS' if x & (1 << 15) else '1/2 CS'
- d['SEDN'] = (32, 8, 2, 1)[(x >> 13) & 3]
- d['SEMAX'] = (x >> 8) & 0xf
- d['SEUP'] = (1, 2, 4, 8)[(x >> 5) & 3]
- semin = x & 0xf
- d['SEMIN'] = semin if semin else 'disabled'
-
- elif addr == 6:
- cmd = 'SGCSCONF'
- d['SFILT'] = bool(x & (1 << 16))
- d['SGT'] = twos_comp((x >> 8) & 0x7f, 7)
- d['CS'] = ((x & 0x1f) + 1) / 32.0
-
- elif addr == 7:
- cmd = 'DRVCONF'
- d['TST'] = bool(x & (1 << 16))
- d['SLPH'] = ('min', 'min temp', 'med temp', 'max')[(x >> 14) & 3]
- d['SLPL'] = ('min', 'min', 'med', 'max')[(x >> 12) & 3]
- d['DISS2G'] = bool(x & (1 << 10))
- d['TS2G'] = (3.2, 1.6, 1.2, 0.8)[(x >> 8) & 3]
- d['SDOFF'] = 'SPI' if x & (1 << 7) else 'step'
- d['VSENSE'] = '165mV' if x & (1 << 6) else '305mV'
- rdsel = (x >> 4) & 3
- d['RDSEL'] = ('mstep', 'SG', 'SG & CS', 'Invalid')[rdsel]
-
- else: cmd = 'INVALID'
-
- return {cmd: d, 'response': tmc2660_decode_response(r, rdsel)}
-
-
-def tmc2660_decode(path):
- with open(path, newline = '') as f:
- reader = csv.reader(f)
-
- first = True
- for time, packet, mosi, miso in reader:
- if first:
- first = False
- continue
-
- cmd = tmc2660_decode_cmd(int(mosi, 0), int(miso, 0))
- cmd['id'] = int(packet)
- cmd['ts'] = float(time)
-
- print(json.dumps(cmd, sort_keys = True))
-
-
-if __name__ == "__main__":
- for path in sys.argv[1:]:
- tmc2660_decode(path)
projects / bbctrl-firmware / commitdiff