recursive-include src/py/bbctrl/http *
-include package.json README.md scripts/install.sh avr/bbctrl-avr-firmware.hex
+include package.json README.md scripts/install.sh src/avr/bbctrl-avr-firmware.hex
include scripts/avr109-flash.py
STATIC := $(patsubst src/resources/%,$(TARGET)/%,$(STATIC))
TEMPLS := $(wildcard src/jade/templates/*.jade)
-AVR_FIRMWARE := avr/bbctrl-avr-firmware.hex
+AVR_FIRMWARE := src/avr/bbctrl-avr-firmware.hex
RSYNC_EXCLUDE := \*.pyc __pycache__ \*.egg-info \\\#* \*~ .\\\#\*
RSYNC_EXCLUDE := $(patsubst %,--exclude %,$(RSYNC_EXCLUDE))
.PHONY: $(AVR_FIRMWARE)
$(AVR_FIRMWARE):
- $(MAKE) -C avr $(shell basename $@)
+ $(MAKE) -C src/avr $(shell basename $@)
publish: pkg
echo -n $(VERSION) > dist/latest.txt
+++ /dev/null
-# Backup files
-*~
-\#*
-
-build
-.dep
-
-/*.eep
-/*.hex
-/*.elf
-/*.lss
-/*.map
+++ /dev/null
-# Cubic Bezier
-
- f(x) = A(1 - x)^3 + 3B(1 - x)^2 x + 3C(1 - x) x^2 + Dx^3
-
- -Ax^3 + 3Ax^2 - 3Ax + A
- 3Bx^3 - 6Bx^2 + 3Bx
- -3Cx^3 + 3Cx^2
- Dx^3
-
-
- f(x) = (-A + 3B -3C + D)x^3 + (3A - 6B + 3C)x^2 + (-3A + 3B)x + A
-
- a = -A + 3B - 3C + D
- b = 3A - 6B + 3C
- c = -3A + 3B
- d = A
-
- f(x) = ax^3 + bx^2 + cx + d
-
- integral f(x) dx = a/4 x^4 + b/3 x^3 + c/2 x^2 + dx + E
-
- = (-A + 3B - 3C + D)/4 x^4 + (A - 2B + B) x^3 + 3/2 (B - A) x^2 + Ax + E
-
-# Quintic Bezier
-
- A(1 - x)^5 + 5A(1 - x)^4 x + 10A(1 - x)^3 x^2 + 10B(1 - x)^2 x^3 +
- 5B(1 - x)x^4 + Bx^5
-
- (-6A + 6B)x^5 + (15A - 15B)x^4 + (-10A + 10B)x^3 + A
-
- 6(B - A)x^5 + 15(A - B)x^4 + 10(B - A)x^3 + A
-
- x^3 (6(B - A)x^2 + 15(A - B)x + 10(B - A)) + A
-
- a = 6(B - A)
- b = -15(B - A)
- c = 10(B - A)
- d = A
-
- f(x) = ax^5 + bx^4 + cx^3 + d
-
- f(x) = (ax^2 + bx + c)x^3 + d
-
-
- integral f(x) = a/6 x^6 + b/5 x^5 + c/4 x^4 + dx + e
-
- = (B - A)x^6 - 3(B - A)x^5 + 5/2(B - A)x^4 + Ax + e
-
- = (B - A)x^4 (x^2 - 3x + 5/2) + Ax + e
-
- A = 0
- B = 1
- e = 0
-
- f(x) = 6x^5 -15x^4 + 10x^3
- int f(x) dx = x^6 - 3x^5 + 5/2x^4 + C
+++ /dev/null
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2017, 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) -flto -fwhole-program
-
-CFLAGS += $(COMMON)
-CFLAGS += -Wall -Werror
-CFLAGS += -Wno-error=strict-aliasing # for _invsqrt
-CFLAGS += -std=gnu99 -DF_CPU=$(CLOCK)UL -O3
-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/boot/*.S)
-BOOT_SRC += $(wildcard src/boot/*.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:
- @$(MAKE) $(PROJECT).hex
- @$(MAKE) build/vars.json
- @$(MAKE) boot
- @$(MAKE) size
-
-boot: boot.hex boot-size
-
-build/vars.json: src/vars.def
-
-# Compile
-build/%.json: src/%.json.in
- cpp -Isrc $< | sed '/^#.*$$/d' > $@
-
-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 $@
-
-boot.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: boot.elf
- @$(MAKE) SIZE_TARGET=$< _size
-
-size: $(TARGET)
- @$(MAKE) SIZE_TARGET=$< _size
-
-# Program
-init:
- $(MAKE) erase
- -$(MAKE) fuses
- $(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: boot.hex
- avrdude $(AVRDUDE_OPTS) -U flash:w:boot.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 boot.hex boot.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 difference 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 fragments for pulse
-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 interrupt, 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 interrupt signals a lower level
-interrupt to call ``mp_exec_move()`` and load the next move fragment.
+++ /dev/null
-# Parsing, Queuing & Planning
- * main()
- * command_callback()
- * gc_gcode_parser(const char *block)
- * _normalize_gcode_block(...)
- * _parse_gcode_block(const char *block)
- * _execute_gcode_block()
- * mach_*()
- * mach_straight_traverse/feed() || mach_arc_feed()
- * mp_aline(const float target[], float feed,. . .)
- * _calc_jerk*()
- * _calc_move_time()
- * _calc_max_velocities()
- * _get_junction_vmax()
- * mp_plan()
- * mp_calculate_trapezoid()
- * mp_get_target_length()
- * mp_get_target_velocity()
- * mp_queue_push(buffer_cb_t cb, uint32_t time)
-
-# Execution
- * STEP_LOW_LEVEL_ISR
- * mp_exec_move()
- * mp_queue_get_head()
- * mp_buffer->cb()
- * _exec_dwell()
- * mp_exec_aline()
- * _exec_aline_init()
- * _exec_aline_head() || _exec_aline_body() || _exec_aline_tail()
- * _exec_aline_section()
- * mp_runtime_move_to_target()
- * mp_kinematics() - Converts target mm to steps and maps motors
- * st_prep_line()
- * motor_prep_move()
-
-# Step Output
- * STEP_TIMER_ISR
- * _load_move()
- * _end_move()
- * motor_end_move()
- * _request_exec_move()
- * Triggers STEP_LOW_LEVEL_ISR
- * motor_load_move()
- * motor_end_move()
-
-
-# Data flow
-## GCode block
-char *line
-
-## Planner buffer
- * mp_buffer_t pool[]
- * float target[] - Target position in mm
- * float unit[] - Direction vector
- * float length, {head,body,tail}_length - Lengths in mm
- * float {entry,cruise,exit,braking}_velocity - Target velocities in mm/min
- * float {entry,cruise,exit,delta}_vmax - Max velocities in mm/min
- * float jerk - Max jerk in km/min^3
-
-``mach_*()`` functions compute ``target`` and call ``mp_aline()``.
-``mp_aline()`` saves ``target`` computes ``unit`` and ``jerk``. Then it
-estimates the move time in order to calculate max velocities.
-
-``mp_plan()`` and friends compute target velocities and trapezoid segment
-lengths in two passes using max velocities in the current and neighboring
-planner buffers.
-
-``mp_exec_aline()`` executes the trapezoidal move in the next planner buffer.
-
-## Move prep
- * stepper_t st
- * uint16_t seg_period - The step timer period
- * motor_t motors[]
- * uint8_t timer_clock - Clock divisor or zero for off
- * uint16_t timer_period - Clock period
- * direction_t direction - Step polarity
+++ /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 - 2017 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>
-#include <ctype.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 travel_max; // max work envelope for soft limits
- float travel_min; // min work envelope for soft limits
- float jerk_max; // max jerk (Jm) in km/min^3
- 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] = {{0}};
-
-
-bool axis_is_enabled(int axis) {
- int motor = axis_get_motor(axis);
- return motor != -1 && motor_is_enabled(motor) &&
- !fp_ZERO(axis_get_velocity_max(axis));
-}
-
-
-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, toupper(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;}
-
-
-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(homed, bool, false)
-AXIS_SET(homed, bool)
-AXIS_GET(homing_mode, homing_mode_t, HOMING_MANUAL)
-AXIS_SET(homing_mode, homing_mode_t)
-AXIS_GET(radius, float, 0)
-AXIS_GET(travel_min, float, 0)
-AXIS_GET(travel_max, float, 0)
-AXIS_GET(search_velocity, float, 0)
-AXIS_GET(latch_velocity, float, 0)
-AXIS_GET(zero_backoff, float, 0)
-AXIS_GET(latch_backoff, 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)
-
-
-AXIS_VAR_SET(velocity_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(jerk_max, float)
-
-
-float get_homing_dir(int axis) {
- switch (axes[axis].homing_mode) {
- case HOMING_MANUAL: break;
- case HOMING_STALL_MIN: case HOMING_SWITCH_MIN: return -1;
- case HOMING_STALL_MAX: case HOMING_SWITCH_MAX: return 1;
- }
- return 0;
-}
-
-
-float get_home(int axis) {
- switch (axes[axis].homing_mode) {
- case HOMING_MANUAL: break;
- case HOMING_STALL_MIN: case HOMING_SWITCH_MIN: return get_travel_min(axis);
- case HOMING_STALL_MAX: case HOMING_SWITCH_MAX: return get_travel_max(axis);
- }
- return NAN;
-}
-
-
-static int _get_homing_switch(int axis) {
- switch (axes[axis].homing_mode) {
- case HOMING_MANUAL: break;
-
- case HOMING_STALL_MIN: case HOMING_SWITCH_MIN:
- switch (axis) {
- case AXIS_X: return SW_MIN_X;
- case AXIS_Y: return SW_MIN_Y;
- case AXIS_Z: return SW_MIN_Z;
- case AXIS_A: return SW_MIN_A;
- }
- break;
-
- case HOMING_STALL_MAX: case HOMING_SWITCH_MAX:
- switch (axis) {
- case AXIS_X: return SW_MAX_X;
- case AXIS_Y: return SW_MAX_Y;
- case AXIS_Z: return SW_MAX_Z;
- case AXIS_A: return SW_MAX_A;
- }
- break;
- }
-
- return -1;
-}
-
-
-bool get_axis_can_home(int axis) {
- return axis_is_enabled(axis) && axes[axis].homing_mode != HOMING_MANUAL &&
- switch_is_enabled(_get_homing_switch(axis));
-}
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2017 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_MANUAL,
- 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_jerk_max(int axis);
-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);
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2017 Buildbotics LLC
- Copyright (c) 2010 Alex Forencich <alex@alexforencich.com>
- 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 "boot.h"
-#include "sp_driver.h"
-
-#include <util/delay.h>
-#include <util/crc16.h>
-
-#include <avr/eeprom.h>
-#include <avr/io.h>
-
-#include <stdbool.h>
-
-
-uint8_t buffer[SPM_PAGESIZE];
-uint16_t block_crc = 0;
-
-
-void clock_init() {
- // 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
-}
-
-
-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);
- UART_DEVICE.CTRLB = USART_RXEN_bm | USART_TXEN_bm;
-
- 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;
-}
-
-
-void watchdog_init() {
- uint8_t temp = WDT_ENABLE_bm | WDT_CEN_bm | WDT_PER_4KCLK_gc;
- CCP = CCP_IOREG_gc;
- WDT.CTRL = temp;
- while (WDT.STATUS & WDT_SYNCBUSY_bm) continue;
-}
-
-
-void watchdog_reset() {asm("wdr");}
-
-
-void watchdog_disable() {
- uint8_t temp = (WDT.CTRL & ~WDT_ENABLE_bm) | WDT_CEN_bm;
- CCP = CCP_IOREG_gc;
- WDT.CTRL = temp;
-}
-
-
-void nvm_wait() {while (NVM_STATUS & NVM_NVMBUSY_bp) watchdog_reset();}
-
-
-void nvm_exec() {
- void *z = (void *)&NVM_CTRLA;
-
- __asm__ volatile
- ("out %[ccp], %[ioreg]\n"
- "st z, %[cmdex]" ::
- [ccp] "I" (_SFR_IO_ADDR(CCP)),
- [ioreg] "d" (CCP_IOREG_gc),
- [cmdex] "r" (NVM_CMDEX_bm),
- [z] "z" (z));
-}
-
-
-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() {
- uint8_t hi = get_char();
- uint8_t lo = get_char();
- return ((uint16_t)hi << 8) | lo;
-}
-
-
-uint8_t BlockLoad(unsigned size, uint8_t mem, uint32_t *address) {
- watchdog_reset();
-
- // fill up buffer
- block_crc = 0xffff;
- for (int i = 0; i < SPM_PAGESIZE; i++)
- if (i < size) {
- buffer[i] = get_char();
- block_crc = _crc16_update(block_crc, buffer[i]);
-
- } else buffer[i] = 0xff;
-
- switch (mem) {
- case MEM_EEPROM:
- eeprom_write_block(buffer, (uint8_t *)(uint16_t)*address, 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) {
- switch (mem) {
- case MEM_EEPROM:
- eeprom_read_block(buffer, (uint8_t *)(uint16_t)*address, size);
- *address += size;
-
- // send bytes
- for (int i = 0; i < size; i++)
- send_char(buffer[i]);
- break;
-
- case MEM_FLASH: case MEM_USERSIG: case MEM_PRODSIG: {
- *address <<= 1; // Convert address to bytes temporarily
-
- do {
- switch (mem) {
- case MEM_FLASH: send_char(SP_ReadByte(*address)); break;
- case MEM_USERSIG: send_char(SP_ReadUserSignatureByte(*address)); break;
- case MEM_PRODSIG: send_char(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: break;
- }
-}
-
-
-int main() {
- // Init
- clock_init();
- uart_init();
- watchdog_init();
-
- // Check for trigger
- bool in_bootloader = false;
- uint16_t j = INITIAL_WAIT;
- while (!in_bootloader && 0 < j--) {
- if (uart_has_char()) in_bootloader = uart_recv_char() == CMD_SYNC;
- watchdog_reset();
- _delay_ms(1);
- }
-
- // Main bootloader
- uint32_t address = 0;
- uint16_t i = 0;
-
- while (in_bootloader) {
- uint8_t val = get_char();
- watchdog_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: {
- uint8_t hi = get_char();
- address = ((uint32_t)hi << 16) | get_word();
- send_char(REPLY_ACK);
- break;
- }
-
- case CMD_FLASH_ERASE:
- SP_EraseApplicationSection();
- nvm_wait();
- send_char(REPLY_ACK);
- break;
-
- case CMD_EEPROM_ERASE:
- NVM.CMD = NVM_CMD_ERASE_EEPROM_gc;
- nvm_exec();
- send_char(REPLY_ACK);
- break;
-
- case CMD_CHECK_BLOCK_SUPPORT:
- send_char(REPLY_YES);
- // Send block size (page size)
- send_char(SPM_PAGESIZE >> 8);
- send_char((uint8_t)SPM_PAGESIZE);
- 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((uint8_t *)(uint16_t)address, get_char());
- address++;
- send_char(REPLY_ACK);
- break;
-
- case CMD_READ_EEPROM_BYTE:
- send_char(eeprom_read_byte((uint8_t *)(uint16_t)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('b');
- send_char('b');
- send_char('c');
- send_char('t');
- send_char('r');
- send_char('l');
- send_char(' ');
- break;
-
- case CMD_VERSION:
- send_char('0');
- send_char('2');
- break;
-
- case CMD_READ_SIGNATURE:
- send_char(SIGNATURE_2);
- send_char(SIGNATURE_1);
- send_char(SIGNATURE_0);
- break;
-
- case CMD_READ_CHECKSUM:
- // Setup
- nvm_wait();
-
- // Reset CRC
- CRC_CTRL |= CRC_RESET_RESET1_gc;
- CRC.CHECKSUM0 = CRC.CHECKSUM1 = CRC.CHECKSUM2 = CRC.CHECKSUM3 = 0xff;
-
- // 32-bit mode, flash source
- CRC_CTRL = CRC_CRC32_bm | CRC_SOURCE_FLASH_gc;
-
- // Start address
- NVM.ADDR0 = (uint8_t)(APP_SECTION_START >> 0);
- NVM.ADDR1 = (uint8_t)(APP_SECTION_START >> 8);
- NVM.ADDR2 = 0;
-
- // End address
- NVM.DATA0 = (uint8_t)(APP_SECTION_END >> 0);
- NVM.DATA1 = (uint8_t)(APP_SECTION_END >> 8);
- NVM.DATA2 = (uint8_t)(APP_SECTION_END >> 16);
-
- NVM.CMD = NVM_CMD_FLASH_RANGE_CRC_gc;
- CCP = CCP_IOREG_gc;
- NVM.CTRLA = NVM_CMDEX_bm;
-
- // Compute
- nvm_wait();
- while (CRC.STATUS & CRC_BUSY_bm) continue;
-
- // Send 32-bit checksum
- send_char(CRC.CHECKSUM3);
- send_char(CRC.CHECKSUM2);
- send_char(CRC.CHECKSUM1);
- send_char(CRC.CHECKSUM0);
- break;
-
- case CMD_FLASH_LENGTH:
- send_char((uint8_t)(APP_SECTION_SIZE >> 16));
- send_char((uint8_t)(APP_SECTION_SIZE >> 8));
- send_char((uint8_t)(APP_SECTION_SIZE >> 0));
- break;
-
- case CMD_BLOCK_CRC:
- send_char(block_crc >> 8);
- send_char((uint8_t)block_crc);
- 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();
- }
-
- // Deinit
- uart_deinit();
- watchdog_disable();
-
- // Disable further self programming until next reset
- SP_LockSPM();
-
- // Jump to application code
- asm("jmp 0");
-}
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2017 Buildbotics LLC
- Copyright (c) 2010 Alex Forencich <alex@alexforencich.com>
- 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
-
-#define INITIAL_WAIT 1000 // In ms
-
-#define UART_RX_PIN 2
-#define UART_TX_PIN 3
-#define UART_PORT PORTC
-#define UART_DEVICE USARTC0
-
-// Baud rate 921600 @ 32Mhz
-#define UART_BSEL_VALUE 150
-#define UART_BSCALE_VALUE -7
-
-
-// Protocol
-enum {
- CMD_SYNC = '\x1b',
-
- // Informational
- CMD_CHECK_AUTOINCREMENT = 'a',
- CMD_CHECK_BLOCK_SUPPORT = 'b',
- CMD_PROGRAMMER_TYPE = 'p',
- CMD_DEVICE_CODE = 't',
- CMD_PROGRAM_ID = 'S',
- CMD_VERSION = 'V',
- CMD_HW_VERSION = 'v', // Unsupported extension
- CMD_READ_SIGNATURE = 's',
- CMD_READ_CHECKSUM = 'X',
- CMD_FLASH_LENGTH = 'n',
-
- // Addressing
- CMD_SET_ADDRESS = 'A',
- CMD_SET_EXT_ADDRESS = 'H',
-
- // Erase
- CMD_FLASH_ERASE = 'e',
- CMD_EEPROM_ERASE = '_',
-
- // Block Access
- CMD_BLOCK_LOAD = 'B',
- CMD_BLOCK_READ = 'g',
- CMD_BLOCK_CRC = 'i',
-
- // Byte Access
- CMD_READ_BYTE = 'R',
- CMD_WRITE_LOW_BYTE = 'c',
- CMD_WRITE_HIGH_BYTE = 'C',
- CMD_WRITE_PAGE = 'm',
- CMD_WRITE_EEPROM_BYTE = 'D',
- CMD_READ_EEPROM_BYTE = 'd',
-
- // Lock and Fuse Bits
- CMD_WRITE_LOCK_BITS = 'l',
- CMD_READ_LOCK_BITS = 'r',
- CMD_READ_LOW_FUSE_BITS = 'F',
- CMD_READ_HIGH_FUSE_BITS = 'N',
- CMD_READ_EXT_FUSE_BITS = 'Q',
-
- // Control
- CMD_ENTER_PROG_MODE = 'P',
- CMD_LEAVE_PROG_MODE = 'L',
- CMD_EXIT_BOOTLOADER = 'E',
- CMD_SET_LED = 'x',
- CMD_CLEAR_LED = 'y',
- CMD_SET_TYPE = 'T',
-};
-
-
-// Memory types for block access
-enum {
- MEM_EEPROM = 'E',
- MEM_FLASH = 'F',
- MEM_USERSIG = 'U',
- MEM_PRODSIG = 'P',
-};
-
-
-// Command Responses
-enum {
- REPLY_ACK = '\r',
- REPLY_YES = 'Y',
- REPLY_ERROR = '?',
-};
+++ /dev/null
-;******************************************************************************
-;* $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.
-;******************************************************************************
-;*
-;* 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 finished using any of the functions in this driver.
-;*
-;* For all routines, it is important that any interrupt handlers do not
-;* perform NVM operations. The user must implement a scheme for mutually
-;* exclusive access to 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
-
-#include <avr/io.h>
-
-; Defines not yet included in header file.
-#define NVM_CMD_NO_OPERATION_gc 0x00
-#define NVM_CMD_READ_USER_SIG_ROW_gc 0x01
-#define NVM_CMD_READ_CALIB_ROW_gc 0x02
-#define NVM_CMD_READ_EEPROM_gc 0x06
-#define NVM_CMD_READ_FUSES_gc 0x07
-#define NVM_CMD_WRITE_LOCK_BITS_gc 0x08
-#define NVM_CMD_ERASE_USER_SIG_ROW_gc 0x18
-#define NVM_CMD_WRITE_USER_SIG_ROW_gc 0x1a
-#define NVM_CMD_ERASE_APP_gc 0x20
-#define NVM_CMD_ERASE_APP_PAGE_gc 0x22
-#define NVM_CMD_LOAD_FLASH_BUFFER_gc 0x23
-#define NVM_CMD_WRITE_APP_PAGE_gc 0x24
-#define NVM_CMD_ERASE_WRITE_APP_PAGE_gc 0x25
-#define NVM_CMD_ERASE_FLASH_BUFFER_gc 0x26
-#define NVM_CMD_ERASE_BOOT_PAGE_gc 0x2a
-#define NVM_CMD_WRITE_BOOT_PAGE_gc 0x2c
-#define NVM_CMD_ERASE_WRITE_BOOT_PAGE_gc 0x2d
-#define NVM_CMD_ERASE_EEPROM_gc 0x30
-#define NVM_CMD_ERASE_EEPROM_PAGE_gc 0x32
-#define NVM_CMD_LOAD_EEPROM_BUFFER_gc 0x33
-#define NVM_CMD_WRITE_EEPROM_PAGE_gc 0x34
-#define NVM_CMD_ERASE_WRITE_EEPROM_PAGE_gc 0x35
-#define NVM_CMD_ERASE_EEPROM_BUFFER_gc 0x36
-#define NVM_CMD_APP_CRC_gc 0x38
-#define NVM_CMD_BOOT_CRC_gc 0x39
-#define NVM_CMD_FLASH_RANGE_CRC_gc 0x3a
-#define CCP_SPM_gc 0x9d
-#define CCP_IOREG_gc 0xd8
-
-
-; 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
-
-
-; 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
-
-
-; 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.
-
-
-; 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.
-
-
-; 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 index into NVM Address Reg 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
-
-
-; Erases the user signature row.
-.section .text
-.global SP_EraseUserSignatureRow
-
-SP_EraseUserSignatureRow:
- in r19, RAMPZ ; Save RAMPZ, 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.
-
-
-; Writes the flash buffer to the user signature row.
-.section .text
-.global SP_WriteUserSignatureRow
-
-SP_WriteUserSignatureRow:
- in r19, RAMPZ ; Save RAMPZ, 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.
-
-
-; Erases the entire application section.
-.section .text
-.global SP_EraseApplicationSection
-
-SP_EraseApplicationSection:
- in r19, RAMPZ ; Save RAMPZ, 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.
-
-
-; 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.
-.section .text
-.global SP_LoadFlashWord
-
-SP_LoadFlashWord:
- in r19, RAMPZ ; Save RAMPZ, 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.
-
-
-; 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.
-.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 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 (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
-
-
-; Writes the page buffer to Flash 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.
-.section .text
-.global SP_WriteApplicationPage
-
-SP_WriteApplicationPage:
- in r19, RAMPZ ; Save RAMPZ, 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.
-
-
-; 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.
-.section .text
-.global SP_EraseWriteApplicationPage
-
-SP_EraseWriteApplicationPage:
- in r19, RAMPZ ; Save RAMPZ, 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.
-
-
-
-; Locks all further access to SPM operations until next reset.
-.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
- ; (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
-
-
-; 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.
-.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
-
-
-; 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
- ; (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
-
-
-; 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
-
-
-; 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.
-.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 (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
+++ /dev/null
-/*******************************************************************************
- * $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.
- ******************************************************************************/
-
-/*! \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
- * Support email: avr@atmel.com
- ******************************************************************************/
-
-#pragma once
-
-#include <avr/io.h>
-
-#include <stdint.h>
-
-#ifndef APP_SECTION_PAGE_SIZE
-#error APP_SECTION_PAGE_SIZE must be defined if not defined in header files.
-#endif
-
-#ifndef APPTABLE_SECTION_START
-#error APPTABLE_SECTION_START must be defined if not defined in header files.
-#endif
-
-
-/*! \brief Read a byte from flash.
- *
- * \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.
- *
- * \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 Read user signature at given index.
- *
- * \param index Index of the byte in the user signature row.
- * \retval User signature byte
- */
-uint8_t SP_ReadUserSignatureByte(uint16_t index);
-
-/// Erase user signature row.
-void SP_EraseUserSignatureRow();
-
-/// Write user signature row.
-void SP_WriteUserSignatureRow();
-
-/*! \brief Erase entire application 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();
-
-/*! \brief Erase and write page buffer to application or application table
- * section at byte address.
- *
- * \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.
- *
- * \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.
- *
- * \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.
- *
- * \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);
-
-/// Flush Flash page buffer.
-void SP_EraseFlashBuffer();
-
-/// Disables the use of SPM until the next reset.
-void SP_LockSPM();
-
-/// Waits for the SPM to finish and clears the command register.
-void SP_WaitForSPM();
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2017 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 "i2c.h"
-#include "plan/buffer.h"
-#include "plan/state.h"
-#include "config.h"
-#include "pgmspace.h"
-
-#ifdef __AVR__
-#include <avr/wdt.h>
-#endif
-
-#include <stdio.h>
-#include <string.h>
-#include <ctype.h>
-#include <stdlib.h>
-
-
-static char *_cmd = 0;
-
-
-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_REBOOT: hw_request_hard_reset(); 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;
-}
-
-
-char *_parse_arg(char **p) {
- char *start = *p;
- char *next = *p;
-
- bool inQuote = false;
- bool escape = false;
-
- while (**p) {
- char c = *(*p)++;
-
- switch (c) {
- case '\\':
- if (!escape) {
- escape = true;
- continue;
- }
- break;
-
- case ' ': case '\t':
- if (!inQuote && !escape) goto done;
- break;
-
- case '"':
- if (!escape) {
- inQuote = !inQuote;
- continue;
- }
- break;
-
- default: break;
- }
-
- *next++ = c;
- escape = false;
- }
-
- done:
- *next = 0;
- return start;
-}
-
-
-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
- char *arg = _parse_arg(&p);
- if (*arg) argv[argc++] = arg;
- }
-
- // 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_is_ready()) return; // Wait for motion planner
-
- // 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 = " $%-12"PRPSTR" %"PRPSTR"\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);
-#ifdef __AVR__
- wdt_reset();
-#endif
- }
-
- puts_P(PSTR("\nVariables:"));
- vars_print_help();
-
- return STAT_OK;
-}
-
-
-uint8_t command_report(int argc, char *argv[]) {
- if (argc == 2) {
- vars_report_all(var_parse_bool(argv[1]));
- return STAT_OK;
- }
-
- vars_report_var(argv[1], var_parse_bool(argv[2]));
- return STAT_OK;
-}
-
-
-uint8_t command_reboot(int argc, char *argv[]) {
- hw_request_hard_reset();
- 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 - 2017 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(report, 1, 2, "[var] <enable>. Enable or disable var reporting")
-CMD(reboot, 0, 0, "Reboot the controller")
-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")
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2017 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 - 2017 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"
-
-#ifdef __AVR__
-#include <avr/interrupt.h>
-#endif
-
-
-// Pins
-enum {
- STALL_X_PIN = PORT_A << 3,
- STALL_Y_PIN,
- STALL_Z_PIN,
- STALL_A_PIN,
- TOOL_DIR_PIN,
- TOOL_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
-
-
-#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 10 // number of supported switches
-#define OUTS 5 // number of supported pin outputs
-
-
-// Switch settings. See switch.c
-#define SWITCH_INTLVL PORT_INT0LVL_MED_gc
-
-
-// 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 ((uint16_t)(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 STEP_LOW_LEVEL_ISR ADCB_CH0_vect
-
-#define SEGMENT_USEC 5000.0 // segment time
-#define SEGMENT_SEC (SEGMENT_USEC / 1000000.0)
-#define SEGMENT_TIME (SEGMENT_SEC / 60.0)
-#define SEGMENT_CLOCKS ((uint24_t)(F_CPU * SEGMENT_SEC))
-#define SEGMENT_PERIOD ((uint16_t)(STEP_TIMER_FREQ * SEGMENT_SEC))
-
-
-// DRV8711 settings
-#if 0 // Doug's settings
-#define DRV8711_OFF 48
-#define DRV8711_BLANK (0x80 | DRV8711_BLANK_ABT_bm)
-#define DRV8711_DECAY (DRV8711_DECAY_DECMOD_AUTO_OPT | 6)
-
-#else
-#define DRV8711_OFF 12
-#define DRV8711_BLANK (0x32 | DRV8711_BLANK_ABT_bm)
-#define DRV8711_DECAY (DRV8711_DECAY_DECMOD_MIXED | 16)
-#endif
-
-#define DRV8711_STALL (DRV8711_STALL_SDCNT_2 | DRV8711_STALL_VDIV_4 | 200)
-#define DRV8711_DRIVE (DRV8711_DRIVE_IDRIVEP_50 | \
- DRV8711_DRIVE_IDRIVEN_100 | DRV8711_DRIVE_TDRIVEP_250 | \
- DRV8711_DRIVE_TDRIVEN_250 | DRV8711_DRIVE_OCPDEG_2 | \
- DRV8711_DRIVE_OCPTH_500)
-#define DRV8711_TORQUE DRV8711_TORQUE_SMPLTH_200
-#define DRV8711_CTRL (DRV8711_CTRL_ISGAIN_10 | DRV8711_CTRL_DTIME_450 | \
- DRV8711_CTRL_EXSTALL_bm)
-
-
-// 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)
-
-
-// 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. 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
-
-
-// Settings ********************************************************************
-
-// Motor settings. See motor.c
-#define MOTOR_IDLE_TIMEOUT 0.25 // secs, motor off after this time
-
-
-#define MIN_HALF_STEP_CORRECTION 4
-#define CHORDAL_TOLERANCE 0.01 // chordal accuracy for arcs
-#define JUNCTION_DEVIATION 0.05 // default value, in mm
-#define JUNCTION_ACCELERATION 100000 // centripetal corner accel
-#define JOG_MIN_VELOCITY 10 // mm/min
-#define CAL_ACCELERATION 500000 // mm/min^2
-#define CURRENT_SENSE_RESISTOR 0.05 // ohms
-#define CURRENT_SENSE_REF 2.75 // volts
-#define MAX_CURRENT 10 // amps
-
-// 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
-
-
-// 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
-#define GCODE_MAX_OPERATOR_DEPTH 16
-#define GCODE_MAX_VALUE_DEPTH 32
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2017 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) {SET_PIN(SWITCH_1_PIN, !x);}
-void coolant_set_flood(bool x) {SET_PIN(SWITCH_2_PIN, !x);}
-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 - 2017 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 - 2017 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 "stepper.h"
-#include "report.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))
-
-
-bool motor_fault = false;
-
-
-typedef struct {
- float current;
- uint16_t isgain;
- uint8_t torque;
-} current_t;
-
-
-typedef struct {
- uint8_t status;
- uint16_t flags;
-
- drv8711_state_t state;
- current_t drive;
- current_t idle;
- float stall_threshold;
-
- 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 _current_set(current_t *c, float current) {
- c->current = current;
-
- float torque_over_gain = current * CURRENT_SENSE_RESISTOR / CURRENT_SENSE_REF;
-
- float gain = 0;
- if (torque_over_gain < 1.0 / 40) {
- c->isgain = DRV8711_CTRL_ISGAIN_40;
- gain = 40;
-
- } else if (torque_over_gain < 1.0 / 20) {
- c->isgain = DRV8711_CTRL_ISGAIN_20;
- gain = 20;
-
- } else if (torque_over_gain < 1.0 / 10) {
- c->isgain = DRV8711_CTRL_ISGAIN_10;
- gain = 10;
-
- } else if (torque_over_gain < 1.0 / 5) {
- c->isgain = DRV8711_CTRL_ISGAIN_5;
- gain = 5;
- }
-
- c->torque = round(torque_over_gain * gain * 256);
-}
-
-
-static bool _driver_get_enabled(int driver) {
- drv8711_state_t state = drivers[driver].state;
- return state == DRV8711_IDLE || state == DRV8711_ACTIVE;
-}
-
-
-static float _driver_get_current(int driver) {
- drv8711_driver_t *drv = &drivers[driver];
-
- switch (drv->state) {
- case DRV8711_IDLE: return drv->idle.current;
- case DRV8711_ACTIVE: return drv->drive.current;
- default: return 0; // Off
- }
-}
-
-
-static uint16_t _driver_get_isgain(int driver) {
- drv8711_driver_t *drv = &drivers[driver];
-
- switch (drv->state) {
- case DRV8711_IDLE: return drv->idle.isgain;
- case DRV8711_ACTIVE: return drv->drive.isgain;
- default: return 0; // Off
- }
-}
-
-
-static uint8_t _driver_get_torque(int driver) {
- drv8711_driver_t *drv = &drivers[driver];
-
- switch (drv->state) {
- case DRV8711_IDLE: return drv->idle.torque;
- case DRV8711_ACTIVE: return drv->drive.torque;
- default: return 0; // Off
- }
-}
-
-
-static uint8_t _spi_next_command(uint8_t cmd) {
- // Process command responses
- for (int driver = 0; driver < DRIVERS; driver++) {
- drv8711_driver_t *drv = &drivers[driver];
- uint16_t command = spi.commands[driver][cmd];
-
- if (DRV8711_CMD_IS_READ(command))
- switch (DRV8711_CMD_ADDR(command)) {
- case DRV8711_STATUS_REG:
- drv->status = spi.responses[driver];
-
- if ((drv->status & drv->flags) != drv->status) {
- drv->flags |= drv->status;
- report_request();
- }
- break;
-
- case DRV8711_OFF_REG:
- // We read back the OFF register to test for communication failure.
- if ((spi.responses[driver] & 0x1ff) != DRV8711_OFF)
- drv->flags |= DRV8711_COMM_ERROR_bm;
- break;
- }
- }
-
- // Next command
- if (++cmd == spi.ncmds) {
- cmd = 0; // Wrap around
- st_enable(); // Enable motors
- }
-
- // Prep next command
- for (int driver = 0; driver < DRIVERS; driver++) {
- drv8711_driver_t *drv = &drivers[driver];
- uint16_t *command = &spi.commands[driver][cmd];
-
- switch (DRV8711_CMD_ADDR(*command)) {
- case DRV8711_STATUS_REG:
- if (!DRV8711_CMD_IS_READ(*command))
- // Clear STATUS flags
- *command = (*command & 0xf000) | (0x0fff & ~(drv->status));
- break;
-
- case DRV8711_TORQUE_REG: // Update motor current setting
- *command = (*command & 0xff00) | _driver_get_torque(driver);
- break;
-
- case DRV8711_CTRL_REG: // Set microsteps
- *command = (*command & 0xfc86) | _driver_get_isgain(driver) |
- (drv->mode << 3) |
- (_driver_get_enabled(driver) ? DRV8711_CTRL_ENBL_bm : 0);
- 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;
-
- commands[spi.ncmds++] = DRV8711_WRITE(DRV8711_OFF_REG, DRV8711_OFF);
- commands[spi.ncmds++] = DRV8711_WRITE(DRV8711_BLANK_REG, DRV8711_BLANK);
- commands[spi.ncmds++] = DRV8711_WRITE(DRV8711_DECAY_REG, DRV8711_DECAY);
- commands[spi.ncmds++] = DRV8711_WRITE(DRV8711_STALL_REG, DRV8711_STALL);
- commands[spi.ncmds++] = DRV8711_WRITE(DRV8711_DRIVE_REG, DRV8711_DRIVE);
- commands[spi.ncmds++] = DRV8711_WRITE(DRV8711_TORQUE_REG, DRV8711_TORQUE);
- commands[spi.ncmds++] = DRV8711_WRITE(DRV8711_CTRL_REG, DRV8711_CTRL);
- commands[spi.ncmds++] = DRV8711_READ(DRV8711_OFF_REG);
- commands[spi.ncmds++] = DRV8711_READ(DRV8711_STATUS_REG);
- 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() {
- // 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();
-}
-
-
-drv8711_state_t drv8711_get_state(int driver) {
- if (driver < 0 || DRIVERS <= driver) return DRV8711_DISABLED;
- return drivers[driver].state;
-}
-
-
-void drv8711_set_state(int driver, drv8711_state_t state) {
- if (driver < 0 || DRIVERS <= driver) return;
- drivers[driver].state = state;
-}
-
-
-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_current(int driver) {
- if (driver < 0 || DRIVERS <= driver) return 0;
- return drivers[driver].drive.current;
-}
-
-
-void set_drive_current(int driver, float value) {
- if (driver < 0 || DRIVERS <= driver || value < 0 || MAX_CURRENT < value)
- return;
- _current_set(&drivers[driver].drive, value);
-}
-
-
-float get_idle_current(int driver) {
- if (driver < 0 || DRIVERS <= driver) return 0;
- return drivers[driver].idle.current;
-}
-
-
-void set_idle_current(int driver, float value) {
- if (driver < 0 || DRIVERS <= driver || value < 0 || MAX_CURRENT < value)
- return;
-
- _current_set(&drivers[driver].idle, value);
-}
-
-
-float get_active_current(int driver) {
- if (driver < 0 || DRIVERS <= driver) return 0;
- return _driver_get_current(driver);
-}
-
-
-bool get_motor_fault() {return motor_fault;}
-
-
-uint16_t get_driver_flags(int driver) {return drivers[driver].flags;}
-
-
-void print_status_flags(uint16_t flags) {
- bool first = true;
-
- putchar('"');
-
- if (DRV8711_STATUS_OTS_bm & flags) {
- if (!first) printf_P(PSTR(", "));
- printf_P(PSTR("temp"));
- first = false;
- }
-
- if (DRV8711_STATUS_AOCP_bm & flags) {
- if (!first) printf_P(PSTR(", "));
- printf_P(PSTR("current a"));
- first = false;
- }
-
- if (DRV8711_STATUS_BOCP_bm & flags) {
- if (!first) printf_P(PSTR(", "));
- printf_P(PSTR("current b"));
- first = false;
- }
-
- if (DRV8711_STATUS_APDF_bm & flags) {
- if (!first) printf_P(PSTR(", "));
- printf_P(PSTR("fault a"));
- first = false;
- }
-
- if (DRV8711_STATUS_BPDF_bm & flags) {
- if (!first) printf_P(PSTR(", "));
- printf_P(PSTR("fault b"));
- first = false;
- }
-
- if (DRV8711_STATUS_UVLO_bm & flags) {
- if (!first) printf_P(PSTR(", "));
- printf_P(PSTR("uvlo"));
- first = false;
- }
-
- if ((DRV8711_STATUS_STD_bm | DRV8711_STATUS_STDLAT_bm) & flags) {
- if (!first) printf_P(PSTR(", "));
- printf_P(PSTR("stall"));
- first = false;
- }
-
- if (DRV8711_COMM_ERROR_bm & flags) {
- if (!first) printf_P(PSTR(", "));
- printf_P(PSTR("comm"));
- first = false;
- }
-
- putchar('"');
-}
-
-
-uint16_t get_status_strings(int driver) {return get_driver_flags(driver);}
-
-
-// Command callback
-void command_mreset(int argc, char *argv[]) {
- if (argc == 1)
- for (int driver = 0; driver < DRIVERS; driver++)
- drivers[driver].flags = 0;
-
- else {
- int driver = atoi(argv[1]);
- if (driver < DRIVERS) drivers[driver].flags = 0;
- }
-
- report_request();
-}
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2017 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,
- DRV8711_COMM_ERROR_bm = 1 << 8,
-};
-
-
-#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))
-
-
-typedef enum {
- DRV8711_DISABLED,
- DRV8711_IDLE,
- DRV8711_ACTIVE,
-} drv8711_state_t;
-
-
-typedef void (*stall_callback_t)(int driver);
-
-
-void drv8711_init();
-drv8711_state_t drv8711_get_state(int driver);
-void drv8711_set_state(int driver, drv8711_state_t state);
-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 - 2017 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 "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
- SET_PIN(FAULT_PIN, estop.triggered);
- 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_stop();
-
- // Set machine state
- mp_state_estop();
-
- // 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 - 2017 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 - 2017 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_expr.h"
-
-#include "gcode_parser.h"
-#include "vars.h"
-
-#include <math.h>
-#include <ctype.h>
-#include <stdlib.h>
-
-
-float parse_gcode_number(char **p) {
- // Avoid parsing G0X10 as a hexadecimal number
- if (**p == '0' && toupper(*(*p + 1)) == 'X') {
- (*p)++; // pointer points to X
- return 0;
- }
-
- // Skip leading zeros so we don't parse as octal
- while (**p == '0' && isdigit(*(*p + 1))) p++;
-
- // Parse number
- char *end;
- float value = strtod(*p, &end);
- if (end == *p) {
- parser.error = STAT_BAD_NUMBER_FORMAT;
- return 0;
- }
- *p = end; // Point to next character after the word
-
- return value;
-}
-
-
-static float _parse_gcode_var(char **p) {
- (*p)++; // Skip #
-
- if (isdigit(**p)) {
- // TODO numbered parameters
- parser.error = STAT_GCODE_NUM_PARAM_UNSUPPORTED;
-
- } else if (**p == '<') {
- (*p)++;
-
- // Assigning vars is not supported so the '_' global prefix is optional
- if (**p == '_') (*p)++;
-
- char *name = *p;
- while (**p && **p != '>') (*p)++;
-
- if (**p != '>') parser.error = STAT_GCODE_UNTERMINATED_VAR;
- else {
- *(*p)++ = 0; // Null terminate
- return vars_get_number(name);
- }
- }
-
- return 0;
-}
-
-
-static float _parse_gcode_func(char **p) {
- // TODO LinuxCNC supports GCode functions: ATAN, ABS, ACOS, ASIN, COS, EXP,
- // FIX, FUP, ROUND, LN, SIN, TAN & EXISTS.
- // See http://linuxcnc.org/docs/html/gcode/overview.html#gcode:functions
- parser.error = STAT_GCODE_FUNC_UNSUPPORTED;
- return 0;
-}
-
-
-static int _op_precedence(op_t op) {
- switch (op) {
- case OP_INVALID: break;
- case OP_MINUS: return 6;
- case OP_EXP: return 5;
- case OP_MUL: case OP_DIV: case OP_MOD: return 4;
- case OP_ADD: case OP_SUB: return 3;
- case OP_EQ: case OP_NE: case OP_GT: case OP_GE: case OP_LT: case OP_LE:
- return 2;
- case OP_AND: case OP_OR: case OP_XOR: return 1;
- }
- return 0;
-}
-
-
-static op_t _parse_gcode_op(char **_p) {
- char *p = *_p;
- op_t op = OP_INVALID;
-
- switch (toupper(p[0])) {
- case '*': op = p[1] == '*' ? OP_EXP : OP_MUL; break;
- case '/': op = OP_DIV; break;
-
- case 'M':
- if (toupper(p[1]) == 'O' && toupper(p[1]) == 'O') op = OP_EXP;
- break;
-
- case '+': op = OP_ADD; break;
- case '-': op = OP_SUB; break;
-
- case 'E': if (toupper(p[1]) == 'Q') op = OP_EQ; break;
- case 'N': if (toupper(p[1]) == 'E') op = OP_NE; break;
-
- case 'G':
- if (toupper(p[1]) == 'T') op = OP_GT;
- if (toupper(p[1]) == 'E') op = OP_GE;
- break;
-
- case 'L':
- if (toupper(p[1]) == 'T') op = OP_LT;
- if (toupper(p[1]) == 'E') op = OP_LE;
- break;
-
- case 'A':
- if (toupper(p[1]) == 'N' && toupper(p[2]) == 'D') op = OP_AND;
- break;
-
- case 'O': if (toupper(p[1]) == 'R') op = OP_OR; break;
-
- case 'X':
- if (toupper(p[1]) == 'O' && toupper(p[2]) == 'R') op = OP_XOR;
- break;
- }
-
- // Advance pointer
- switch (op) {
- case OP_INVALID: break;
- case OP_MINUS: case OP_MUL: case OP_DIV: case OP_ADD:
- case OP_SUB: *_p += 1; break;
- case OP_EXP: case OP_EQ: case OP_NE: case OP_GT: case OP_GE: case OP_LT:
- case OP_LE: case OP_OR: *_p += 2; break;
- case OP_MOD: case OP_AND: case OP_XOR: *_p += 3; break;
- }
-
- return op;
-}
-
-
-static float _apply_binary(op_t op, float left, float right) {
- switch (op) {
- case OP_INVALID: case OP_MINUS: return 0;
-
- case OP_EXP: return pow(left, right);
-
- case OP_MUL: return left * right;
- case OP_DIV: return left / right;
- case OP_MOD: return fmod(left, right);
-
- case OP_ADD: return left + right;
- case OP_SUB: return left - right;
-
- case OP_EQ: return left == right;
- case OP_NE: return left != right;
- case OP_GT: return left > right;
- case OP_GE: return left >= right;
- case OP_LT: return left > right;
- case OP_LE: return left <= right;
-
- case OP_AND: return left && right;
- case OP_OR: return left || right;
- case OP_XOR: return (bool)left ^ (bool)right;
- }
-
- return 0;
-}
-
-
-static void _val_push(float val) {
- if (parser.valPtr < GCODE_MAX_VALUE_DEPTH) parser.vals[parser.valPtr++] = val;
- else parser.error = STAT_EXPR_VALUE_STACK_OVERFLOW;
-}
-
-
-static float _val_pop() {
- if (parser.valPtr) return parser.vals[--parser.valPtr];
- parser.error = STAT_EXPR_VALUE_STACK_UNDERFLOW;
- return 0;
-}
-
-
-static bool _op_empty() {return !parser.opPtr;}
-
-
-static void _op_push(op_t op) {
- if (parser.opPtr < GCODE_MAX_OPERATOR_DEPTH) parser.ops[parser.opPtr++] = op;
- else parser.error = STAT_EXPR_OP_STACK_OVERFLOW;
-}
-
-
-static op_t _op_pop() {
- if (parser.opPtr) return parser.ops[--parser.opPtr];
- parser.error = STAT_EXPR_OP_STACK_UNDERFLOW;
- return OP_INVALID;
-}
-
-
-static op_t _op_top() {
- if (parser.opPtr) return parser.ops[parser.opPtr - 1];
- parser.error = STAT_EXPR_OP_STACK_UNDERFLOW;
- return OP_INVALID;
-}
-
-
-static void _op_apply() {
- op_t op = _op_pop();
-
- if (op == OP_MINUS) _val_push(-_val_pop());
-
- else {
- float right = _val_pop();
- float left = _val_pop();
-
- _val_push(_apply_binary(op, left, right));
- }
-}
-
-
-// Parse expressions with Dijkstra's Shunting Yard Algorithm
-float parse_gcode_expression(char **p) {
- bool unary = true; // Used to detect unary minus
-
- while (!parser.error && **p) {
- switch (**p) {
- case ' ': case '\n': case '\r': case '\t': (*p)++; break;
- case '#': _val_push(_parse_gcode_var(p)); unary = false; break;
- case '[': _op_push(OP_INVALID); (*p)++; unary = true; break;
-
- case ']':
- (*p)++;
-
- while (!parser.error && _op_top() != OP_INVALID)
- _op_apply();
-
- _op_pop(); // Pop opening bracket
- if (_op_empty() && parser.valPtr == 1) return _val_pop();
- unary = false;
- break;
-
- default:
- if (isdigit(**p) || **p == '.') {
- _val_push(parse_gcode_number(p));
- unary = false;
-
- } else if (isalpha(**p)) {
- _val_push(_parse_gcode_func(p));
- unary = false;
-
- } else {
- op_t op = _parse_gcode_op(p);
-
- if (unary && op == OP_ADD) continue; // Ignore it
- if (unary && op == OP_SUB) {_op_push(OP_MINUS); continue;}
-
- if (op == OP_INVALID) parser.error = STAT_INVALID_OR_MALFORMED_COMMAND;
- else {
- int precedence = _op_precedence(op);
-
- while (!parser.error && !_op_empty() &&
- precedence <= _op_precedence(_op_top()))
- _op_apply();
-
- _op_push(op);
- unary = true;
- }
- }
- }
- }
-
- return _val_pop();
-}
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2017 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
-
-
-float parse_gcode_number(char **p);
-float parse_gcode_expression(char **p);
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2017 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 "gcode_expr.h"
-#include "machine.h"
-#include "plan/arc.h"
-#include "axis.h"
-#include "util.h"
-
-#include <stdbool.h>
-#include <string.h>
-#include <ctype.h>
-#include <stdlib.h>
-#include <math.h>
-#include <stdio.h>
-
-
-parser_t parser = {{0}};
-
-
-#define SET_MODAL(m, parm, val) \
- {parser.gn.parm = val; parser.gf.parm = true; parser.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)
-
-
-// NOTE Nested comments are not allowed. E.g. (msg (hello))
-static char *_parse_gcode_comment(char *p) {
- char *msg = 0;
-
- p++; // Skip leading paren
-
- while (isspace(*p)) p++; // skip whitespace
-
- // Look for "(MSG"
- if (tolower(*(p + 0)) == 'm' &&
- tolower(*(p + 1)) == 's' &&
- tolower(*(p + 2)) == 'g') {
- p += 3;
- while (isspace(*p)) p++; // skip whitespace
- if (*p && *p != ')') msg = p;
- }
-
- // Find end
- while (*p && *p != ')') p++;
- *p = 0; // Terminate string
-
- // Queue message
- if (msg) mach_message(msg);
-
- return p;
-}
-
-
-static stat_t _parse_gcode_value(char **p, float *value) {
- while (isspace(**p)) (*p)++; // skip whitespace
-
- if (**p == '[') *value = parse_gcode_expression(p);
- else *value = parse_gcode_number(p);
-
- return parser.error;
-}
-
-
-/// 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 (parser.modals[MODAL_GROUP_G0] && parser.modals[MODAL_GROUP_G1])
- return STAT_MODAL_GROUP_VIOLATION;
-
-#if 0 // TODO 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 (parser.modals[MODAL_GROUP_G0] || parser.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) // TODO update this
- * 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_CLEAR_HOME: // G28.2
- mach_clear_home(parser.gf.target);
- break;
- case NEXT_ACTION_SET_HOME: // G28.3
- mach_set_home(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,
- parser.modals[MODAL_GROUP_G1],
- parser.gn.motion_mode);
- break;
- case MOTION_MODE_STRAIGHT_PROBE_CLOSE_ERR: // G38.2
- status = mach_probe(parser.gn.target, parser.gf.target,
- parser.gn.motion_mode);
- break;
- case MOTION_MODE_STRAIGHT_PROBE_CLOSE: // G38.3
- status = mach_probe(parser.gn.target, parser.gf.target,
- parser.gn.motion_mode);
- break;
- case MOTION_MODE_STRAIGHT_PROBE_OPEN_ERR: // G38.4
- status = mach_probe(parser.gn.target, parser.gf.target,
- parser.gn.motion_mode);
- break;
- case MOTION_MODE_STRAIGHT_PROBE_OPEN: // G38.5
- status = mach_probe(parser.gn.target, parser.gf.target,
- parser.gn.motion_mode);
- break;
- case MOTION_MODE_SEEK_CLOSE_ERR: // G38.6
- status = mach_seek(parser.gn.target, parser.gf.target,
- parser.gn.motion_mode);
- break;
- case MOTION_MODE_SEEK_CLOSE: // G38.7
- status = mach_seek(parser.gn.target, parser.gf.target,
- parser.gn.motion_mode);
- break;
- case MOTION_MODE_SEEK_OPEN_ERR: // G38.8
- status = mach_seek(parser.gn.target, parser.gf.target,
- parser.gn.motion_mode);
- break;
- case MOTION_MODE_SEEK_OPEN: // G38.9
- status = mach_seek(parser.gn.target, parser.gf.target,
- 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;
-}
-
-
-/// Load the state values in gn (next model state) and set flags in gf (model
-/// state flags).
-static stat_t _process_gcode_word(char letter, float value) {
- 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_CLEAR_HOME);
- case 3: SET_NON_MODAL(next_action, NEXT_ACTION_SET_HOME);
- default: return 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: return STAT_GCODE_COMMAND_UNSUPPORTED;
- }
- break;
-
- case 38:
- switch (_point(value)) {
- case 2: SET_MODAL(MODAL_GROUP_G1, motion_mode,
- MOTION_MODE_STRAIGHT_PROBE_CLOSE_ERR);
- case 3: SET_MODAL(MODAL_GROUP_G1, motion_mode,
- MOTION_MODE_STRAIGHT_PROBE_CLOSE);
- case 4: SET_MODAL(MODAL_GROUP_G1, motion_mode,
- MOTION_MODE_STRAIGHT_PROBE_OPEN_ERR);
- case 5: SET_MODAL(MODAL_GROUP_G1, motion_mode,
- MOTION_MODE_STRAIGHT_PROBE_OPEN);
- case 6: SET_MODAL(MODAL_GROUP_G1, motion_mode,
- MOTION_MODE_SEEK_CLOSE_ERR);
- case 7: SET_MODAL(MODAL_GROUP_G1, motion_mode, MOTION_MODE_SEEK_CLOSE);
- case 8: SET_MODAL(MODAL_GROUP_G1, motion_mode,
- MOTION_MODE_SEEK_OPEN_ERR);
- case 9: SET_MODAL(MODAL_GROUP_G1, motion_mode, MOTION_MODE_SEEK_OPEN);
- default: return 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: return 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:
- 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: return 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: return 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: return 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: return 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: return 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: return STAT_GCODE_COMMAND_UNSUPPORTED;
- }
-
- return STAT_OK;
-}
-
-
-/// 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) {
-#ifdef DEBUG
- printf("GCODE: %s\n", block);
-#endif
-
- // Delete block if it starts with /
- if (*block == '/') return STAT_NOOP;
-
- // Set initial state for new block
- // 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
- memset(&parser, 0, sizeof(parser)); // clear all parser values
-
- // get motion mode from previous block
- parser.gn.motion_mode = mach_get_motion_mode();
-
- // Parse words
- for (char *p = block; *p;) {
- switch (*p) {
- case ' ': case '\t': case '\r': case '\n': p++; break; // Skip whitespace
- case '(': p = _parse_gcode_comment(p); break;
- case ';': *p = 0; break; // Comment
-
- default: {
- char letter = toupper(*p++);
- float value = 0;
- if (!isalpha(letter)) return STAT_INVALID_OR_MALFORMED_COMMAND;
- RITORNO(_parse_gcode_value(&p, &value));
- RITORNO(_process_gcode_word(letter, value));
- }
- }
- }
-
- RITORNO(_validate_gcode_block());
-
- return _execute_gcode_block();
-}
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2017 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 enum {
- OP_INVALID,
- OP_MINUS,
- OP_EXP,
- OP_MUL, OP_DIV, OP_MOD,
- OP_ADD, OP_SUB,
- OP_EQ, OP_NE, OP_GT,OP_GE, OP_LT, OP_LE,
- OP_AND, OP_OR, OP_XOR,
-} op_t;
-
-
-typedef struct {
- gcode_state_t gn; // gcode input values
- gcode_flags_t gf; // gcode input flags
-
- uint8_t modals[MODAL_GROUP_COUNT]; // collects modal groups in a block
-
- op_t ops[GCODE_MAX_OPERATOR_DEPTH];
- float vals[GCODE_MAX_VALUE_DEPTH];
- int opPtr;
- int valPtr;
-
- stat_t error;
-} 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 - 2017 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"
-
-
-static const char INVALID_PGMSTR[] PROGMEM = "INVALID";
-
-static const char INCHES_PGMSTR[] PROGMEM = "IN";
-static const char MILLIMETERS_PGMSTR[] PROGMEM = "MM";
-static const char DEGREES_PGMSTR[] PROGMEM = "DEG";
-
-static const char INVERSE_TIME_MODE_PGMSTR[] PROGMEM = "INVERSE TIME";
-static const char UNITS_PER_MINUTE_MODE_PGMSTR[] PROGMEM = "PER MIN";
-static const char UNITS_PER_REVOLUTION_MODE_PGMSTR[] PROGMEM = "PER REV";
-
-static const char PLANE_XY_PGMSTR[] PROGMEM = "XY";
-static const char PLANE_XZ_PGMSTR[] PROGMEM = "XZ";
-static const char PLANE_YZ_PGMSTR[] PROGMEM = "YZ";
-
-static const char ABSOLUTE_COORDS_PGMSTR[] PROGMEM = "ABS";
-static const char G54_PGMSTR[] PROGMEM = "G54";
-static const char G55_PGMSTR[] PROGMEM = "G55";
-static const char G56_PGMSTR[] PROGMEM = "G56";
-static const char G57_PGMSTR[] PROGMEM = "G57";
-static const char G58_PGMSTR[] PROGMEM = "G58";
-static const char G59_PGMSTR[] PROGMEM = "G59";
-
-static const char PATH_EXACT_PATH_PGMSTR[] PROGMEM = "EXACT PATH";
-static const char PATH_EXACT_STOP_PGMSTR[] PROGMEM = "EXACT STOP";
-static const char PATH_CONTINUOUS_PGMSTR[] PROGMEM = "CONTINUOUS";
-
-static const char ABSOLUTE_MODE_PGMSTR[] PROGMEM = "ABSOLUTE";
-static const char INCREMENTAL_MODE_PGMSTR[] PROGMEM = "INCREMENTAL";
-
-
-PGM_P gs_get_units_pgmstr(units_t mode) {
- switch (mode) {
- case INCHES: return INCHES_PGMSTR;
- case MILLIMETERS: return MILLIMETERS_PGMSTR;
- case DEGREES: return DEGREES_PGMSTR;
- }
-
- return INVALID_PGMSTR;
-}
-
-
-units_t gs_parse_units(const char *s) {
- if (!strcmp_P(s, INCHES_PGMSTR)) return INCHES;
- if (!strcmp_P(s, MILLIMETERS_PGMSTR)) return MILLIMETERS;
- if (!strcmp_P(s, DEGREES_PGMSTR)) return DEGREES;
- return -1;
-}
-
-
-PGM_P gs_get_feed_mode_pgmstr(feed_mode_t mode) {
- switch (mode) {
- case INVERSE_TIME_MODE: return INVERSE_TIME_MODE_PGMSTR;
- case UNITS_PER_MINUTE_MODE: return UNITS_PER_MINUTE_MODE_PGMSTR;
- case UNITS_PER_REVOLUTION_MODE: return UNITS_PER_REVOLUTION_MODE_PGMSTR;
- }
-
- return INVALID_PGMSTR;
-}
-
-
-feed_mode_t gs_parse_feed_mode(const char *s) {
- if (!strcmp_P(s, INVERSE_TIME_MODE_PGMSTR)) return INVERSE_TIME_MODE;
- if (!strcmp_P(s, UNITS_PER_MINUTE_MODE_PGMSTR)) return UNITS_PER_MINUTE_MODE;
- if (!strcmp_P(s, UNITS_PER_REVOLUTION_MODE_PGMSTR))
- return UNITS_PER_REVOLUTION_MODE;
- return -1;
-}
-
-
-PGM_P gs_get_plane_pgmstr(plane_t plane) {
- switch (plane) {
- case PLANE_XY: return PLANE_XY_PGMSTR;
- case PLANE_XZ: return PLANE_XZ_PGMSTR;
- case PLANE_YZ: return PLANE_YZ_PGMSTR;
- }
-
- return INVALID_PGMSTR;
-}
-
-
-plane_t gs_parse_plane(const char *s) {
- if (!strcmp_P(s, PLANE_XY_PGMSTR)) return PLANE_XY;
- if (!strcmp_P(s, PLANE_XZ_PGMSTR)) return PLANE_XZ;
- if (!strcmp_P(s, PLANE_YZ_PGMSTR)) return PLANE_YZ;
- return -1;
-}
-
-
-PGM_P gs_get_coord_system_pgmstr(coord_system_t cs) {
- switch (cs) {
- case ABSOLUTE_COORDS: return ABSOLUTE_COORDS_PGMSTR;
- case G54: return G54_PGMSTR;
- case G55: return G55_PGMSTR;
- case G56: return G56_PGMSTR;
- case G57: return G57_PGMSTR;
- case G58: return G58_PGMSTR;
- case G59: return G59_PGMSTR;
- }
-
- return INVALID_PGMSTR;
-}
-
-
-coord_system_t gs_parse_coord_system(const char *s) {
- if (!strcmp_P(s, ABSOLUTE_COORDS_PGMSTR)) return ABSOLUTE_COORDS;
- if (!strcmp_P(s, G54_PGMSTR)) return G54;
- if (!strcmp_P(s, G55_PGMSTR)) return G55;
- if (!strcmp_P(s, G56_PGMSTR)) return G56;
- if (!strcmp_P(s, G57_PGMSTR)) return G57;
- if (!strcmp_P(s, G58_PGMSTR)) return G58;
- if (!strcmp_P(s, G59_PGMSTR)) return G59;
- return -1;
-}
-
-
-PGM_P gs_get_path_mode_pgmstr(path_mode_t mode) {
- switch (mode) {
- case PATH_EXACT_PATH: return PATH_EXACT_PATH_PGMSTR;
- case PATH_EXACT_STOP: return PATH_EXACT_STOP_PGMSTR;
- case PATH_CONTINUOUS: return PATH_CONTINUOUS_PGMSTR;
- }
-
- return INVALID_PGMSTR;
-}
-
-
-path_mode_t gs_parse_path_mode(const char *s) {
- if (!strcmp_P(s, PATH_EXACT_PATH_PGMSTR)) return PATH_EXACT_PATH;
- if (!strcmp_P(s, PATH_EXACT_STOP_PGMSTR)) return PATH_EXACT_STOP;
- if (!strcmp_P(s, PATH_CONTINUOUS_PGMSTR)) return PATH_CONTINUOUS;
- return -1;
-}
-
-
-PGM_P gs_get_distance_mode_pgmstr(distance_mode_t mode) {
- switch (mode) {
- case ABSOLUTE_MODE: return ABSOLUTE_MODE_PGMSTR;
- case INCREMENTAL_MODE: return INCREMENTAL_MODE_PGMSTR;
- }
-
- return INVALID_PGMSTR;
-}
-
-
-distance_mode_t gs_parse_distance_mode(const char *s) {
- if (!strcmp_P(s, ABSOLUTE_MODE_PGMSTR)) return ABSOLUTE_MODE;
- if (!strcmp_P(s, INCREMENTAL_MODE_PGMSTR)) return INCREMENTAL_MODE;
- return -1;
-}
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2017 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 - 2017 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 "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
- */
-
-typedef enum {
- NEXT_ACTION_DEFAULT, // Must be zero (invokes motion modes)
- NEXT_ACTION_DWELL, // G4
- NEXT_ACTION_SET_COORD_DATA, // G10
- NEXT_ACTION_GOTO_G28_POSITION, // G28 go to machine position
- NEXT_ACTION_SET_G28_POSITION, // G28.1 set position in abs coordinates
- NEXT_ACTION_CLEAR_HOME, // G28.3 clear axis home
- NEXT_ACTION_SET_HOME, // G28.3 set axis home position
- NEXT_ACTION_GOTO_G30_POSITION, // G30
- NEXT_ACTION_SET_G30_POSITION, // G30.1
- 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_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_STRAIGHT_PROBE_CLOSE_ERR, // G38.2
- MOTION_MODE_STRAIGHT_PROBE_CLOSE, // G38.3
- MOTION_MODE_STRAIGHT_PROBE_OPEN_ERR, // G38.4
- MOTION_MODE_STRAIGHT_PROBE_OPEN, // G38.5
- MOTION_MODE_SEEK_CLOSE_ERR, // G38.6
- MOTION_MODE_SEEK_CLOSE, // G38.7
- MOTION_MODE_SEEK_OPEN_ERR, // G38.8
- MOTION_MODE_SEEK_OPEN, // G38.9
- MOTION_MODE_CANCEL_MOTION_MODE, // G80
- 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, man 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 {
- UNITS_PER_MINUTE_MODE, // G94
- INVERSE_TIME_MODE, // G93
- 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;
-
-
-/* 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);
-units_t gs_parse_units(const char *units);
-PGM_P gs_get_feed_mode_pgmstr(feed_mode_t mode);
-feed_mode_t gs_parse_feed_mode(const char *mode);
-PGM_P gs_get_plane_pgmstr(plane_t plane);
-plane_t gs_parse_plane(const char *plane);
-PGM_P gs_get_coord_system_pgmstr(coord_system_t cs);
-coord_system_t gs_parse_coord_system(const char *cs);
-PGM_P gs_get_path_mode_pgmstr(path_mode_t mode);
-path_mode_t gs_parse_path_mode(const char *mode);
-PGM_P gs_get_distance_mode_pgmstr(distance_mode_t mode);
-distance_mode_t gs_parse_distance_mode(const char *mode);
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2017 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 "pgmspace.h"
-
-#include <avr/interrupt.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]
-
-
-static void _init_clock() {
-#if 0 // 32Mhz Int RC
- OSC.CTRL |= OSC_RC32MEN_bm | OSC_RC32KEN_bm; // Enable 32MHz & 32KHz osc
- while (!(OSC.STATUS & OSC_RC32KRDY_bm)); // Wait for 32Khz oscillator
- while (!(OSC.STATUS & OSC_RC32MRDY_bm)); // Wait for 32MHz oscillator
-
- // Defaults to calibrate against internal 32Khz clock
- DFLLRC32M.CTRL = DFLL_ENABLE_bm; // Enable DFLL
- CCP = CCP_IOREG_gc; // Disable register security
- CLK.CTRL = CLK_SCLKSEL_RC32M_gc; // Switch to 32MHz clock
-
-#else
- // 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
-#endif
-
- OSC.CTRL &= ~OSC_RC2MEN_bm; // disable internal 2 MHz clock
-}
-
-
-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 (!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 - 2017 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 - 2017 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 "pgmspace.h"
-
-#include <avr/io.h>
-#include <avr/interrupt.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_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;
- spindle_mode_t mode;
- float speed;
-
- float actual_freq;
- float actual_current;
- uint16_t actual_rpm;
- 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) {SET_PIN(RS485_RW_PIN, x);}
-
-
-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.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_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) {
- 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_stop() {
- huanyang_set(SPINDLE_OFF, 0);
- huanyang_reset();
-}
-
-
-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_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 - 2017 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_stop();
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2017 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 - 2017 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_REBOOT,
-} 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 - 2017 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 "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 = {
- .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;}
-bool mach_in_absolute_mode() {return mach.gm.distance_mode == ABSOLUTE_MODE;}
-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 unit conversion
- * is performed.
- */
-float mach_get_axis_position(uint8_t axis) {return mach.position[axis];}
-
-
-/* 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 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
- * 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;
-
- // TODO should set work position, accounting for offsets
-
- mach.position[axis] = position;
- mp_set_axis_position(axis, position);
- mp_runtime_set_axis_position(axis, position);
- mp_runtime_set_steps_from_position();
-}
-
-
-/// Do not call this function while machine is moving or queue is not empty
-void mach_set_position_from_runtime() {
- for (int axis = 0; axis < AXES; axis++) {
- mach.position[axis] = mp_runtime_get_work_position(axis);
- mp_set_axis_position(axis, mach.position[axis]);
- }
-}
-
-
-/*** 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[], bool absolute) {
- for (int axis = 0; axis < AXES; axis++) {
- target[axis] = mach.position[axis];
- if (!flags[axis] || !axis_is_enabled(axis)) continue;
-
- target[axis] = absolute ? 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
- // TODO This should depend on the axis mode
- else if (AXIS_Z < axis) target[axis] += values[axis];
- else target[axis] += TO_MM(values[axis]);
- }
-}
-
-
-/*** Return error code if soft limit is exceeded
- *
- * Tests for soft limit for any axis if min and max are different values. You
- * can set min and max to 0 to disable soft limits for an axis.
- */
-stat_t mach_test_soft_limits(float target[]) {
- for (int axis = 0; axis < AXES; axis++) {
- if (!axis_is_enabled(axis) || !axis_get_homed(axis)) continue;
-
- 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 (target[axis] < min || max < target[axis])
- 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) {
- if (plane != (plane_t)-1) mach.gm.plane = plane;
-}
-
-
-/// G20, G21
-void mach_set_units(units_t mode) {
- if (mode != (units_t)-1) mach.gm.units = mode;
-}
-
-
-/// G90, G91
-void mach_set_distance_mode(distance_mode_t mode) {
- if (mode != (distance_mode_t)-1) mach.gm.distance_mode = mode;
-}
-
-
-/// G90.1, G91.1
-void mach_set_arc_distance_mode(distance_mode_t mode) {
- if (mode != (distance_mode_t)-1) 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 cs) {
- if (cs != (coord_system_t)-1) mach.gm.coord_system = cs;
-}
-
-
-// G28.3 functions and support
-static stat_t _exec_home(mp_buffer_t *bf) {
- const float *target = bf->target;
- const float *flags = bf->unit;
-
- for (int axis = 0; axis < AXES; axis++)
- if (flags[axis]) {
- mp_runtime_set_axis_position(axis, target[axis]);
- axis_set_homed(axis, true);
- }
-
- 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.
- */
-void mach_set_home(float origin[], bool flags[]) {
- mp_buffer_t *bf = mp_queue_get_tail();
-
- // Compute target position
- mach_calc_target(bf->target, origin, flags, true);
-
- for (int axis = 0; axis < AXES; axis++)
- if (flags[axis] && isfinite(origin[axis])) {
- bf->target[axis] -= mach_get_active_coord_offset(axis);
- mach.position[axis] = bf->target[axis];
- mp_set_axis_position(axis, bf->target[axis]); // set mm position
- bf->unit[axis] = true;
-
- } else bf->unit[axis] = false;
-
- // Synchronized update of runtime position
- mp_queue_push_nonstop(_exec_home, mach_get_line());
-}
-
-
-void mach_clear_home(bool flags[]) {
- for (int axis = 0; axis < AXES; axis++)
- if (flags[axis]) axis_set_homed(axis, false);
-}
-
-
-/* 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[]) {
- 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]);
-
- mach_update_work_offsets(); // update resolved offsets
-}
-
-
-/// G92.1
-void mach_reset_origin_offsets() {
- mach.origin_offset_enable = false;
-
- for (int axis = 0; axis < AXES; axis++)
- mach.origin_offset[axis] = 0;
-
- mach_update_work_offsets(); // update resolved offsets
-}
-
-
-/// G92.2
-void mach_suspend_origin_offsets() {
- mach.origin_offset_enable = false;
- mach_update_work_offsets(); // update resolved offsets
-}
-
-
-/// G92.3
-void mach_resume_origin_offsets() {
- mach.origin_offset_enable = true;
- mach_update_work_offsets(); // update resolved offsets
-}
-
-
-stat_t mach_plan_line(float target[], switch_id_t sw) {
- buffer_flags_t flags = 0;
-
- switch (mach_get_motion_mode()) {
- case MOTION_MODE_STRAIGHT_PROBE_CLOSE_ERR:
- case MOTION_MODE_SEEK_CLOSE_ERR:
- flags |= BUFFER_SEEK_CLOSE | BUFFER_SEEK_ERROR | BUFFER_EXACT_STOP;
- break;
-
- case MOTION_MODE_STRAIGHT_PROBE_CLOSE:
- case MOTION_MODE_SEEK_CLOSE:
- flags |= BUFFER_SEEK_CLOSE | BUFFER_EXACT_STOP;
- break;
-
- case MOTION_MODE_STRAIGHT_PROBE_OPEN_ERR:
- case MOTION_MODE_SEEK_OPEN_ERR:
- flags |= BUFFER_SEEK_OPEN | BUFFER_SEEK_ERROR | BUFFER_EXACT_STOP;
- break;
-
- case MOTION_MODE_STRAIGHT_PROBE_OPEN:
- case MOTION_MODE_SEEK_OPEN:
- flags |= BUFFER_SEEK_OPEN | BUFFER_EXACT_STOP;
- break;
-
- default: break;
- }
-
- if (mach_is_rapid()) flags |= BUFFER_RAPID;
- if (mach_is_inverse_time_mode()) flags |= BUFFER_INVERSE_TIME;
- if (mach_is_exact_stop()) flags |= BUFFER_EXACT_STOP;
-
- return mp_aline(target, flags, sw, 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[], switch_id_t sw) {
- // Trap inverse time mode wo/ feed rate
- if (!mach_is_rapid() && mach.gm.feed_mode == INVERSE_TIME_MODE &&
- !parser.gf.feed_rate)
- return STAT_GCODE_FEEDRATE_NOT_SPECIFIED;
-
- // Compute target position
- float target[AXES];
- mach_calc_target(target, values, flags, mach_in_absolute_mode());
-
- // 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
- RITORNO(mach_plan_line(target, sw));
- copy_vector(mach.position, target); // update model position
-
- return STAT_OK;
-}
-
-
-/// G0 linear rapid
-stat_t mach_rapid(float values[], bool flags[]) {
- mach_set_motion_mode(MOTION_MODE_RAPID);
- return _feed(values, flags, 0);
-}
-
-
-/// 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);
-}
-
-
-stat_t mach_probe(float values[], bool flags[], motion_mode_t mode) {
- mach_set_motion_mode(mode);
- return _feed(values, flags, SW_PROBE);
-}
-
-
-stat_t _exec_set_seek_position(mp_buffer_t *bf) {
- mach_set_position_from_runtime();
- mp_pause_queue(false);
- return STAT_NOOP; // No move queued
-}
-
-
-stat_t mach_seek(float target[], bool flags[], motion_mode_t mode) {
- mach_set_motion_mode(mode);
-
- switch_id_t sw = SW_PROBE;
-
- for (int axis = 0; axis < AXES; axis++)
- if (flags[axis] && isfinite(target[axis])) {
- // Convert to incremental move
- if (mach.gm.distance_mode == ABSOLUTE_MODE)
- target[axis] += mach.position[axis];
-
- if (!axis_is_enabled(axis)) return STAT_SEEK_AXIS_DISABLED;
- if (sw != SW_PROBE) return STAT_SEEK_MULTIPLE_AXES;
- if (fp_EQ(target[axis], mach.position[axis])) return STAT_SEEK_ZERO_MOVE;
-
- bool min = target[axis] < mach.position[axis];
-
- if (mode == MOTION_MODE_SEEK_OPEN_ERR ||
- mode == MOTION_MODE_SEEK_OPEN) min = !min;
-
- switch (axis) {
- case AXIS_X: sw = min ? SW_MIN_X : SW_MAX_X; break;
- case AXIS_Y: sw = min ? SW_MIN_Y : SW_MAX_Y; break;
- case AXIS_Z: sw = min ? SW_MIN_Z : SW_MAX_Z; break;
- case AXIS_A: sw = min ? SW_MIN_A : SW_MAX_A; break;
- }
-
- if (!switch_is_enabled(sw)) return STAT_SEEK_SWITCH_DISABLED;
- }
-
- if (sw == SW_PROBE) return STAT_SEEK_MISSING_AXIS;
-
- RITORNO(_feed(target, flags, sw));
-
- mp_pause_queue(true);
- mp_queue_push_nonstop(_exec_set_seek_position, mach_get_line());
-
- return STAT_OK;
-}
-
-
-// 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 (mode == (feed_mode_t)-1 || 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) {
- if (mode != (path_mode_t)-1) mach.gm.path_mode = mode;
-}
-
-
-// Machining Functions (4.3.6) see also arc.c
-
-/// G1
-stat_t mach_feed(float values[], bool flags[]) {
- mach_set_motion_mode(MOTION_MODE_FEED);
- return _feed(values, flags, 0);
-}
-
-
-/// G4, P parameter (seconds)
-stat_t mach_dwell(float seconds) {return mp_dwell(seconds, mach_get_line());}
-
-
-// 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
- */
-
-void mach_set_feed_override(float value) {
- mach.gm.feed_override = value;
- mach.gm.feed_override_enable = !fp_ZERO(value);
-}
-
-
-void mach_set_spindle_override(float value) {
- mach.gm.spindle_override = value;
- mach.gm.spindle_override_enable = !fp_ZERO(value);
-}
-
-
-/// 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 - 2017 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"
-#include "switch.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();
-bool mach_in_absolute_mode();
-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);
-void mach_set_axis_position(unsigned axis, float position);
-void mach_set_position_from_runtime();
-
-// Critical helpers
-void mach_calc_target(float target[], const float values[], const bool flags[],
- bool absolute);
-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_coord_system(coord_system_t coord_system);
-
-void mach_set_home(float origin[], bool flags[]);
-void mach_clear_home(bool flags[]);
-
-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[], switch_id_t sw);
-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[]);
-stat_t mach_probe(float target[], bool flags[], motion_mode_t mode);
-stat_t mach_seek(float target[], bool flags[], motion_mode_t mode);
-
-// 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_set_feed_override(float override);
-void mach_set_spindle_override(float override);
-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 - 2017 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 "i2c.h"
-#include "pgmspace.h"
-#include "outputs.h"
-
-#include "plan/planner.h"
-#include "plan/arc.h"
-#include "plan/state.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
- outputs_init(); // output pins
- 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"));
-
- // Main loop
- while (true) {
- hw_reset_handler(); // handle hard reset requests
- if (!estop_triggered()) {
- mp_state_callback();
- mach_arc_callback(); // arc generation runs
- }
- 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 - 2017 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")
-STAT_MSG(EXPR_VALUE_STACK_OVERFLOW, "Expression parser value overflow")
-STAT_MSG(EXPR_VALUE_STACK_UNDERFLOW, "Expression parser value underflow")
-STAT_MSG(EXPR_OP_STACK_OVERFLOW, "Expression parser operator overflow")
-STAT_MSG(EXPR_OP_STACK_UNDERFLOW, "Expression parser operator underflow")
-STAT_MSG(GCODE_FUNC_UNSUPPORTED, "GCode function call unsupported")
-STAT_MSG(GCODE_NUM_PARAM_UNSUPPORTED, "GCode numbered parameters unsupported")
-STAT_MSG(GCODE_UNTERMINATED_VAR, "GCode variable reference untermiated")
-
-// 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")
-
-// Seeking
-STAT_MSG(SEEK_MULTIPLE_AXES, "Seek error - multiple axes specified")
-STAT_MSG(SEEK_MISSING_AXIS, "Seek error - no axis specified")
-STAT_MSG(SEEK_AXIS_DISABLED, "Seek error - specified axis is disabled")
-STAT_MSG(SEEK_SWITCH_DISABLED, "Seek error - target switch is disabled")
-STAT_MSG(SEEK_ZERO_MOVE, "Seek error - axis move too short or zero")
-STAT_MSG(SEEK_SWTICH_NOT_FOUND, "Seek error - move end before switch change")
-
-// End of stats marker
-STAT_MSG(MAX, "")
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2017 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 "pgmspace.h"
-
-#include "plan/runtime.h"
-
-#include <util/delay.h>
-
-#include <string.h>
-#include <math.h>
-#include <stdio.h>
-#include <stdlib.h>
-
-
-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;
-
- // Computed
- float steps_per_unit;
-
- // Runtime state
- uint32_t power_timeout;
- int32_t commanded;
- int32_t encoder;
- int16_t error;
- bool last_negative;
- uint8_t last_clock;
-
- // Move prep
- bool prepped;
- uint8_t timer_clock;
- uint16_t timer_period;
- bool negative;
- int32_t position;
-} motor_t;
-
-
-static motor_t motors[MOTORS] = {
- {
- .axis = AXIS_X,
- .step_pin = STEP_X_PIN,
- .dir_pin = DIR_X_PIN,
- .timer = &M1_TIMER,
- .dma = &M1_DMA_CH,
- .dma_trigger = M1_DMA_TRIGGER,
- }, {
- .axis = AXIS_Y,
- .step_pin = STEP_Y_PIN,
- .dir_pin = DIR_Y_PIN,
- .timer = &M2_TIMER,
- .dma = &M2_DMA_CH,
- .dma_trigger = M2_DMA_TRIGGER,
- }, {
- .axis = AXIS_Z,
- .step_pin = STEP_Z_PIN,
- .dir_pin = DIR_Z_PIN,
- .timer = &M3_TIMER,
- .dma = &M3_DMA_CH,
- .dma_trigger = M3_DMA_TRIGGER,
- }, {
- .axis = AXIS_A,
- .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;
-
-
-static void _update_config(int motor) {
- motor_t *m = &motors[motor];
-
- m->steps_per_unit = 360.0 * m->microsteps / m->travel_rev / m->step_angle;
-}
-
-
-void motor_init() {
- // Enable DMA
- DMA.CTRL = DMA_RESET_bm;
- DMA.CTRL = DMA_ENABLE_bm;
- DMA.INTFLAGS = 0xff; // clear all pending interrupts
-
- for (int motor = 0; motor < MOTORS; motor++) {
- motor_t *m = &motors[motor];
-
- _update_config(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;
-
- // 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->TRFCNT = 0xffff;
- m->dma->REPCNT = 0;
- m->dma->CTRLB = 0;
- m->dma->CTRLA = DMA_CH_SINGLE_bm | DMA_CH_BURSTLEN_1BYTE_gc;
-
- drv8711_set_microsteps(motor, m->microsteps);
- }
-}
-
-
-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_axis(int motor, uint8_t axis) {
- if (MOTORS <= motor || AXES <= axis || axis == motors[motor].axis) return;
- axis_set_motor(motors[motor].axis, -1);
- motors[motor].axis = axis;
- axis_set_motor(axis, motor);
-}
-
-
-float motor_get_steps_per_unit(int motor) {return motors[motor].steps_per_unit;}
-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;
- _update_config(motor);
- drv8711_set_microsteps(motor, microsteps);
-}
-
-
-void motor_set_position(int motor, int32_t position) {
- //if (st_is_busy()) ALARM(STAT_INTERNAL_ERROR); TODO
-
- motor_t *m = &motors[motor];
-
- m->commanded = m->encoder = m->position = position << 1; // We use half steps
- m->error = 0;
-}
-
-
-int32_t motor_get_position(int motor) {
- return motors[motor].position >> 1; // Convert from half to full steps
-}
-
-
-static void _update_power(int motor) {
- motor_t *m = &motors[motor];
-
- switch (m->power_mode) {
- case MOTOR_POWERED_ONLY_WHEN_MOVING:
- case MOTOR_POWERED_IN_CYCLE:
- if (rtc_expired(m->power_timeout)) {
- drv8711_set_state(motor, DRV8711_IDLE);
- break;
- }
- // Fall through
-
- case MOTOR_ALWAYS_POWERED:
- // TODO is ~5ms enough time to enable the motor?
- drv8711_set_state(motor, DRV8711_ACTIVE);
- break;
-
- default: // Disabled
- drv8711_set_state(motor, DRV8711_DISABLED);
- }
-}
-
-
-/// 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++)
- _update_power(motor);
-
- return STAT_OK;
-}
-
-
-void motor_end_move(int motor) {
- motor_t *m = &motors[motor];
-
- if (!m->timer->CTRLA) return;
-
- // Stop clock
- m->timer->CTRLA = 0;
-
- // Get actual step count from DMA channel
- const int24_t half_steps = 0xffff - m->dma->TRFCNT;
-
- // Accumulate encoder
- m->encoder += m->last_negative ? -half_steps : half_steps;
-
- // Compute error
- m->error = m->commanded - m->encoder;
-}
-
-
-void motor_load_move(int motor) {
- motor_t *m = &motors[motor];
-
- ASSERT(m->prepped);
-
- motor_end_move(motor);
-
- // Set direction, compensating for polarity
- const bool counterclockwise = m->negative ^ m->reverse;
- SET_PIN(m->dir_pin, counterclockwise);
-
- // Adjust clock count
- if (m->last_clock) {
- uint24_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 >> 1;
-
- m->timer->CNT = count;
-
- } else m->timer->CNT = m->timer_period >> 1;
-
- // Reset DMA channel counter
- m->dma->CTRLA &= ~DMA_CH_ENABLE_bm;
- m->dma->TRFCNT = 0xffff;
- m->dma->CTRLA |= DMA_CH_ENABLE_bm;
-
- // Set 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; // Save clock value
- m->timer_clock = 0; // Clear clock
- m->last_negative = m->negative;
- m->commanded = m->position;
-
- // Clear move
- m->prepped = false;
-}
-
-
-void motor_prep_move(int motor, float time, int32_t target) {
- motor_t *m = &motors[motor];
-
- // Validate input
- ASSERT(0 <= motor && motor < MOTORS);
- ASSERT(!m->prepped);
-
- // We count in half steps
- target = target << 1;
-
- // Compute travel in steps
- int24_t half_steps = target - m->position;
- m->position = target;
-
- // Error correction
- int16_t correction = abs(m->error);
- if (MIN_HALF_STEP_CORRECTION <= correction) {
- // Allowed step correction is proportional to velocity
- int24_t positive_half_steps = half_steps < 0 ? -half_steps : half_steps;
- int16_t max_correction = (positive_half_steps >> 5) + 1;
- if (max_correction < correction) correction = max_correction;
-
- if (m->error < 0) correction = -correction;
-
- half_steps += correction;
- m->error -= correction;
- }
-
- // Positive steps from here on
- m->negative = half_steps < 0;
- if (m->negative) half_steps = -half_steps;
-
- // Find the fastest clock rate that will fit the required number of steps.
- // Note, clock toggles step line so we need two clocks per step.
- uint24_t seg_clocks = time * F_CPU * 60;
- uint24_t ticks_per_step = seg_clocks / half_steps + 1; // Round up
- 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)) + 1; // Round up
-
- if (!m->timer_period || !half_steps) m->timer_clock = 0;
-
- // Power motor
- switch (m->power_mode) {
- case MOTOR_POWERED_ONLY_WHEN_MOVING:
- if (!m->timer_clock) break; // Not moving
- // Fall through
-
- case MOTOR_ALWAYS_POWERED: case MOTOR_POWERED_IN_CYCLE:
- // Reset timeout
- m->power_timeout = rtc_get_time() + MOTOR_IDLE_TIMEOUT * 1000;
- break;
-
- default: break;
- }
- _update_power(motor);
-
- // Queue move
- m->prepped = true;
-}
-
-
-// Var callbacks
-float get_step_angle(int motor) {return motors[motor].step_angle;}
-
-
-void set_step_angle(int motor, float value) {
- motors[motor].step_angle = value;
- _update_config(motor);
-}
-
-
-float get_travel(int motor) {return motors[motor].travel_rev;}
-
-
-void set_travel(int motor, float value) {
- motors[motor].travel_rev = value;
- _update_config(motor);
-}
-
-
-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;}
-char get_motor_axis(int motor) {return motors[motor].axis;}
-void set_motor_axis(int motor, uint8_t axis) {motor_set_axis(motor, axis);}
-
-
-uint8_t get_power_mode(int motor) {return motors[motor].power_mode;}
-
-
-void set_power_mode(int motor, uint8_t value) {
- if (value <= MOTOR_POWERED_ONLY_WHEN_MOVING)
- motors[motor].power_mode = value;
- else motors[motor].power_mode = MOTOR_DISABLED;
-}
-
-
-int32_t get_encoder(int m) {return motors[m].encoder;}
-int32_t get_error(int m) {return motors[m].error;}
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2017 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_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_t;
-
-
-void motor_init();
-
-bool motor_is_enabled(int motor);
-int motor_get_axis(int motor);
-float motor_get_steps_per_unit(int motor);
-uint16_t motor_get_microsteps(int motor);
-void motor_set_microsteps(int motor, uint16_t microsteps);
-void motor_set_position(int motor, int32_t position);
-int32_t motor_get_position(int motor);
-
-stat_t motor_rtc_callback();
-
-void motor_end_move(int motor);
-void motor_load_move(int motor);
-void motor_prep_move(int motor, float time, int32_t target);
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2017 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 "outputs.h"
-#include "config.h"
-
-
-typedef struct {
- uint8_t pin;
- bool active;
- output_state_t state;
- output_mode_t mode;
-} output_t;
-
-
-output_t outputs[OUTS] = {
- {TOOL_ENABLE_PIN},
- {TOOL_DIR_PIN},
- {SWITCH_1_PIN},
- {SWITCH_2_PIN},
- {FAULT_PIN},
-};
-
-
-static output_t *_get_output(uint8_t pin) {
- switch (pin) {
- case TOOL_ENABLE_PIN: return &outputs[0];
- case TOOL_DIR_PIN: return &outputs[1];
- case SWITCH_1_PIN: return &outputs[2];
- case SWITCH_2_PIN: return &outputs[3];
- case FAULT_PIN: return &outputs[4];
- }
-
- return 0;
-}
-
-
-static void _update_state(output_t *output) {
- switch (output->mode) {
- case OUT_DISABLED: output->state = OUT_TRI; break;
- case OUT_LO_HI: output->state = output->active ? OUT_HI : OUT_LO; break;
- case OUT_HI_LO: output->state = output->active ? OUT_LO : OUT_HI; break;
- case OUT_TRI_LO: output->state = output->active ? OUT_LO : OUT_TRI; break;
- case OUT_TRI_HI: output->state = output->active ? OUT_HI : OUT_TRI; break;
- case OUT_LO_TRI: output->state = output->active ? OUT_TRI : OUT_LO; break;
- case OUT_HI_TRI: output->state = output->active ? OUT_TRI : OUT_HI; break;
- }
-
- switch (output->state) {
- case OUT_TRI: DIRCLR_PIN(output->pin); break;
- case OUT_HI: OUTSET_PIN(output->pin); DIRSET_PIN(output->pin); break;
- case OUT_LO: OUTCLR_PIN(output->pin); DIRSET_PIN(output->pin); break;
- }
-}
-
-
-void outputs_init() {
- for (int i = 0; i < OUTS; i++) _update_state(&outputs[i]);
-}
-
-
-bool outputs_is_active(uint8_t pin) {
- output_t *output = _get_output(pin);
- return output ? output->active : false;
-}
-
-
-void outputs_set_active(uint8_t pin, bool active) {
- output_t *output = _get_output(pin);
- if (!output) return;
-
- output->active = active;
- _update_state(output);
-}
-
-
-void outputs_set_mode(uint8_t pin, output_mode_t mode) {
- output_t *output = _get_output(pin);
- if (!output) return;
- output->mode = mode;
-}
-
-
-output_state_t outputs_get_state(uint8_t pin) {
- output_t *output = _get_output(pin);
- if (output) return OUT_TRI;
- return output->state;
-}
-
-
-uint8_t get_output_state(uint8_t id) {
- return OUTS <= id ? OUT_TRI : outputs[id].state;
-}
-
-
-uint8_t get_output_mode(uint8_t id) {
- return OUTS <= id ? OUT_DISABLED : outputs[id].mode;
-}
-
-
-void set_output_mode(uint8_t id, uint8_t mode) {
- if (OUTS <= id) return;
- outputs[id].mode = mode;
- _update_state(&outputs[id]);
-}
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2017 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>
-
-
-typedef enum {
- OUT_LO,
- OUT_HI,
- OUT_TRI,
-} output_state_t;
-
-
-// OUT_inactive_active
-typedef enum {
- OUT_DISABLED,
- OUT_LO_HI,
- OUT_HI_LO,
- OUT_TRI_LO,
- OUT_TRI_HI,
- OUT_LO_TRI,
- OUT_HI_TRI,
-} output_mode_t;
-
-
-void outputs_init();
-bool outputs_is_active(uint8_t pin);
-void outputs_set_active(uint8_t pin, bool active);
-void outputs_set_mode(uint8_t pin, output_mode_t mode);
-output_state_t outputs_get_state(uint8_t pin);
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2017 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
-
-#ifdef __AVR__
-
-#include <avr/pgmspace.h>
-#define PRPSTR "S"
-
-#else // __AVR__
-
-#define PRPSTR "s"
-#define PROGMEM
-#define PGM_P char *
-#define PSTR(X) X
-#define vfprintf_P vfprintf
-#define printf_P printf
-#define puts_P puts
-#define sprintf_P sprintf
-#define strcmp_P strcmp
-#define pgm_read_ptr(x) *(x)
-#define pgm_read_word(x) *(x)
-#define pgm_read_byte(x) *(x)
-
-#endif // __AVR__
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2017 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 - 2017 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
-
-enum {PORT_A = 1, PORT_B, PORT_C, PORT_D, PORT_E, PORT_F};
-
-#define PORT(PIN) pin_ports[(PIN >> 3) - 1]
-#define BM(PIN) (1 << (PIN & 7))
-
-#ifdef __AVR__
-#include <avr/io.h>
-
-extern PORT_t *pin_ports[];
-
-#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])
-
-#define SET_PIN(PIN, X) \
- do {if (X) OUTSET_PIN(PIN); else OUTCLR_PIN(PIN);} while (0);
-
-#endif // __AVR__
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2017 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 <plan/state.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_velocity_max(arc.plane_axis_0),
- planar_travel / axis_get_velocity_max(arc.plane_axis_1),
- fabs(linear_travel) / axis_get_velocity_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, mach_in_absolute_mode());
-
- // 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
- mp_pause_queue(true); // Hold queue until arc is done
- 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, 0);
-
- if (!--arc.segments) mach_abort_arc();
- }
-}
-
-
-/// Stop arc movement without maintaining position
-/// OK to call if no arc is running
-void mach_abort_arc() {
- arc.running = false;
- mp_pause_queue(false);
-}
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2017 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();
-void mach_abort_arc();
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2017 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 {
- uint16_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_buffer_validate(mb.tail);
- 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();
-}
-
-
-#ifdef DEBUG
-void mp_queue_dump() {
- mp_buffer_t *bf = mp_queue_get_head();
- if (!bf) return;
- mp_buffer_t *bp = bf;
-
- do {
- if (bp != bf) putchar(',');
- mp_buffer_print(bp);
- bp = mp_buffer_next(bp);
- } while (bp != bf && bp->state != BUFFER_OFF);
-
- if (bp != bf) mp_buffer_print(bp);
-
- putchar('\n');
-}
-
-
-void mp_buffer_print(const mp_buffer_t *bf) {
- printf("{"
- "\"ts\":%d,"
- "\"line\":%d,"
- "\"state\":%d,"
- "\"replannable\":%s,"
- "\"hold\":%s,"
- "\"value\":%0.2f,"
- "\"target\":[%0.2f, %0.2f, %0.2f, %0.2f],"
- "\"unit\":[%0.2f, %0.2f, %0.2f, %0.2f],"
- "\"length\":%0.2f,"
- "\"head_length\":%0.2f,"
- "\"body_length\":%0.2f,"
- "\"tail_length\":%0.2f,"
- "\"entry_velocity\":%0.2f,"
- "\"cruise_velocity\":%0.2f,"
- "\"exit_velocity\":%0.2f,"
- "\"braking_velocity\":%0.2f,"
- "\"entry_vmax\":%0.2f,"
- "\"cruise_vmax\":%0.2f,"
- "\"exit_vmax\":%0.2f,"
- "\"delta_vmax\":%0.2f,"
- "\"jerk\":%0.2f,"
- "\"cbrt_jerk\":%0.2f"
- "}", bf->ts, bf->line, bf->state,
- (bf->flags & BUFFER_REPLANNABLE) ? "true" : "false",
- (bf->flags & BUFFER_HOLD) ? "true" : "false",
- bf->value, bf->target[0], bf->target[1],
- bf->target[2], bf->target[3], bf->unit[0], bf->unit[1], bf->unit[2],
- bf->unit[3], bf->length, bf->head_length, bf->body_length,
- bf->tail_length, bf->entry_velocity, bf->cruise_velocity,
- bf->exit_velocity, bf->braking_velocity, bf->entry_vmax,
- bf->cruise_vmax, bf->exit_vmax, bf->delta_vmax, bf->jerk,
- bf->cbrt_jerk);
-}
-#endif // DEBUG
-
-
-void mp_buffer_validate(const mp_buffer_t *bp) {
- ASSERT(bp);
-
- if (!(bp->flags & BUFFER_LINE)) return; // Only check line buffers
-
- ASSERT(isfinite(bp->value));
-
- ASSERT(isfinite(bp->target[0]) && isfinite(bp->target[1]) &&
- isfinite(bp->target[2]) && isfinite(bp->target[3]));
- ASSERT(isfinite(bp->unit[0]) && isfinite(bp->unit[1]) &&
- isfinite(bp->unit[2]) && isfinite(bp->unit[3]));
-
- ASSERT(isfinite(bp->length));
- ASSERT(isfinite(bp->head_length));
- ASSERT(isfinite(bp->body_length));
- ASSERT(isfinite(bp->tail_length));
-
- ASSERT(isfinite(bp->entry_velocity));
- ASSERT(isfinite(bp->cruise_velocity));
- ASSERT(isfinite(bp->exit_velocity));
- ASSERT(isfinite(bp->braking_velocity));
-
- ASSERT(isfinite(bp->jerk));
- ASSERT(isfinite(bp->cbrt_jerk));
-}
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2017 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;
-
-
-typedef enum {
- BUFFER_REPLANNABLE = 1 << 0,
- BUFFER_HOLD = 1 << 1,
- BUFFER_SEEK_CLOSE = 1 << 2,
- BUFFER_SEEK_OPEN = 1 << 3,
- BUFFER_SEEK_ERROR = 1 << 4,
- BUFFER_RAPID = 1 << 5,
- BUFFER_INVERSE_TIME = 1 << 6,
- BUFFER_EXACT_STOP = 1 << 7,
- BUFFER_LINE = 1 << 8,
-} buffer_flags_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
- buffer_flags_t flags; // buffer flags
- switch_id_t sw; // Switch to seek
-
- float value; // used in dwell and other callbacks
-
- float target[AXES]; // XYZABC where the move should go in mm
- 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();
-
-void mp_queue_dump();
-
-void mp_buffer_print(const mp_buffer_t *bp);
-void mp_buffer_validate(const mp_buffer_t *bp);
-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 - 2017 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>
-#include <inttypes.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 = 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_position(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_position(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);
-
- 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 - 2017 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 - 2017 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 - 2017 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>
-
-
-stat_t mp_dwell(float seconds, int32_t line);
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2017 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 "stepper.h"
-#include "motor.h"
-#include "rtc.h"
-#include "util.h"
-#include "velocity_curve.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;
-
- uint24_t segment_count; // count of running segments
- uint24_t segment; // current segment
- float segment_time;
- float segment_velocity; // computed velocity for segment
- float segment_start[AXES];
- float segment_delta;
- float segment_dist;
- 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}};
-
-/// Common code for head and tail sections
-static stat_t _exec_aline_section(float length, float Vi, float Vt) {
- if (ex.section_new) {
- ASSERT(isfinite(length));
-
- if (fp_ZERO(length)) return STAT_NOOP; // end the section
-
- ASSERT(isfinite(Vi) && isfinite(Vt));
- ASSERT(0 <= Vi && 0 <= Vt);
- ASSERT(Vi || Vt);
-
- // len / avg. velocity
- const float move_time = 2 * length / (Vi + Vt); // in mins
- const float segments = ceil(move_time / SEGMENT_TIME);
- ex.segment_count = segments;
- ex.segment_time = move_time / segments; // in mins
- ex.segment = 0;
- ex.segment_dist = 0;
-
- for (int axis = 0; axis < AXES; axis++)
- ex.segment_start[axis] = mp_runtime_get_axis_position(axis);
-
- if (Vi == Vt) {
- ex.segment_delta = length / segments;
- ex.segment_velocity = Vi;
-
- } else ex.segment_delta = 1 / (segments + 1);
-
- ex.section_new = false;
- }
-
- float target[AXES];
- ex.segment++;
-
- // 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 section waypoint compute target from segment time and velocity. Don't
- // do waypoint correction when going into a hold.
- if (ex.segment == ex.segment_count && !ex.section_new && !ex.hold_planned)
- copy_vector(target, ex.waypoint[ex.section]);
-
- else {
- if (Vi == Vt) ex.segment_dist += ex.segment_delta;
- else {
- // Compute quintic Bezier curve
- ex.segment_velocity =
- velocity_curve(Vi, Vt, ex.segment * ex.segment_delta);
- ex.segment_dist += ex.segment_velocity * ex.segment_time;
- }
-
- // Avoid overshoot
- if (length < ex.segment_dist) ex.segment_dist = length;
-
- for (int axis = 0; axis < AXES; axis++)
- target[axis] = ex.segment_start[axis] + ex.unit[axis] * ex.segment_dist;
- }
-
- mp_runtime_set_velocity(ex.segment_velocity);
- RITORNO(mp_runtime_move_to_target(ex.segment_time, target));
-
- // Return EAGAIN to continue or OK if this segment is done
- return ex.segment < ex.segment_count ? STAT_EAGAIN : STAT_OK;
-}
-
-
-/// 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;
- }
-}
-
-
-/// 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());
- // Velocity left to shed to brake to zero
- float braking_velocity = ex.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.
- 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 if (HOLD_VELOCITY_TOLERANCE < braking_velocity) {
- // 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);
-
- } else { // Were're close enough
- ex.tail_length = available_length;
- ex.exit_velocity = 0;
- }
-
- // Don't error if seek was stopped
- bf->flags &= ~BUFFER_SEEK_ERROR;
-}
-
-
-/// Initializes move runtime with a new planner buffer
-static stat_t _exec_aline_init(mp_buffer_t *bf) {
-#ifdef DEBUG
- printf("buffer:");
- mp_buffer_print(bf);
- putchar('\n');
-#endif
-
- // 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;
-
- // Enforce min cruise velocity
- // TODO How does cruise_velocity ever end up zero when length is non-zero?
- if (ex.cruise_velocity < 10) ex.cruise_velocity = 10;
-
- 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;
- }
-
- // If seeking, end move when switch is in target state.
- if ((((bf->flags & BUFFER_SEEK_CLOSE) && switch_is_active(bf->sw)) ||
- ((bf->flags & BUFFER_SEEK_OPEN) && !switch_is_active(bf->sw))) &&
- !ex.hold_planned) {
- if (!fp_ZERO(mp_runtime_get_velocity())) _plan_hold();
- else {
- mp_runtime_set_velocity(0);
- bf->flags &= ~BUFFER_SEEK_ERROR;
- return STAT_NOOP;
- }
- }
-
- // 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;
- }
-
- // Exiting
- if (status != STAT_EAGAIN) {
- // Set runtime velocity on exit
- mp_runtime_set_velocity(ex.exit_velocity);
-
- // If seeking, switch was not found. Signal error if necessary.
- if ((bf->flags & (BUFFER_SEEK_CLOSE | BUFFER_SEEK_OPEN)) &&
- (bf->flags & BUFFER_SEEK_ERROR))
- return STAT_SEEK_SWTICH_NOT_FOUND;
- }
-
- 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);
- if (mp_get_state() == STATE_STOPPING) mp_state_holding();
-
- 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->flags &= ~BUFFER_REPLANNABLE;
-
- // 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->flags & BUFFER_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)->flags &= ~BUFFER_REPLANNABLE;
-
- 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 - 2017 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 - 2017 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 "velocity_curve.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 {
- float delta;
- float t;
- bool changed;
-
- int sign;
- float velocity;
- float accel;
- float next;
- float initial;
- float target;
-} jog_axis_t;
-
-
-typedef struct {
- bool writing;
- bool done;
-
- float Vi;
- float Vt;
-
- jog_axis_t axes[AXES];
-} jog_runtime_t;
-
-
-static jog_runtime_t jr;
-
-
-static bool _next_axis_velocity(int axis) {
- jog_axis_t *a = &jr.axes[axis];
-
- float Vn = a->next * axis_get_velocity_max(axis);
- float Vi = a->velocity;
- float Vt = a->target;
-
- if (JOG_MIN_VELOCITY < fabs(Vn)) jr.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) return false; // No change
-
- a->target = Vn;
- if (Vn) a->sign = Vn < 0 ? -1 : 1;
-
- return true;
-}
-
-
-static float _compute_axis_velocity(int axis) {
- jog_axis_t *a = &jr.axes[axis];
-
- float V = fabs(a->velocity);
- float Vt = fabs(a->target);
-
- if (JOG_MIN_VELOCITY < Vt) jr.done = false;
-
- if (fp_EQ(V, Vt)) return Vt;
-
- // Compute axis max jerk
- float jerk = axis_get_jerk_max(axis) * JERK_MULTIPLIER;
-
- // Compute target accel
- float accel = sqrt(jerk * fabs(Vt - V)) * (Vt < V ? -1 : 1);
-
- // TODO apply max accel here
-
- // Compute max delta accel
- float deltaAccel = jerk * SEGMENT_TIME;
-
- // Update accel
- if (a->accel < accel) {
- if (accel < a->accel + deltaAccel) a->accel = accel;
- else a->accel += deltaAccel;
-
- } else if (accel < a->accel) {
- if (a->accel - deltaAccel < accel) a->accel = accel;
- else a->accel -= deltaAccel;
- }
-
- return V + a->accel * SEGMENT_TIME;
-}
-
-
-#if 0
-static float _axis_velocity_at_limits(int axis) {
- float V = jr.axes[axis].velocity;
-
- if (axis_get_homed(axis)) {
- float min = axis_get_travel_min(axis);
- float max = axis_get_travel_max(axis);
-
- if (position <= min) return 0;
- if (max <= position) return 0;
-
- float position = mp_runtime_get_axis_position(axis);
- float jerk = axis_get_jerk_max(axis) * JERK_MULTIPLIER;
- float decelDist = mp_get_target_length(V, 0, jerk);
-
- if (position < min + decelDist) {
- }
-
- if (max - decelDist < position) {
- }
- }
-
- return V;
-}
-#endif
-
-
-static stat_t _exec_jog(mp_buffer_t *bf) {
- // Load next velocity
- jr.done = true;
-
- if (!jr.writing)
- for (int axis = 0; axis < AXES; axis++) {
- if (!axis_is_enabled(axis)) continue;
- jr.axes[axis].changed = _next_axis_velocity(axis);
- }
-
- float velocity_sqr = 0;
-
- // Compute per axis velocities
- for (int axis = 0; axis < AXES; axis++) {
- if (!axis_is_enabled(axis)) continue;
- float V = _compute_axis_velocity(axis);
- velocity_sqr += square(V);
- jr.axes[axis].velocity = V * jr.axes[axis].sign;
- if (JOG_MIN_VELOCITY < V) jr.done = false;
- }
-
- // Check if we are done
- if (jr.done) {
- // Update machine position
- mach_set_position_from_runtime();
- mp_set_cycle(CYCLE_MACHINING); // Default cycle
- mp_pause_queue(false);
-
- 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.axes[axis].velocity * SEGMENT_TIME;
-
- // Set velocity and target
- mp_runtime_set_velocity(sqrt(velocity_sqr));
- stat_t status = mp_runtime_move_to_target(SEGMENT_TIME, target);
- if (status != STAT_OK) return status;
-
- return STAT_EAGAIN;
-}
-
-
-uint8_t command_jog(int argc, char *argv[]) {
- if (mp_get_cycle() != CYCLE_JOGGING &&
- (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_get_cycle() != CYCLE_JOGGING) memset(&jr, 0, sizeof(jr));
-
- jr.writing = true;
- for (int axis = 0; axis < AXES; axis++)
- jr.axes[axis].next = velocity[axis];
- jr.writing = false;
-
- if (mp_get_cycle() != CYCLE_JOGGING) {
- mp_set_cycle(CYCLE_JOGGING);
- mp_pause_queue(true);
- mp_queue_push_nonstop(_exec_jog, -1);
- }
-
- return STAT_OK;
-}
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2017 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
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2017 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"
-
-#include <stdio.h>
-#include <float.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 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(dot(a, b) / (norm(a) * norm(b)));
- * 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 FLT_MAX; // 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] * JUNCTION_DEVIATION);
- b_delta += square(b_unit[axis] * JUNCTION_DEVIATION);
- }
-
- if (!a_delta || !b_delta) return 0; // One or both unit vectors are null
-
- float delta = (sqrt(a_delta) + sqrt(b_delta)) / 2;
- float sin_half_theta = sqrt((1 - costheta) / 2);
- float radius = delta * sin_half_theta / (1 - sin_half_theta);
- 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.
- */
-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_jerk_max(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);
-
- ASSERT(isfinite(unit[axis]) && unit[axis]);
-
- // 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) {
- ASSERT(0 < move_time && isfinite(move_time));
-
- bf->cruise_vmax = bf->length / move_time; // target velocity requested
-
- float junction_velocity = FLT_MAX;
-
- mp_buffer_t *prev = mp_buffer_prev(bf);
- while (prev->state != BUFFER_OFF)
- if (prev->flags & BUFFER_LINE) {
- junction_velocity = _get_junction_vmax(prev->unit, bf->unit);
- break;
-
- } else prev = mp_buffer_prev(prev);
-
- 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;
-}
-
-
-/* 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) {
- ASSERT(0 < 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]) / axis_get_velocity_max(axis);
- if (max_time < time) max_time = time;
- }
-
- return max_time < SEGMENT_TIME ? 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[], buffer_flags_t flags, switch_id_t sw,
- float feed_rate, float feed_override, int32_t line) {
- DEBUG_CALL("(%f, %f, %f, %f), %s%s%s, %f, %f, %d",
- target[0], target[1], target[2], target[3],
- (flags & BUFFER_RAPID) ? "rapid|" : "",
- (flags & BUFFER_INVERSE_TIME) ? "inverse_time|" : "",
- (flags & BUFFER_EXACT_STOP) ? "exact_stop|" : "",
- feed_rate, feed_override, line);
-
- // Trap zero feed rate condition
- if (!(flags & BUFFER_RAPID) && fp_ZERO(feed_rate))
- return STAT_GCODE_FEEDRATE_NOT_SPECIFIED;
-
- // 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, flags & BUFFER_RAPID,
- flags & BUFFER_INVERSE_TIME, feed_rate,
- feed_override);
- _calc_max_velocities(bf, time, flags & BUFFER_EXACT_STOP);
-
- flags |= BUFFER_LINE;
- if (!(flags & BUFFER_EXACT_STOP)) flags |= BUFFER_REPLANNABLE;
-
- // Note, the following lines must remain in order.
- bf->line = line; // Planner needs this when planning steps
- bf->flags = flags; // Move flags
- bf->sw = sw; // Seek switch, if any
- mp_plan(bf); // Plan block list
- mp_set_position(target); // Set planner position before pushing 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 - 2017 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 "buffer.h"
-
-#include <stdbool.h>
-#include <stdint.h>
-
-
-stat_t mp_aline(const float target[], buffer_flags_t flags, switch_id_t sw,
- 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 - 2017 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 \
- (SEGMENT_TIME * (bf->cruise_velocity + bf->entry_velocity))
-#define MIN_TAIL_LENGTH \
- (SEGMENT_TIME * (bf->cruise_velocity + bf->exit_velocity))
-#define MIN_BODY_LENGTH (SEGMENT_TIME * 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. 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 lowered.
- * 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) {
- 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 (isfinite(naive_move_time) && naive_move_time < SEGMENT_TIME) {
- bf->cruise_velocity = bf->length / SEGMENT_TIME;
- 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 (isfinite(naive_move_time) && naive_move_time <= SEGMENT_TIME) {
- bf->entry_velocity = mp_buffer_prev(bf)->exit_velocity;
-
- if (!fp_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 ((fabs(bf->cruise_velocity - bf->entry_velocity) <
- TRAPEZOID_VELOCITY_TOLERANCE) &&
- (fabs(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->tail_length < bf->head_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 if (bf->head_length + bf->tail_length) {
- 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);
-
- } else break;
-
- } while (TRAPEZOID_ITERATION_ERROR_PERCENT <
- (fabs(bf->cruise_velocity - computed_velocity) /
- computed_velocity));
-
- // 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_ZERO(bf->body_length)) {
- if (!fp_ZERO(bf->head_length)) {
- if (!fp_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
-void mp_print_buffer(mp_buffer_t *bp) {
- printf_P(PSTR("{\"msg\":\""));
- printf_P(PSTR("%d,"), bp->flags & BUFFER_REPLANNABLE);
- printf_P(PSTR("0x%04lx,"), (intptr_t)bp->cb);
- printf_P(PSTR("%0.2f,"), bp->length);
- printf_P(PSTR("%0.2f,"), bp->head_length);
- printf_P(PSTR("%0.2f,"), bp->body_length);
- printf_P(PSTR("%0.2f,"), bp->tail_length);
- printf_P(PSTR("%0.2f,"), bp->entry_velocity);
- printf_P(PSTR("%0.2f,"), bp->cruise_velocity);
- printf_P(PSTR("%0.2f,"), bp->exit_velocity);
- printf_P(PSTR("%0.2f,"), bp->braking_velocity);
- printf_P(PSTR("%0.2f,"), bp->entry_vmax);
- printf_P(PSTR("%0.2f,"), bp->cruise_vmax);
- printf_P(PSTR("%0.2f"), bp->exit_vmax);
- printf_P(PSTR("\"}\n"));
-
- while (!usart_tx_empty()) continue;
-}
-
-
-/// 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\":\"replannable,callback,"
- "length,head_length,body_length,tail_length,"
- "entry_velocity,cruise_velocity,exit_velocity,braking_velocity,"
- "entry_vmax,cruise_vmax,exit_vmax\"}\n"));
-
- mp_buffer_t *bp = bf;
- while (bp) {
- mp_print_buffer(bp);
- bp = mp_buffer_prev(bp);
- if (bp == bf || bp->state == BUFFER_OFF) break;
- }
-}
-#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->flags - replanable, hold, probe, etc [Note 1]
- * 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->flags - replanable
- * 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->flags
- * BUFFER_REPLANNABLE bit. 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 *bf) {
- mp_buffer_t *bp = bf;
-
- // 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)) != bf) {
- if (!(bp->flags & BUFFER_REPLANNABLE)) break;
-
- // TODO what about non-move buffers?
- 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 bf.
- mp_buffer_t *prev = bp;
- while ((bp = mp_buffer_next(bp)) != bf) {
- mp_buffer_t *next = mp_buffer_next(bp);
-
- bp->entry_velocity = prev == bf ? 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->flags |= BUFFER_HOLD;
-
- } else bp->flags &= ~BUFFER_HOLD;
-
- 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->flags & BUFFER_REPLANNABLE) &&
- fp_EQ(bp->exit_velocity, (bp->entry_velocity + bp->delta_vmax))))
- bp->flags &= ~BUFFER_REPLANNABLE;
-
- prev = bp;
- }
-
- // Finish last block
- bf->entry_velocity = prev->exit_velocity;
- bf->cruise_velocity = bf->cruise_vmax;
- bf->exit_velocity = 0;
-
- mp_calculate_trapezoid(bf);
-}
-
-
-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->flags |= BUFFER_REPLANNABLE;
- 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;
- bp->flags |= BUFFER_REPLANNABLE;
-
- 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) {
- ASSERT(0 <= Vi);
- ASSERT(0 <= Vf);
- ASSERT(isfinite(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 equation c) solved for Vf. We use f) since
- * it is more simple.
- *
- * 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) {
- ASSERT(0 <= Vi);
- ASSERT(0 <= L);
- ASSERT(isfinite(bf->jerk));
- ASSERT(isfinite(bf->cbrt_jerk));
-
- if (!L) return Vi;
-
- // 0 iterations (a reasonable estimate)
- float x = pow(L, 0.66666666) * bf->cbrt_jerk + Vi; // First estimate
-
-#if GET_VELOCITY_ITERATIONS
- 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 - 2017 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
-
-/// 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
-#define HOLD_VELOCITY_TOLERANCE 60 // In mm/min
-
-
-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_print_buffer(mp_buffer_t *bp);
-
-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 - 2017 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
-
-#if 0 // TODO These are not currently being set
- 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;
-#endif
-} 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_position(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, 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(const 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 time, const float target[]) {
- ASSERT(isfinite(target[0]) && isfinite(target[1]) &&
- isfinite(target[2]) && isfinite(target[3]));
-
- // Convert target position to steps.
- float steps[MOTORS];
- mp_kinematics(target, steps);
-
- // Call the stepper prep function
- RITORNO(st_prep_line(time, steps));
-
- // Update positions
- mp_runtime_set_position(target);
-
- return STAT_OK;
-}
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2017 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, float position);
-float mp_runtime_get_axis_position(uint8_t axis);
-
-float *mp_runtime_get_position();
-void mp_runtime_set_position(const 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 time, const float target[]);
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2017 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 "spindle.h"
-
-#include "report.h"
-
-#include <stdbool.h>
-
-
-typedef struct {
- mp_state_t state;
- mp_cycle_t cycle;
- mp_hold_reason_t hold_reason;
- bool pause;
-
- 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();
-}
-
-
-bool mp_is_ready() {
- return mp_queue_get_room() && !mp_is_resuming() && !ps.pause;
-}
-
-
-void mp_pause_queue(bool x) {ps.pause = x;}
-
-
-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);
- mp_pause_queue(false);
-}
-
-
-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.
- mach_set_position_from_runtime();
- }
-
- // Stop spindle
- spindle_stop();
-
- // 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 - 2017 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 "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();
-bool mp_is_ready();
-void mp_pause_queue(bool x);
-
-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 - 2017 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 "velocity_curve.h"
-
-#include <math.h>
-
-
-/// We are using a quintic (fifth-degree) Bezier polynomial for the velocity
-/// curve. This yields a constant pop; with pop being the sixth derivative
-/// of position:
-///
-/// 1st - velocity
-/// 2nd - acceleration
-/// 3rd - jerk
-/// 4th - snap
-/// 5th - crackle
-/// 6th - pop
-///
-/// The Bezier curve takes the form:
-///
-/// f(t) = P_0(1 - t)^5 + 5P_1(1 - t)^4 t + 10P_2(1 - t)^3 t^2 +
-/// 10P_3(1 - t)^2 t^3 + 5P_4(1 - t)t^4 + P_5t^5
-///
-/// Where 0 <= t <= 1, f(t) is the velocity and P_0 through P_5 are the control
-/// points. In our case:
-///
-/// P_0 = P_1 = P2 = Vi
-/// P_3 = P_4 = P5 = Vt
-///
-/// Where Vi is the initial velocity and Vt is the target velocity.
-///
-/// After substitution, expanding the polynomial and collecting terms we have:
-///
-/// f(t) = (Vt - Vi)(6t^5 - 15t^4 + 10t^3) + Vi
-///
-/// Computing this directly using 32bit float-point on a 32MHz AtXMega processor
-/// takes about 60uS or about 1,920 clocks. The code was compiled with avr-gcc
-/// v4.9.2 with -O3.
-float velocity_curve(float Vi, float Vt, float t) {
- // If the change is small enough just do a linear velocity transition.
- // TODO revisit this.
- if (fabs(Vt - Vi) < 200) return Vi + (Vt - Vi) * t;
-
- const float t2 = t * t;
- const float t3 = t2 * t;
-
- return (Vt - Vi) * (6 * t2 - 15 * t + 10) * t3 + Vi;
-}
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2017 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
-
-float velocity_curve(float Vi, float Vt, float t);
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2017 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"
-#include "estop.h"
-#include "outputs.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 pwm_invert;
-} pwm_spindle_t;
-
-
-static pwm_spindle_t spindle = {0};
-
-
-static void _set_enable(bool enable) {
- outputs_set_active(TOOL_ENABLE_PIN, enable);
-}
-
-
-static void _set_dir(bool clockwise) {
- outputs_set_active(TOOL_DIR_PIN, !clockwise);
-}
-
-
-static void _set_pwm(spindle_mode_t mode, float speed) {
- if (mode == SPINDLE_OFF || speed < spindle.min_rpm || estop_triggered()) {
- TIMER_PWM.CTRLA = 0;
- OUTCLR_PIN(SPIN_PWM_PIN);
- _set_enable(false);
- return;
- }
-
- // Invert PWM
- if (spindle.pwm_invert) PINCTRL_PIN(SPIN_PWM_PIN) |= PORT_INVEN_bm;
- else PINCTRL_PIN(SPIN_PWM_PIN) &= ~PORT_INVEN_bm;
-
- // 100% duty
- if (spindle.max_rpm <= speed && spindle.max_duty == 1) {
- TIMER_PWM.CTRLB = 0;
- OUTSET_PIN(SPIN_PWM_PIN);
- return;
- }
-
- // Clamp speed
- if (spindle.max_rpm < speed) speed = spindle.max_rpm;
- if (speed < spindle.min_rpm) speed = 0;
-
- // Set clock period and optimal prescaler value
- float prescale = (float)(F_CPU >> 16) / 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;
-
- // Configure clock
- TIMER_PWM.CTRLB = TC1_CCAEN_bm | TC_WGMODE_SINGLESLOPE_gc;
- TIMER_PWM.CCA = TIMER_PWM.PER * duty;
-}
-
-
-void pwm_spindle_init() {
- // Configure IO
- _set_dir(true);
- _set_enable(false);
-
- DIRSET_PIN(SPIN_PWM_PIN); // Output
-}
-
-
-void pwm_spindle_set(spindle_mode_t mode, float speed) {
- if (mode != SPINDLE_OFF) _set_dir(mode == SPINDLE_CW);
- _set_pwm(mode, speed);
- _set_enable(mode != SPINDLE_OFF);
-}
-
-
-void pwm_spindle_stop() {pwm_spindle_set(SPINDLE_OFF, 0);}
-
-
-// TODO these need more effort and should work with the huanyang spindle too
-float get_max_spin() {return spindle.max_rpm;}
-void set_max_spin(float value) {spindle.max_rpm = value;}
-float get_min_spin() {return spindle.min_rpm;}
-void set_min_spin(float value) {spindle.min_rpm = value;}
-float get_spin_min_duty() {return spindle.min_duty * 100;}
-void set_spin_min_duty(float value) {spindle.min_duty = value / 100;}
-float get_spin_max_duty() {return spindle.max_duty * 100;}
-void set_spin_max_duty(float value) {spindle.max_duty = value / 100;}
-float get_spin_up() {return 0;} // TODO
-void set_spin_up(float value) {} // TODO
-float get_spin_down() {return 0;} // TODO
-void set_spin_down(float value) {} // TODO
-uint16_t get_spin_freq() {return spindle.freq;}
-void set_spin_freq(uint16_t value) {spindle.freq = value;}
-bool get_pwm_invert() {return spindle.pwm_invert;}
-void set_pwm_invert(bool value) {spindle.pwm_invert = value;}
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2017 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_stop();
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2017 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 "pgmspace.h"
-
-#include <stdio.h>
-#include <stdbool.h>
-
-
-static bool _requested = false;
-static bool _full = false;
-static uint32_t _last = 0;
-
-
-void report_request() {_requested = true;}
-void report_request_full() {_requested = _full = true;}
-
-
-void report_callback() {
- if (_requested && usart_tx_empty()) {
- uint32_t now = rtc_get_time();
- if (now - _last < 250) return;
- _last = now;
-
- vars_report(_full);
- _requested = _full = false;
- }
-}
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2017 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 - 2017 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 - 2017 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 - 2017 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 - 2017 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_HUANYANG,
- SPINDLE_TYPE_PWM,
-} spindle_type_t;
-
-
-typedef struct {
- spindle_type_t type;
- spindle_mode_t mode;
- float speed;
- bool reversed;
-} spindle_t;
-
-
-static spindle_t spindle = {0};
-
-
-void spindle_init() {
- pwm_spindle_init();
- huanyang_init();
-}
-
-
-void _spindle_set(spindle_mode_t mode, float speed) {
- if (speed < 0) speed = 0;
- if (mode != SPINDLE_CW && mode != SPINDLE_CCW) mode = SPINDLE_OFF;
-
- spindle.mode = mode;
- spindle.speed = speed;
-
- if (spindle.reversed) {
- if (mode == SPINDLE_CW) mode = SPINDLE_CCW;
- else if (mode == SPINDLE_CCW) mode = SPINDLE_CW;
- }
-
- 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_set(mode, spindle.speed);}
-void spindle_set_speed(float speed) {_spindle_set(spindle.mode, speed);}
-spindle_mode_t spindle_get_mode() {return spindle.mode;}
-float spindle_get_speed() {return spindle.speed;}
-
-
-void spindle_stop() {
- switch (spindle.type) {
- case SPINDLE_TYPE_PWM: pwm_spindle_stop(); break;
- case SPINDLE_TYPE_HUANYANG: huanyang_stop(); 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);
- }
-}
-
-
-bool spindle_is_reversed() {return spindle.reversed;}
-bool get_spin_reversed() {return spindle.reversed;}
-
-
-void set_spin_reversed(bool reversed) {
- spindle.reversed = reversed;
- _spindle_set(spindle.mode, spindle.speed);
-}
-
-
-PGM_P get_spin_mode() {
- switch (spindle.mode) {
- case SPINDLE_CW: return PSTR("Clockwise");
- case SPINDLE_CCW: return PSTR("Counterclockwise");
- default: break;
- }
- return PSTR("Off");
-}
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2017 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_stop();
-bool spindle_is_reversed();
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2017 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\":\"%"PRPSTR"\", \"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\": \"%"PRPSTR"\""), 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\": \"%"PRPSTR"\""), 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, const char *msg) {
- status_message_P(location, STAT_LEVEL_ERROR, code, msg);
- estop_trigger(code);
- while (!usart_tx_empty()) continue;
- return code;
-}
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2017 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 "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, const char *msg);
-
-#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_DEBUG, 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, 0)
-#define ASSERT(COND) \
- do { \
- if (!(COND)) \
- status_alarm(STATUS_LOCATION, STAT_INTERNAL_ERROR, PSTR(#COND)); \
- } while (0)
-
-
-#ifdef DEBUG
-#define DEBUG_CALL(FMT, ...) \
- printf("%s(" FMT ")\n", __FUNCTION__, ##__VA_ARGS__)
-#else // DEBUG
-#define DEBUG_CALL(...)
-#endif // DEBUG
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2017 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 "report.h"
-
-#include <string.h>
-#include <stdio.h>
-
-
-typedef enum {
- MOVE_TYPE_NULL, // null move - does a no-op
- MOVE_TYPE_LINE, // acceleration planned line
- MOVE_TYPE_DWELL, // delay with no movement
-} move_type_t;
-
-
-typedef struct {
- // Runtime
- bool busy;
- bool requesting;
- float dwell;
-
- // Move prep
- bool move_ready; // prepped move ready for loader
- bool move_queued; // prepped move queued
- move_type_t move_type;
- float prep_dwell;
- uint16_t clock_period;
-} stepper_t;
-
-
-static stepper_t st = {0};
-
-
-void stepper_init() {
- // Motor enable
- OUTSET_PIN(MOTOR_ENABLE_PIN); // Low (disabled)
- DIRSET_PIN(MOTOR_ENABLE_PIN); // Output
-
- // 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 rate
- TIMER_STEP.CTRLA = STEP_TIMER_ENABLE; // start step timer
-
- st.clock_period = STEP_TIMER_POLL;
-}
-
-
-void st_shutdown() {
- OUTCLR_PIN(MOTOR_ENABLE_PIN);
- st.dwell = 0;
- st.move_type = MOVE_TYPE_NULL;
-}
-
-
-void st_enable() {
- if (!estop_triggered()) OUTSET_PIN(MOTOR_ENABLE_PIN); // Active high
- report_request();
-}
-
-
-/// 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(STEP_LOW_LEVEL_ISR) {
- 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 "software" interrupt to trigger next move exec as low interrupt.
- ADCB_CH0_INTCTRL = ADC_CH_INTLVL_LO_gc;
- ADCB_CTRLA = ADC_ENABLE_bm | ADC_CH0START_bm;
-}
-
-
-static void _end_move() {
- for (int motor = 0; motor < MOTORS; motor++)
- motor_end_move(motor);
-}
-
-
-/// Dwell or dequeue and load next move.
-static void _load_move() {
- // Check EStop
- if (estop_triggered()) {
- st.move_type = MOVE_TYPE_NULL;
- _end_move();
- return;
- }
-
- // New clock period
- TIMER_STEP.PER = st.clock_period;
-
- // Dwell
- if (0 < st.dwell) {
- st.dwell -= 0.001; // 1ms
- return;
- } else st.dwell = 0;
-
- // If the next move is not ready try to load it
- if (!st.move_ready) {
- _request_exec_move();
- _end_move();
- return;
- }
-
- // Start move
- if (st.move_type == MOVE_TYPE_LINE)
- for (int motor = 0; motor < MOTORS; motor++)
- motor_load_move(motor);
-
- else _end_move();
-
- if (st.move_type != MOVE_TYPE_NULL) {
- st.busy = true;
-
- // Start dwell
- st.dwell = st.prep_dwell;
- }
-
- // We are done with this move
- st.move_type = MOVE_TYPE_NULL;
- st.prep_dwell = 0; // clear dwell
- st.move_ready = false; // flip the flag back
- st.clock_period = STEP_TIMER_POLL;
-
- // 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();
-}
-
-
-/// Step timer interrupt routine.
-ISR(STEP_TIMER_ISR) {
- _load_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.
- */
-stat_t st_prep_line(float time, const float target[]) {
- // Trap conditions that would prevent queueing the line
- ASSERT(!st.move_ready);
-
- // Setup segment parameters
- st.move_type = MOVE_TYPE_LINE;
- st.clock_period = round(time * STEP_TIMER_FREQ * 60);
-
- // Prepare motor moves
- for (int motor = 0; motor < MOTORS; motor++) {
- ASSERT(isfinite(target[motor]));
- motor_prep_move(motor, time, round(target[motor]));
- }
-
- 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) {
- ASSERT(!st.move_ready);
- st.move_type = MOVE_TYPE_DWELL;
- st.prep_dwell = seconds;
- st.move_queued = true; // signal prep buffer ready
-}
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2017 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();
-void st_enable();
-bool st_is_busy();
-stat_t st_prep_line(float time, const float target[]);
-void st_prep_dwell(float seconds);
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2017 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 "switch.h"
-#include "config.h"
-
-#include <avr/interrupt.h>
-
-#include <stdbool.h>
-
-
-typedef struct {
- uint8_t pin;
- switch_type_t type;
-
- switch_callback_t cb;
- bool state;
- bool triggered;
-} switch_t;
-
-
-// Order must match indices in var functions below
-static switch_t switches[SWITCHES] = {
- {.pin = MIN_X_PIN, .type = SW_DISABLED},
- {.pin = MAX_X_PIN, .type = SW_DISABLED},
- {.pin = MIN_Y_PIN, .type = SW_DISABLED},
- {.pin = MAX_Y_PIN, .type = SW_DISABLED},
- {.pin = MIN_Z_PIN, .type = SW_DISABLED},
- {.pin = MAX_Z_PIN, .type = SW_DISABLED},
- {.pin = MIN_A_PIN, .type = SW_DISABLED},
- {.pin = MAX_A_PIN, .type = SW_DISABLED},
- {.pin = ESTOP_PIN, .type = SW_DISABLED},
- {.pin = PROBE_PIN, .type = SW_DISABLED},
-};
-
-
-static bool _read_state(const switch_t *s) {return IN_PIN(s->pin);}
-
-
-static void _switch_isr() {
- for (int i = 0; i < SWITCHES; i++) {
- switch_t *s = &switches[i];
- if (s->type == SW_DISABLED || s->triggered) continue;
- s->triggered = _read_state(s) != s->state;
- if (s->triggered) 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) {
- s->triggered = false;
- s->state = _read_state(s); // Initialize state
- PORT(s->pin)->INT0MASK |= BM(s->pin); // Enable INT0
-
- } 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->triggered) continue;
-
- bool state = _read_state(s);
- s->triggered = false;
- PORT(s->pin)->INT0MASK |= BM(s->pin); // Reenable INT0
-
- if (state != s->state) {
- s->state = state;
- if (s->cb) s->cb(i, switch_is_active(i));
- }
- }
-}
-
-
-bool switch_is_active(int index) {
- switch (switches[index].type) {
- case SW_DISABLED: break; // A disabled switch cannot be active
- case SW_NORMALLY_OPEN: return !switches[index].state;
- case SW_NORMALLY_CLOSED: return switches[index].state;
- }
- return false;
-}
-
-
-bool switch_is_enabled(int index) {
- return switch_get_type(index) != SW_DISABLED;
-}
-
-
-switch_type_t switch_get_type(int index) {
- return (index < 0 || SWITCHES <= index) ? SW_DISABLED : switches[index].type;
-}
-
-
-void switch_set_type(int index, switch_type_t type) {
- if (index < 0 || SWITCHES <= index) return;
- 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_min_sw_mode(int index) {return switch_get_type(MIN_SWITCH(index));}
-
-
-void set_min_sw_mode(int index, uint8_t value) {
- switch_set_type(MIN_SWITCH(index), value);
-}
-
-
-uint8_t get_max_sw_mode(int index) {return switch_get_type(MAX_SWITCH(index));}
-
-
-void set_max_sw_mode(int index, uint8_t value) {
- switch_set_type(MAX_SWITCH(index), value);
-}
-
-
-uint8_t get_estop_mode() {return switch_get_type(SW_ESTOP);}
-void set_estop_mode(uint8_t value) {switch_set_type(SW_ESTOP, value);}
-uint8_t get_probe_mode() {return switch_get_type(SW_PROBE);}
-void set_probe_mode(uint8_t value) {switch_set_type(SW_PROBE, value);}
-
-
-static uint8_t _get_state(int index) {
- if (!switch_is_enabled(index)) return 2; // Disabled
- return switches[index].state;
-}
-
-
-uint8_t get_min_switch(int index) {return _get_state(MIN_SWITCH(index));}
-uint8_t get_max_switch(int index) {return _get_state(MAX_SWITCH(index));}
-uint8_t get_estop_switch() {return _get_state(SW_ESTOP);}
-uint8_t get_probe_switch() {return _get_state(SW_PROBE);}
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2017 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 - 2017 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 - 2017 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 - 2017 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"
-
-#include <stdint.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 x) {
- // evil floating point bit level hacking
- union {
- float f;
- int32_t i;
- } u;
-
- const float xhalf = x * 0.5f;
- u.f = x;
- u.i = 0x5f3759df - (u.i >> 1); // what the fuck?
- u.f = u.f * (1.5f - xhalf * u.f * u.f); // 1st iteration
- u.f = u.f * (1.5f - xhalf * u.f * u.f); // 2nd iteration, can be removed
-
- return u.f;
-}
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2017 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 static float min(float a, float b) {return a < b ? a : b;}
-inline static float max(float a, float b) {return a < b ? b : a;}
-inline static float min3(float a, float b, float c) {return min(min(a, b), c);}
-inline static float max3(float a, float b, float c) {return max(max(a, b), c);}
-inline static float min4(float a, float b, float c, float d)
-{return min(min(a, b), min(c, d));}
-inline static float max4(float a, float b, float c, float d)
-{return max(max(a, b), max(c, d));}
-
-float invsqrt(float number);
-
-#ifndef __AVR__
-inline static float square(float x) {return x * x;}
-#endif
-
-// Floating-point utilities
-#define EPSILON 0.00001 // allowable rounding error for floats
-inline static bool fp_EQ(float a, float b) {return fabs(a - b) < EPSILON;}
-inline static bool fp_NE(float a, float b) {return fabs(a - b) > EPSILON;}
-inline static bool fp_ZERO(float a) {return fabs(a) < EPSILON;}
-inline static bool fp_FALSE(float a) {return fp_ZERO(a);}
-inline static 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
-
-// 24bit integers
-#ifdef __AVR__
-typedef __int24 int24_t;
-typedef __uint24 uint24_t;
-#else
-typedef int32_t int24_t;
-typedef uint32_t uint24_t;
-#endif
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2017 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 "machine.h"
-#include "spindle.h"
-#include "coolant.h"
-#include "plan/runtime.h"
-#include "plan/state.h"
-
-// Axis
-float get_axis_mach_coord(int axis) {return mp_runtime_get_axis_position(axis);}
-
-
-void set_axis_mach_coord(int axis, float position) {
- mach_set_axis_position(axis, position);
-}
-
-
-float get_axis_work_coord(int axis) {return mp_runtime_get_work_position(axis);}
-
-
-// GCode getters
-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();}
-
-
-// GCode setters
-void set_unit(const char *units) {mach_set_units(gs_parse_units(units));}
-void set_speed(float speed) {spindle_set_speed(speed);}
-void set_feed(float feed) {mach_set_feed_rate(feed);}
-
-
-void set_tool(uint8_t tool) {
- mp_runtime_set_tool(tool);
- mach_select_tool(tool);
-}
-
-
-void set_feed_mode(const char *mode) {
- mach_set_feed_mode(gs_parse_feed_mode(mode));
-}
-
-
-void set_plane(const char *plane) {mach_set_plane(gs_parse_plane(plane));}
-
-
-void set_coord_system(const char *cs) {
- mach_set_coord_system(gs_parse_coord_system(cs));
-}
-
-
-void set_abs_override(bool enable) {mach_set_absolute_mode(enable);}
-
-
-void set_path_mode(const char *mode) {
- mach_set_path_mode(gs_parse_path_mode(mode));
-}
-
-
-void set_distance_mode(const char *mode) {
- mach_set_distance_mode(gs_parse_distance_mode(mode));
-}
-
-
-void set_arc_dist_mode(const char *mode) {
- mach_set_arc_distance_mode(gs_parse_distance_mode(mode));
-}
-
-
-void set_feed_override(float value) {mach_set_feed_override(value);}
-void set_speed_override(float value) {mach_set_spindle_override(value);}
-void set_mist_coolant(bool enable) {coolant_set_mist(enable);}
-void set_flood_coolant(bool enable) {coolant_set_flood(enable);}
-
-
-// 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 - 2017 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 "axis.h"
-#include "pgmspace.h"
-
-#include <stdint.h>
-#include <stdbool.h>
-#include <stdio.h>
-#include <string.h>
-#include <stdlib.h>
-#include <ctype.h>
-#include <math.h>
-#include <inttypes.h>
-
-
-typedef uint16_t flags_t;
-typedef const char *string;
-typedef const 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);
-
-
-// Eq functions
-#define EQ_FUNC(TYPE) \
- inline static bool var_eq_##TYPE(const TYPE a, const TYPE b) {return a == b;}
-MAP(EQ_FUNC, SEMI, flags_t, string, pstring, uint8_t, uint16_t, int32_t, char);
-
-
-// String
-static void var_print_string(string s) {printf_P(PSTR("\"%s\""), s);}
-static float var_string_to_float(string s) {return 0;}
-
-
-// Program string
-static void var_print_pstring(pstring s) {printf_P(PSTR("\"%"PRPSTR"\""), s);}
-static const char *var_parse_pstring(const char *value) {return value;}
-static float var_pstring_to_float(pstring s) {return 0;}
-
-
-// Flags
-static void var_print_flags_t(flags_t x) {
- extern void print_status_flags(flags_t x);
- print_status_flags(x);
-}
-
-static float var_flags_t_to_float(flags_t x) {return x;}
-
-
-// Float
-static bool var_eq_float(float a, float b) {
- return a == b || (isnan(a) && isnan(b));
-}
-
-
-static void var_print_float(float x) {
- if (isnan(x)) printf_P(PSTR("\"nan\""));
- else if (isinf(x)) printf_P(PSTR("\"%cinf\""), x < 0 ? '-' : '+');
-
- else {
- char buf[20];
-
- int len = sprintf_P(buf, PSTR("%.3f"), 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);}
-static float var_float_to_float(float x) {return x;}
-
-
-// Bool
-inline static bool var_eq_bool(float a, float b) {return a == b;}
-static void var_print_bool(bool x) {printf_P(x ? PSTR("true") : PSTR("false"));}
-
-
-bool var_parse_bool(const char *value) {
- return !strcasecmp(value, "true") || var_parse_float(value);
-}
-
-static float var_bool_to_float(bool x) {return x;}
-
-
-// Char
-#if 0
-static void var_print_char(char x) {putchar('"'); putchar(x); putchar('"');}
-static char var_parse_char(const char *value) {return value[1];}
-static float var_char_to_float(char x) {return x;}
-#endif
-
-
-// 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 float var_int8_t_to_float(int8_t x) {return x;}
-#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 float var_uint8_t_to_float(uint8_t x) {return x;}
-
-
-// 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 float var_uint16_t_to_float(uint16_t x) {return x;}
-
-
-// int32
-static void var_print_int32_t(int32_t x) {printf_P(PSTR("%"PRIi32), x);}
-static float var_int32_t_to_float(int32_t x) {return x;}
-
-
-// Ensure no code is used more than once
-enum {
-#define VAR(NAME, CODE, ...) var_code_##CODE,
-#include "vars.def"
-#undef VAR
- var_code_count
-};
-
-// 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 & help
-#define VAR(NAME, CODE, TYPE, INDEX, SET, REPORT, HELP) \
- static const char NAME##_name[] PROGMEM = #NAME; \
- static const char NAME##_help[] PROGMEM = HELP;
-
-#include "vars.def"
-#undef VAR
-
-// Last value
-#define VAR(NAME, CODE, TYPE, INDEX, ...) \
- static TYPE NAME##_state IF(INDEX)([INDEX]);
-
-#include "vars.def"
-#undef VAR
-
-// Report
-static uint8_t _report_var[(var_code_count >> 3) + 1] = {0,};
-
-
-static bool _get_report_var(int index) {
- return _report_var[index >> 3] & (1 << (index & 7));
-}
-
-
-static void _set_report_var(int index, bool enable) {
- if (enable) _report_var[index >> 3] |= 1 << (index & 7);
- else _report_var[index >> 3] &= ~(1 << (index & 7));
-}
-
-
-static int _find_code(const char *code) {
-#define VAR(NAME, CODE, TYPE, INDEX, ...) \
- if (!strcmp(code, #CODE)) return var_code_##CODE; \
-
-#include "vars.def"
-#undef VAR
- return -1;
-}
-
-
-void vars_init() {
- // Initialize var state
-#define VAR(NAME, CODE, TYPE, INDEX, ...) \
- IF(INDEX)(for (int i = 0; i < INDEX; i++)) \
- (NAME##_state)IF(INDEX)([i]) = get_##NAME(IF(INDEX)(i));
-
-#include "vars.def"
-#undef VAR
-
-// Report
-#define VAR(NAME, CODE, TYPE, INDEX, SET, REPORT, ...) \
- _set_report_var(var_code_##CODE, REPORT);
-
-#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]); \
- bool report = _get_report_var(var_code_##CODE); \
- \
- if ((report && !var_eq_##TYPE(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);
-}
-
-
-void vars_report_all(bool enable) {
-#define VAR(NAME, CODE, TYPE, INDEX, SET, REPORT, ...) \
- _set_report_var(var_code_##CODE, enable);
-
-#include "vars.def"
-#undef VAR
-}
-
-
-void vars_report_var(const char *code, bool enable) {
- int index = _find_code(code);
- if (index != -1) _set_report_var(index, enable);
-}
-
-
-static char *_resolve_name(const char *_name) {
- unsigned len = strlen(_name);
-
- if (!len || 4 < len) return 0;
-
- static char name[5];
- strncpy(name, _name, 4);
- name[4] = 0;
-
- // Handle axis to motor mapping
- if (2 < len && name[1] == '.') {
- int axis = axis_get_id(name[0]);
- if (axis < 0) return 0;
- int motor = axis_get_motor(axis);
- if (motor < 0) return 0;
-
- name[0] = MOTORS_LABEL[motor];
- for (int i = 1; _name[i]; i++)
- name[i] = _name[i + 1];
- }
-
- return name;
-}
-
-
-bool vars_print(const char *_name) {
- char *name = _resolve_name(_name);
- if (!name) return false;
-
- int i;
-#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); \
- \
- printf("{\"%s\":", _name); \
- 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) {
- char *name = _resolve_name(_name);
- if (!name) return false;
-
- int i;
-#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;
-}
-
-
-float vars_get_number(const char *_name) {
- char *name = _resolve_name(_name);
- if (!name) return 0;
-
- int i;
-#define VAR(NAME, CODE, TYPE, INDEX, SET, ...) \
- if (!strcmp(IF_ELSE(INDEX)(name + 1, name), #CODE)) { \
- IF(INDEX) \
- (i = strchr(INDEX##_LABEL, name[0]) - INDEX##_LABEL; \
- if (INDEX <= i) return 0); \
- \
- TYPE x = get_##NAME(IF(INDEX)(i)); \
- return var_##TYPE##_to_float(x); \
- } \
-
-#include "vars.def"
-#undef VAR
-
- return 0;
-}
-
-
-int vars_parser(char *vars) {
- if (!*vars || *vars != '{') return STAT_OK;
- vars++;
-
- 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 %-20"PRPSTR" %-16"PRPSTR" %"PRPSTR"\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);
-}
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2017 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 OUTS_LABEL "ed12f"
-
-// VAR(name, code, type, index, settable, report, help)
-
-// Motor
-VAR(motor_axis, an, uint8_t, MOTORS, 1, 1, "Maps motor to axis")
-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_current, dc, float, MOTORS, 1, 1, "Max motor drive current")
-VAR(idle_current, ic, float, MOTORS, 1, 1, "Motor idle current")
-VAR(active_current, ac, float, MOTORS, 0, 1, "Motor current now")
-VAR(driver_flags, df, uint16_t, MOTORS, 0, 1, "Motor driver status flags")
-VAR(status_strings, ds, flags_t, MOTORS, 0, 1, "Motor driver status strings")
-VAR(encoder, en, int32_t, MOTORS, 0, 0, "Motor encoder")
-VAR(error, ee, int32_t, MOTORS, 0, 0, "Motor position error")
-
-VAR(motor_fault, fa, bool, 0, 0, 1, "Motor fault status")
-
-VAR(velocity_max, vm, float, MOTORS, 1, 1, "Maxium velocity in mm/min")
-VAR(jerk_max, jm, float, MOTORS, 1, 1, "Maxium jerk in mm/min^3")
-VAR(radius, ra, float, MOTORS, 1, 1, "Axis radius or zero")
-
-// Switches
-VAR(travel_min, tn, float, MOTORS, 1, 1, "Minimum soft limit")
-VAR(travel_max, tm, float, MOTORS, 1, 1, "Maximum soft limit")
-VAR(min_sw_mode, ls, uint8_t, MOTORS, 1, 1, "Minimum switch mode")
-VAR(max_sw_mode, xs, uint8_t, MOTORS, 1, 1, "Maximum switch mode")
-VAR(estop_mode, et, uint8_t, 0, 1, 1, "Estop switch mode")
-VAR(probe_mode, pt, uint8_t, 0, 1, 1, "Probe switch mode")
-VAR(min_switch, lw, uint8_t, MOTORS, 0, 1, "Minimum switch state")
-VAR(max_switch, xw, uint8_t, MOTORS, 0, 1, "Maximum switch state")
-VAR(estop_switch, ew, uint8_t, 0, 0, 1, "Estop switch state")
-VAR(probe_switch, pw, uint8_t, 0, 0, 1, "Probe switch state")
-
-// Homing
-VAR(homing_mode, ho, uint8_t, MOTORS, 1, 1, "Homing type")
-VAR(homing_dir, hd, float, MOTORS, 0, 1, "Homing direction")
-VAR(home, hp, float, MOTORS, 0, 1, "Home position")
-VAR(homed, h, bool, MOTORS, 1, 1, "True if axis is homed")
-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 backoff")
-VAR(zero_backoff, zb, float, MOTORS, 1, 1, "Homing zero backoff")
-
-// Axis
-VAR(axis_mach_coord, p, float, AXES, 1, 1, "Axis machine coordinate")
-VAR(axis_work_coord, w, float, AXES, 0, 1, "Axis work coordinate")
-VAR(axis_can_home, ch, bool, AXES, 0, 1, "Is axis configured for homing")
-
-// Outputs
-VAR(output_state, os, uint8_t, OUTS, 0, 1, "Output pin state")
-VAR(output_mode, om, uint8_t, OUTS, 1, 1, "Output pin mode")
-
-// Spindle
-VAR(spindle_type, st, uint8_t, 0, 1, 1, "PWM=0 or HUANYANG=1")
-VAR(spin_reversed, 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_duty, nd, float, 0, 1, 1, "Minimum PWM duty cycle")
-VAR(spin_max_duty, md, float, 0, 1, 1, "Maximum PWM duty cycle")
-VAR(spin_up, su, float, 0, 1, 1, "Spin up velocity")
-VAR(spin_down, sd, float, 0, 1, 1, "Spin down velocity")
-VAR(spin_freq, sf, uint16_t, 0, 1, 1, "Spindle PWM frequency")
-VAR(spin_mode, ss, pstring, 0, 0, 1, "Spindle mode")
-
-// PWM spindle
-VAR(pwm_invert, pi, bool, 0, 1, 1, "Inverted spindle PWM")
-
-// Huanyang spindle
-VAR(huanyang_id, hi, uint8_t, 0, 1, 1, "Huanyang ID")
-VAR(huanyang_freq, hz, float, 0, 0, 1, "Huanyang actual freq")
-VAR(huanyang_current, hc, float, 0, 0, 1, "Huanyang actual current")
-VAR(huanyang_rpm, hr, uint16_t, 0, 0, 1, "Huanyang actual RPM")
-VAR(huanyang_temp, ht, uint16_t, 0, 0, 1, "Huanyang temperature")
-VAR(huanyang_max_freq, hx, float, 0, 0, 1, "Huanyang max freq")
-VAR(huanyang_min_freq, hm, float, 0, 0, 1, "Huanyang min freq")
-VAR(huanyang_rated_rpm, hq, uint16_t, 0, 0, 1, "Huanyang rated RPM")
-VAR(huanyang_status, hs, uint8_t, 0, 0, 1, "Huanyang status flags")
-VAR(huanyang_debug, hb, bool, 0, 1, 1, "Huanyang debugging")
-VAR(huanyang_connected, he, bool, 0, 0, 1, "Huanyang connected")
-
-// GCode
-VAR(line, ln, int32_t, 0, 0, 1, "Last GCode line executed")
-VAR(unit, u, pstring, 0, 1, 1, "Current unit of measure")
-VAR(speed, s, float, 0, 1, 1, "Current spindle speed")
-VAR(feed, f, float, 0, 1, 1, "Current feed rate")
-VAR(tool, t, uint8_t, 0, 1, 1, "Current tool")
-VAR(feed_mode, fm, pstring, 0, 1, 1, "Current feed rate mode")
-VAR(plane, pa, pstring, 0, 1, 1, "Current plane")
-VAR(coord_system, cs, pstring, 0, 1, 1, "Current coordinate system")
-VAR(abs_override, ao, bool, 0, 1, 1, "Absolute override enabled")
-VAR(path_mode, pc, pstring, 0, 1, 1, "Current path control mode")
-VAR(distance_mode, dm, pstring, 0, 1, 1, "Current distance mode")
-VAR(arc_dist_mode, ad, pstring, 0, 1, 1, "Current arc distance mode")
-VAR(feed_override, fo, float, 0, 1, 1, "Feed rate override")
-VAR(speed_override, so, float, 0, 1, 1, "Spindle speed override")
-VAR(mist_coolant, mc, bool, 0, 1, 1, "Mist coolant enabled")
-VAR(flood_coolant, fc, bool, 0, 1, 1, "Flood coolant enabled")
-
-// System
-VAR(velocity, v, float, 0, 0, 1, "Current velocity")
-VAR(hw_id, id, string, 0, 0, 1, "Hardware ID")
-VAR(echo, ec, bool, 0, 1, 1, "Enable or disable echo")
-VAR(estop, es, bool, 0, 1, 1, "Emergency stop")
-VAR(estop_reason, er, pstring, 0, 0, 1, "Emergency stop reason")
-VAR(state, x, pstring, 0, 0, 1, "Machine state")
-VAR(cycle, c, pstring, 0, 0, 1, "Machine cycle")
-VAR(hold_reason, pr, pstring, 0, 0, 1, "Machine pause reason")
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2017 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 var_parse_bool(const char *value);
-
-void vars_init();
-
-void vars_report(bool full);
-void vars_report_all(bool enable);
-void vars_report_var(const char *code, bool enable);
-bool vars_print(const char *name);
-bool vars_set(const char *name, const char *value);
-float vars_get_number(const char *name);
-int vars_parser(char *vars);
-void vars_print_help();
+++ /dev/null
-#include "cpp_magic.h"
-{
-#define VAR(NAME, CODE, TYPE, INDEX, SET, REPORT, HELP) \
-#CODE: { \
- "name": #NAME, \
- "type": #TYPE, \
- "index": IF_ELSE(INDEX)(true, false), \
- "setable": IF_ELSE(SET)(true, false), \
- "report": IF_ELSE(REPORT)(true, false), \
- "help": HELP \
- },
-#include "vars.def"
-#undef VAR
- "_": {}
-}
+++ /dev/null
-TESTS=planner-test
-
-PLANNER_TEST_SRC = gcode_parser.c machine.c status.c util.c axis.c report.c \
- homing.c probing.c command.c vars.c
-PLANNER_TEST_SRC := $(patsubst %,../src/%,$(PLANNER_TEST_SRC))
-PLANNER_TEST_SRC += $(wildcard ../src/plan/*.c) planner-test.c hal.c
-
-CFLAGS = -I../src -Wall -Werror -DDEBUG -g -std=gnu99
-CFLAGS += -MD -MP -MT $@ -MF .dep/$(@F).d
-CFLAGS += -DF_CPU=320000000
-LDFLAGS = -lm
-
-all: $(TESTS)
-
-planner-test: $(PLANNER_TEST_SRC)
- gcc -o $@ $(PLANNER_TEST_SRC) $(CFLAGS) $(LDFLAGS)
-
-%.csv: %.gc planner-test
- ./planner-test < $< | grep -E '^-?[0-9.]+,'
-
-%-test: %.gc planner-test
- ./planner-test < $<
-
-%-plot: %.gc planner-test
- ./planner-test < $< | grep -E '^-?[0-9.]+,' | ./plot.py
-
-# Clean
-tidy:
- rm -f $(shell find -name \*~ -o -name \#\*) .dep
-
-clean: tidy
- rm -rf $(TESTS)
-
-.PHONY: tidy clean all
-
-# Dependencies
--include $(shell mkdir -p .dep) $(wildcard .dep/*)
+++ /dev/null
-$resume
-
-$0vm=10000
-$1vm=10000
-$2vm=10000
-$0jm=50
-$1jm=50
-$2jm=50
-
-G3 I2 J2 F10000
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2017 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 "spindle.h"
-#include "i2c.h"
-#include "cpp_magic.h"
-#include "plan/buffer.h"
-
-#include <stdint.h>
-#include <stdio.h>
-#include <stdlib.h>
-#include <math.h>
-
-
-typedef uint8_t flags_t;
-typedef const char *string;
-typedef const char *pstring;
-
-
-#define VAR(NAME, CODE, TYPE, INDEX, SET, ...) \
- TYPE get_##NAME(IF(INDEX)(int index)) __attribute__((weak)); \
- TYPE get_##NAME(IF(INDEX)(int index)) { \
- DEBUG_CALL(); \
- return 0; \
- } \
- IF(SET) \
- (void set_##NAME(IF(INDEX)(int index,) TYPE value) __attribute__((weak)); \
- void set_##NAME(IF(INDEX)(int index,) TYPE value) { \
- DEBUG_CALL(); \
- })
-
-#include "vars.def"
-#undef VAR
-
-
-void command_mreset(int argc, char *argv[]) {}
-void command_home(int argc, char *argv[]) {}
-void i2c_set_read_callback(i2c_read_cb_t cb) {}
-void print_status_flags(uint8_t flags) {DEBUG_CALL();}
-uint8_t hw_disable_watchdog() {return 0;}
-void hw_restore_watchdog(uint8_t state) {}
-
-
-bool estop = false;
-
-
-void estop_trigger(stat_t reason) {
- DEBUG_CALL("%s", status_to_pgmstr(reason));
- mp_queue_dump();
- estop = true;
- abort();
-}
-
-
-void estop_clear() {
- DEBUG_CALL();
- estop = false;
-}
-
-
-bool estop_triggered() {return estop;}
-
-
-void hw_request_hard_reset() {
- DEBUG_CALL();
- exit(0);
-}
-
-
-bool usart_tx_empty() {return true;}
-bool usart_tx_full() {return false;}
-
-
-char *usart_readline() {
- static char *cmd = 0;
-
- if (cmd) {
- free(cmd);
- cmd = 0;
- }
-
- size_t n = 0;
- if (getline(&cmd, &n, stdin) == -1) {
- free(cmd);
- cmd = 0;
- }
-
- return cmd;
-}
-
-
-void coolant_init() {}
-
-
-void coolant_set_mist(bool x) {
- DEBUG_CALL("%s", x ? "true" : "false");
-}
-
-
-void coolant_set_flood(bool x) {
- DEBUG_CALL("%s", x ? "true" : "false");
-}
-
-
-void spindle_init() {}
-
-
-void spindle_set_speed(float speed) {
- DEBUG_CALL("%f", speed);
-}
-
-
-void spindle_set_mode(spindle_mode_t mode) {
- DEBUG_CALL("%d", mode);
-}
-
-
-void spindle_stop() {}
-
-
-void motor_set_position(int motor, int32_t position) {
- DEBUG_CALL("%d, %d", motor, position);
-}
-
-
-bool switch_is_active(int index) {
- DEBUG_CALL("%d", index);
- return false;
-}
-
-
-bool switch_is_enabled(int index) {
- DEBUG_CALL("%d", index);
- return false;
-}
-
-
-static uint32_t ticks = 0;
-
-
-uint32_t rtc_get_time() {return ticks;}
-
-
-bool rtc_expired(uint32_t t) {
- return true;
- return 0 <= (int32_t)(ticks - t);
-}
-
-
-bool motor_is_enabled(int motor) {return true;}
-int motor_get_axis(int motor) {return motor;}
-
-
-#define MICROSTEPS 32
-#define TRAVEL_REV 5
-#define STEP_ANGLE 1.8
-
-
-float motor_position[MOTORS] = {0};
-
-
-float motor_get_steps_per_unit(int motor) {
- return 360 * MICROSTEPS / TRAVEL_REV / STEP_ANGLE;
-}
-
-
-int32_t motor_get_encoder(int motor) {
- DEBUG_CALL("%d", motor);
- return 0;
-}
-
-
-void motor_end_move(int motor) {
- DEBUG_CALL("%d", motor);
-}
-
-
-int32_t motor_get_error(int motor) {return 0;}
-int32_t motor_get_position(int motor) {return motor_position[motor];}
-
-
-bool st_is_busy() {return false;}
-
-
-float square(float x) {return x * x;}
-
-
-stat_t st_prep_line(float time, const float target[]) {
- DEBUG_CALL("%f, (%f, %f, %f, %f)",
- time, target[0], target[1], target[2], target[3]);
-
- float p[MOTORS] = {0, 0, 0, 0};
-
- for (int i = 0; i < MOTORS; i++) {
- motor_position[i] = target[i];
- p[i] = target[i] / motor_get_steps_per_unit(i);
- }
-
- printf("%0.10f, %0.10f, %0.10f, %0.10f\n", time, p[0], p[1], p[2]);
-
- return STAT_OK;
-}
-
-
-void st_prep_dwell(float seconds) {
- DEBUG_CALL("%f", seconds);
-}
+++ /dev/null
-$resume
-
-$0vm=10000
-$1vm=10000
-$2vm=10000
-$0jm=50
-$1jm=50
-$2jm=50
-
-G0 X500Y500
-G0 X0Y0
-G0 X-500Y0
-G0 X-500Y-500
-
-G0 Z-100
-G0 Z0
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2017 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 "machine.h"
-#include "command.h"
-#include "plan/arc.h"
-#include "plan/planner.h"
-#include "plan/exec.h"
-#include "plan/state.h"
-
-#include <stdio.h>
-#include <stdlib.h>
-
-
-int main(int argc, char *argv[]) {
- mp_init(); // motion planning
- machine_init(); // gcode machine
- for (int i = 0; i < 4; i++) axis_set_motor(i, i);
-
- stat_t status = STAT_OK;
-
- while (true) {
- mach_arc_callback(); // arc generation runs
-
- bool reading = !feof(stdin);
-
- if (reading && mp_queue_get_room()) {
- mp_state_callback();
- command_callback();
-
- if (mp_queue_get_room()) continue;
- }
-
- status = mp_exec_move();
- printf("EXEC: %s\n", status_to_pgmstr(status));
-
- switch (status) {
- case STAT_NOOP: 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;
- continue;
-
- default:
- printf("ERROR: %s\n", status_to_pgmstr(status));
- }
-
- if (!reading) break;
- }
-
- printf("STATE: %s\n", mp_get_state_pgmstr(mp_get_state()));
-
- return 0;
-}
+++ /dev/null
-#!/usr/bin/env python3
-
-import sys
-import csv
-import matplotlib.pyplot as plt
-import numpy as np
-import math
-
-
-def compute_velocity(rows):
- p = [0, 0, 0]
-
- for row in rows:
- t = float(row[0])
- d = 0.0
-
- for i in range(3):
- x = float(row[i + 1]) - p[i]
- d += x * x
- p[i] = float(row[i + 1])
-
- d = math.sqrt(d)
-
- yield d / t
-
-
-time_step = 0.005
-
-data = list(csv.reader(sys.stdin))
-
-velocity = list(compute_velocity(data))
-acceleration = np.diff(velocity)
-jerk = np.diff(acceleration)
-
-t = np.cumsum([float(row[0]) for row in data])
-x = [float(row[1]) for row in data]
-y = [float(row[2]) for row in data]
-z = [float(row[3]) for row in data]
-
-rows = 3
-row = 1
-if np.sum(x): rows += 1
-if np.sum(y): rows += 1
-if np.sum(z): rows += 1
-
-def next_subplot():
- global row
- plt.subplot(rows * 100 + 10 + row)
- row += 1
-
-if np.sum(x):
- next_subplot()
- plt.title('X')
- plt.plot(t, x, 'r')
- plt.ylabel(r'$mm$')
-
-if np.sum(y):
- next_subplot()
- plt.title('X')
- plt.title('Y')
- plt.plot(t, [row[2] for row in data], 'g')
- plt.ylabel(r'$mm$')
-
-if np.sum(z):
- next_subplot()
- plt.title('Z')
- plt.plot(t, [row[3] for row in data], 'b')
- plt.ylabel(r'$mm$')
-
-next_subplot()
-plt.title('Velocity')
-plt.plot(t, velocity, 'g')
-plt.ylabel(r'$\frac{mm}{min}$')
-
-next_subplot()
-plt.title('Acceleration')
-plt.plot(t[1:], acceleration, 'b')
-plt.ylabel(r'$\frac{mm}{min^2}$')
-
-next_subplot()
-plt.title('Jerk')
-plt.plot(t[2:], jerk, 'r')
-plt.ylabel(r'$\frac{mm}{min^3}$')
-plt.xlabel('Seconds')
-
-plt.tight_layout()
-plt.subplots_adjust(hspace = 0.5)
-mng = plt.get_current_fig_manager()
-mng.resize(*mng.window.maxsize())
-plt.show()
+++ /dev/null
-$resume
-
-$0vm=10000
-$1vm=10000
-$2vm=10000
-$0jm=50
-$1jm=50
-$2jm=50
-
-G0 X0.0
-G0 X0.1
-G0 X0.2
-G0 X0.3
-G0 X0.4
-G0 X0.5
-G0 X0.6
-G0 X0.7
-G0 X0.8
-G0 X0.9
-G0 X1.0
-G0 X1.1
-G0 X1.2
-G0 X1.3
-G0 X1.4
-G0 X1.5
-G0 X1.6
-G0 X1.7
-G0 X1.8
-G0 X1.9
-G0 X2.0
-G0 X2.1
-G0 X2.2
-G0 X2.3
-G0 X2.4
-G0 X2.5
-G0 X2.6
-G0 X2.7
-G0 X2.8
-G0 X2.9
-G0 X3.0
-G0 X3.1
-G0 X3.2
-G0 X3.3
-G0 X3.4
-G0 X3.5
-G0 X3.6
-G0 X3.7
-G0 X3.8
-G0 X3.9
-G0 X4.0
-G0 X4.1
-G0 X4.2
-G0 X4.3
-G0 X4.4
-G0 X4.5
-G0 X4.6
-G0 X4.7
-G0 X4.8
-G0 X4.9
--- /dev/null
+# Backup files
+*~
+\#*
+
+build
+.dep
+
+/*.eep
+/*.hex
+/*.elf
+/*.lss
+/*.map
--- /dev/null
+# Cubic Bezier
+
+ f(x) = A(1 - x)^3 + 3B(1 - x)^2 x + 3C(1 - x) x^2 + Dx^3
+
+ -Ax^3 + 3Ax^2 - 3Ax + A
+ 3Bx^3 - 6Bx^2 + 3Bx
+ -3Cx^3 + 3Cx^2
+ Dx^3
+
+
+ f(x) = (-A + 3B -3C + D)x^3 + (3A - 6B + 3C)x^2 + (-3A + 3B)x + A
+
+ a = -A + 3B - 3C + D
+ b = 3A - 6B + 3C
+ c = -3A + 3B
+ d = A
+
+ f(x) = ax^3 + bx^2 + cx + d
+
+ integral f(x) dx = a/4 x^4 + b/3 x^3 + c/2 x^2 + dx + E
+
+ = (-A + 3B - 3C + D)/4 x^4 + (A - 2B + B) x^3 + 3/2 (B - A) x^2 + Ax + E
+
+# Quintic Bezier
+
+ A(1 - x)^5 + 5A(1 - x)^4 x + 10A(1 - x)^3 x^2 + 10B(1 - x)^2 x^3 +
+ 5B(1 - x)x^4 + Bx^5
+
+ (-6A + 6B)x^5 + (15A - 15B)x^4 + (-10A + 10B)x^3 + A
+
+ 6(B - A)x^5 + 15(A - B)x^4 + 10(B - A)x^3 + A
+
+ x^3 (6(B - A)x^2 + 15(A - B)x + 10(B - A)) + A
+
+ a = 6(B - A)
+ b = -15(B - A)
+ c = 10(B - A)
+ d = A
+
+ f(x) = ax^5 + bx^4 + cx^3 + d
+
+ f(x) = (ax^2 + bx + c)x^3 + d
+
+
+ integral f(x) = a/6 x^6 + b/5 x^5 + c/4 x^4 + dx + e
+
+ = (B - A)x^6 - 3(B - A)x^5 + 5/2(B - A)x^4 + Ax + e
+
+ = (B - A)x^4 (x^2 - 3x + 5/2) + Ax + e
+
+ A = 0
+ B = 1
+ e = 0
+
+ f(x) = 6x^5 -15x^4 + 10x^3
+ int f(x) dx = x^6 - 3x^5 + 5/2x^4 + C
--- /dev/null
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2017, 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) -flto -fwhole-program
+
+CFLAGS += $(COMMON)
+CFLAGS += -Wall -Werror
+CFLAGS += -Wno-error=strict-aliasing # for _invsqrt
+CFLAGS += -std=gnu99 -DF_CPU=$(CLOCK)UL -O3
+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/boot/*.S)
+BOOT_SRC += $(wildcard src/boot/*.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:
+ @$(MAKE) $(PROJECT).hex
+ @$(MAKE) build/vars.json
+ @$(MAKE) boot
+ @$(MAKE) size
+
+boot: boot.hex boot-size
+
+build/vars.json: src/vars.def
+
+# Compile
+build/%.json: src/%.json.in
+ cpp -Isrc $< | sed '/^#.*$$/d' > $@
+
+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 $@
+
+boot.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: boot.elf
+ @$(MAKE) SIZE_TARGET=$< _size
+
+size: $(TARGET)
+ @$(MAKE) SIZE_TARGET=$< _size
+
+# Program
+init:
+ $(MAKE) erase
+ -$(MAKE) fuses
+ $(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: boot.hex
+ avrdude $(AVRDUDE_OPTS) -U flash:w:boot.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 boot.hex boot.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 difference 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 fragments for pulse
+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 interrupt, 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 interrupt signals a lower level
+interrupt to call ``mp_exec_move()`` and load the next move fragment.
--- /dev/null
+# Parsing, Queuing & Planning
+ * main()
+ * command_callback()
+ * gc_gcode_parser(const char *block)
+ * _normalize_gcode_block(...)
+ * _parse_gcode_block(const char *block)
+ * _execute_gcode_block()
+ * mach_*()
+ * mach_straight_traverse/feed() || mach_arc_feed()
+ * mp_aline(const float target[], float feed,. . .)
+ * _calc_jerk*()
+ * _calc_move_time()
+ * _calc_max_velocities()
+ * _get_junction_vmax()
+ * mp_plan()
+ * mp_calculate_trapezoid()
+ * mp_get_target_length()
+ * mp_get_target_velocity()
+ * mp_queue_push(buffer_cb_t cb, uint32_t time)
+
+# Execution
+ * STEP_LOW_LEVEL_ISR
+ * mp_exec_move()
+ * mp_queue_get_head()
+ * mp_buffer->cb()
+ * _exec_dwell()
+ * mp_exec_aline()
+ * _exec_aline_init()
+ * _exec_aline_head() || _exec_aline_body() || _exec_aline_tail()
+ * _exec_aline_section()
+ * mp_runtime_move_to_target()
+ * mp_kinematics() - Converts target mm to steps and maps motors
+ * st_prep_line()
+ * motor_prep_move()
+
+# Step Output
+ * STEP_TIMER_ISR
+ * _load_move()
+ * _end_move()
+ * motor_end_move()
+ * _request_exec_move()
+ * Triggers STEP_LOW_LEVEL_ISR
+ * motor_load_move()
+ * motor_end_move()
+
+
+# Data flow
+## GCode block
+char *line
+
+## Planner buffer
+ * mp_buffer_t pool[]
+ * float target[] - Target position in mm
+ * float unit[] - Direction vector
+ * float length, {head,body,tail}_length - Lengths in mm
+ * float {entry,cruise,exit,braking}_velocity - Target velocities in mm/min
+ * float {entry,cruise,exit,delta}_vmax - Max velocities in mm/min
+ * float jerk - Max jerk in km/min^3
+
+``mach_*()`` functions compute ``target`` and call ``mp_aline()``.
+``mp_aline()`` saves ``target`` computes ``unit`` and ``jerk``. Then it
+estimates the move time in order to calculate max velocities.
+
+``mp_plan()`` and friends compute target velocities and trapezoid segment
+lengths in two passes using max velocities in the current and neighboring
+planner buffers.
+
+``mp_exec_aline()`` executes the trapezoidal move in the next planner buffer.
+
+## Move prep
+ * stepper_t st
+ * uint16_t seg_period - The step timer period
+ * motor_t motors[]
+ * uint8_t timer_clock - Clock divisor or zero for off
+ * uint16_t timer_period - Clock period
+ * direction_t direction - Step polarity
--- /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 - 2017 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>
+#include <ctype.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 travel_max; // max work envelope for soft limits
+ float travel_min; // min work envelope for soft limits
+ float jerk_max; // max jerk (Jm) in km/min^3
+ 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] = {{0}};
+
+
+bool axis_is_enabled(int axis) {
+ int motor = axis_get_motor(axis);
+ return motor != -1 && motor_is_enabled(motor) &&
+ !fp_ZERO(axis_get_velocity_max(axis));
+}
+
+
+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, toupper(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;}
+
+
+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(homed, bool, false)
+AXIS_SET(homed, bool)
+AXIS_GET(homing_mode, homing_mode_t, HOMING_MANUAL)
+AXIS_SET(homing_mode, homing_mode_t)
+AXIS_GET(radius, float, 0)
+AXIS_GET(travel_min, float, 0)
+AXIS_GET(travel_max, float, 0)
+AXIS_GET(search_velocity, float, 0)
+AXIS_GET(latch_velocity, float, 0)
+AXIS_GET(zero_backoff, float, 0)
+AXIS_GET(latch_backoff, 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)
+
+
+AXIS_VAR_SET(velocity_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(jerk_max, float)
+
+
+float get_homing_dir(int axis) {
+ switch (axes[axis].homing_mode) {
+ case HOMING_MANUAL: break;
+ case HOMING_STALL_MIN: case HOMING_SWITCH_MIN: return -1;
+ case HOMING_STALL_MAX: case HOMING_SWITCH_MAX: return 1;
+ }
+ return 0;
+}
+
+
+float get_home(int axis) {
+ switch (axes[axis].homing_mode) {
+ case HOMING_MANUAL: break;
+ case HOMING_STALL_MIN: case HOMING_SWITCH_MIN: return get_travel_min(axis);
+ case HOMING_STALL_MAX: case HOMING_SWITCH_MAX: return get_travel_max(axis);
+ }
+ return NAN;
+}
+
+
+static int _get_homing_switch(int axis) {
+ switch (axes[axis].homing_mode) {
+ case HOMING_MANUAL: break;
+
+ case HOMING_STALL_MIN: case HOMING_SWITCH_MIN:
+ switch (axis) {
+ case AXIS_X: return SW_MIN_X;
+ case AXIS_Y: return SW_MIN_Y;
+ case AXIS_Z: return SW_MIN_Z;
+ case AXIS_A: return SW_MIN_A;
+ }
+ break;
+
+ case HOMING_STALL_MAX: case HOMING_SWITCH_MAX:
+ switch (axis) {
+ case AXIS_X: return SW_MAX_X;
+ case AXIS_Y: return SW_MAX_Y;
+ case AXIS_Z: return SW_MAX_Z;
+ case AXIS_A: return SW_MAX_A;
+ }
+ break;
+ }
+
+ return -1;
+}
+
+
+bool get_axis_can_home(int axis) {
+ return axis_is_enabled(axis) && axes[axis].homing_mode != HOMING_MANUAL &&
+ switch_is_enabled(_get_homing_switch(axis));
+}
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2017 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_MANUAL,
+ 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_jerk_max(int axis);
+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);
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2017 Buildbotics LLC
+ Copyright (c) 2010 Alex Forencich <alex@alexforencich.com>
+ 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 "boot.h"
+#include "sp_driver.h"
+
+#include <util/delay.h>
+#include <util/crc16.h>
+
+#include <avr/eeprom.h>
+#include <avr/io.h>
+
+#include <stdbool.h>
+
+
+uint8_t buffer[SPM_PAGESIZE];
+uint16_t block_crc = 0;
+
+
+void clock_init() {
+ // 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
+}
+
+
+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);
+ UART_DEVICE.CTRLB = USART_RXEN_bm | USART_TXEN_bm;
+
+ 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;
+}
+
+
+void watchdog_init() {
+ uint8_t temp = WDT_ENABLE_bm | WDT_CEN_bm | WDT_PER_4KCLK_gc;
+ CCP = CCP_IOREG_gc;
+ WDT.CTRL = temp;
+ while (WDT.STATUS & WDT_SYNCBUSY_bm) continue;
+}
+
+
+void watchdog_reset() {asm("wdr");}
+
+
+void watchdog_disable() {
+ uint8_t temp = (WDT.CTRL & ~WDT_ENABLE_bm) | WDT_CEN_bm;
+ CCP = CCP_IOREG_gc;
+ WDT.CTRL = temp;
+}
+
+
+void nvm_wait() {while (NVM_STATUS & NVM_NVMBUSY_bp) watchdog_reset();}
+
+
+void nvm_exec() {
+ void *z = (void *)&NVM_CTRLA;
+
+ __asm__ volatile
+ ("out %[ccp], %[ioreg]\n"
+ "st z, %[cmdex]" ::
+ [ccp] "I" (_SFR_IO_ADDR(CCP)),
+ [ioreg] "d" (CCP_IOREG_gc),
+ [cmdex] "r" (NVM_CMDEX_bm),
+ [z] "z" (z));
+}
+
+
+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() {
+ uint8_t hi = get_char();
+ uint8_t lo = get_char();
+ return ((uint16_t)hi << 8) | lo;
+}
+
+
+uint8_t BlockLoad(unsigned size, uint8_t mem, uint32_t *address) {
+ watchdog_reset();
+
+ // fill up buffer
+ block_crc = 0xffff;
+ for (int i = 0; i < SPM_PAGESIZE; i++)
+ if (i < size) {
+ buffer[i] = get_char();
+ block_crc = _crc16_update(block_crc, buffer[i]);
+
+ } else buffer[i] = 0xff;
+
+ switch (mem) {
+ case MEM_EEPROM:
+ eeprom_write_block(buffer, (uint8_t *)(uint16_t)*address, 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) {
+ switch (mem) {
+ case MEM_EEPROM:
+ eeprom_read_block(buffer, (uint8_t *)(uint16_t)*address, size);
+ *address += size;
+
+ // send bytes
+ for (int i = 0; i < size; i++)
+ send_char(buffer[i]);
+ break;
+
+ case MEM_FLASH: case MEM_USERSIG: case MEM_PRODSIG: {
+ *address <<= 1; // Convert address to bytes temporarily
+
+ do {
+ switch (mem) {
+ case MEM_FLASH: send_char(SP_ReadByte(*address)); break;
+ case MEM_USERSIG: send_char(SP_ReadUserSignatureByte(*address)); break;
+ case MEM_PRODSIG: send_char(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: break;
+ }
+}
+
+
+int main() {
+ // Init
+ clock_init();
+ uart_init();
+ watchdog_init();
+
+ // Check for trigger
+ bool in_bootloader = false;
+ uint16_t j = INITIAL_WAIT;
+ while (!in_bootloader && 0 < j--) {
+ if (uart_has_char()) in_bootloader = uart_recv_char() == CMD_SYNC;
+ watchdog_reset();
+ _delay_ms(1);
+ }
+
+ // Main bootloader
+ uint32_t address = 0;
+ uint16_t i = 0;
+
+ while (in_bootloader) {
+ uint8_t val = get_char();
+ watchdog_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: {
+ uint8_t hi = get_char();
+ address = ((uint32_t)hi << 16) | get_word();
+ send_char(REPLY_ACK);
+ break;
+ }
+
+ case CMD_FLASH_ERASE:
+ SP_EraseApplicationSection();
+ nvm_wait();
+ send_char(REPLY_ACK);
+ break;
+
+ case CMD_EEPROM_ERASE:
+ NVM.CMD = NVM_CMD_ERASE_EEPROM_gc;
+ nvm_exec();
+ send_char(REPLY_ACK);
+ break;
+
+ case CMD_CHECK_BLOCK_SUPPORT:
+ send_char(REPLY_YES);
+ // Send block size (page size)
+ send_char(SPM_PAGESIZE >> 8);
+ send_char((uint8_t)SPM_PAGESIZE);
+ 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((uint8_t *)(uint16_t)address, get_char());
+ address++;
+ send_char(REPLY_ACK);
+ break;
+
+ case CMD_READ_EEPROM_BYTE:
+ send_char(eeprom_read_byte((uint8_t *)(uint16_t)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('b');
+ send_char('b');
+ send_char('c');
+ send_char('t');
+ send_char('r');
+ send_char('l');
+ send_char(' ');
+ break;
+
+ case CMD_VERSION:
+ send_char('0');
+ send_char('2');
+ break;
+
+ case CMD_READ_SIGNATURE:
+ send_char(SIGNATURE_2);
+ send_char(SIGNATURE_1);
+ send_char(SIGNATURE_0);
+ break;
+
+ case CMD_READ_CHECKSUM:
+ // Setup
+ nvm_wait();
+
+ // Reset CRC
+ CRC_CTRL |= CRC_RESET_RESET1_gc;
+ CRC.CHECKSUM0 = CRC.CHECKSUM1 = CRC.CHECKSUM2 = CRC.CHECKSUM3 = 0xff;
+
+ // 32-bit mode, flash source
+ CRC_CTRL = CRC_CRC32_bm | CRC_SOURCE_FLASH_gc;
+
+ // Start address
+ NVM.ADDR0 = (uint8_t)(APP_SECTION_START >> 0);
+ NVM.ADDR1 = (uint8_t)(APP_SECTION_START >> 8);
+ NVM.ADDR2 = 0;
+
+ // End address
+ NVM.DATA0 = (uint8_t)(APP_SECTION_END >> 0);
+ NVM.DATA1 = (uint8_t)(APP_SECTION_END >> 8);
+ NVM.DATA2 = (uint8_t)(APP_SECTION_END >> 16);
+
+ NVM.CMD = NVM_CMD_FLASH_RANGE_CRC_gc;
+ CCP = CCP_IOREG_gc;
+ NVM.CTRLA = NVM_CMDEX_bm;
+
+ // Compute
+ nvm_wait();
+ while (CRC.STATUS & CRC_BUSY_bm) continue;
+
+ // Send 32-bit checksum
+ send_char(CRC.CHECKSUM3);
+ send_char(CRC.CHECKSUM2);
+ send_char(CRC.CHECKSUM1);
+ send_char(CRC.CHECKSUM0);
+ break;
+
+ case CMD_FLASH_LENGTH:
+ send_char((uint8_t)(APP_SECTION_SIZE >> 16));
+ send_char((uint8_t)(APP_SECTION_SIZE >> 8));
+ send_char((uint8_t)(APP_SECTION_SIZE >> 0));
+ break;
+
+ case CMD_BLOCK_CRC:
+ send_char(block_crc >> 8);
+ send_char((uint8_t)block_crc);
+ 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();
+ }
+
+ // Deinit
+ uart_deinit();
+ watchdog_disable();
+
+ // Disable further self programming until next reset
+ SP_LockSPM();
+
+ // Jump to application code
+ asm("jmp 0");
+}
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2017 Buildbotics LLC
+ Copyright (c) 2010 Alex Forencich <alex@alexforencich.com>
+ 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
+
+#define INITIAL_WAIT 1000 // In ms
+
+#define UART_RX_PIN 2
+#define UART_TX_PIN 3
+#define UART_PORT PORTC
+#define UART_DEVICE USARTC0
+
+// Baud rate 921600 @ 32Mhz
+#define UART_BSEL_VALUE 150
+#define UART_BSCALE_VALUE -7
+
+
+// Protocol
+enum {
+ CMD_SYNC = '\x1b',
+
+ // Informational
+ CMD_CHECK_AUTOINCREMENT = 'a',
+ CMD_CHECK_BLOCK_SUPPORT = 'b',
+ CMD_PROGRAMMER_TYPE = 'p',
+ CMD_DEVICE_CODE = 't',
+ CMD_PROGRAM_ID = 'S',
+ CMD_VERSION = 'V',
+ CMD_HW_VERSION = 'v', // Unsupported extension
+ CMD_READ_SIGNATURE = 's',
+ CMD_READ_CHECKSUM = 'X',
+ CMD_FLASH_LENGTH = 'n',
+
+ // Addressing
+ CMD_SET_ADDRESS = 'A',
+ CMD_SET_EXT_ADDRESS = 'H',
+
+ // Erase
+ CMD_FLASH_ERASE = 'e',
+ CMD_EEPROM_ERASE = '_',
+
+ // Block Access
+ CMD_BLOCK_LOAD = 'B',
+ CMD_BLOCK_READ = 'g',
+ CMD_BLOCK_CRC = 'i',
+
+ // Byte Access
+ CMD_READ_BYTE = 'R',
+ CMD_WRITE_LOW_BYTE = 'c',
+ CMD_WRITE_HIGH_BYTE = 'C',
+ CMD_WRITE_PAGE = 'm',
+ CMD_WRITE_EEPROM_BYTE = 'D',
+ CMD_READ_EEPROM_BYTE = 'd',
+
+ // Lock and Fuse Bits
+ CMD_WRITE_LOCK_BITS = 'l',
+ CMD_READ_LOCK_BITS = 'r',
+ CMD_READ_LOW_FUSE_BITS = 'F',
+ CMD_READ_HIGH_FUSE_BITS = 'N',
+ CMD_READ_EXT_FUSE_BITS = 'Q',
+
+ // Control
+ CMD_ENTER_PROG_MODE = 'P',
+ CMD_LEAVE_PROG_MODE = 'L',
+ CMD_EXIT_BOOTLOADER = 'E',
+ CMD_SET_LED = 'x',
+ CMD_CLEAR_LED = 'y',
+ CMD_SET_TYPE = 'T',
+};
+
+
+// Memory types for block access
+enum {
+ MEM_EEPROM = 'E',
+ MEM_FLASH = 'F',
+ MEM_USERSIG = 'U',
+ MEM_PRODSIG = 'P',
+};
+
+
+// Command Responses
+enum {
+ REPLY_ACK = '\r',
+ REPLY_YES = 'Y',
+ REPLY_ERROR = '?',
+};
--- /dev/null
+;******************************************************************************
+;* $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.
+;******************************************************************************
+;*
+;* 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 finished using any of the functions in this driver.
+;*
+;* For all routines, it is important that any interrupt handlers do not
+;* perform NVM operations. The user must implement a scheme for mutually
+;* exclusive access to 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
+
+#include <avr/io.h>
+
+; Defines not yet included in header file.
+#define NVM_CMD_NO_OPERATION_gc 0x00
+#define NVM_CMD_READ_USER_SIG_ROW_gc 0x01
+#define NVM_CMD_READ_CALIB_ROW_gc 0x02
+#define NVM_CMD_READ_EEPROM_gc 0x06
+#define NVM_CMD_READ_FUSES_gc 0x07
+#define NVM_CMD_WRITE_LOCK_BITS_gc 0x08
+#define NVM_CMD_ERASE_USER_SIG_ROW_gc 0x18
+#define NVM_CMD_WRITE_USER_SIG_ROW_gc 0x1a
+#define NVM_CMD_ERASE_APP_gc 0x20
+#define NVM_CMD_ERASE_APP_PAGE_gc 0x22
+#define NVM_CMD_LOAD_FLASH_BUFFER_gc 0x23
+#define NVM_CMD_WRITE_APP_PAGE_gc 0x24
+#define NVM_CMD_ERASE_WRITE_APP_PAGE_gc 0x25
+#define NVM_CMD_ERASE_FLASH_BUFFER_gc 0x26
+#define NVM_CMD_ERASE_BOOT_PAGE_gc 0x2a
+#define NVM_CMD_WRITE_BOOT_PAGE_gc 0x2c
+#define NVM_CMD_ERASE_WRITE_BOOT_PAGE_gc 0x2d
+#define NVM_CMD_ERASE_EEPROM_gc 0x30
+#define NVM_CMD_ERASE_EEPROM_PAGE_gc 0x32
+#define NVM_CMD_LOAD_EEPROM_BUFFER_gc 0x33
+#define NVM_CMD_WRITE_EEPROM_PAGE_gc 0x34
+#define NVM_CMD_ERASE_WRITE_EEPROM_PAGE_gc 0x35
+#define NVM_CMD_ERASE_EEPROM_BUFFER_gc 0x36
+#define NVM_CMD_APP_CRC_gc 0x38
+#define NVM_CMD_BOOT_CRC_gc 0x39
+#define NVM_CMD_FLASH_RANGE_CRC_gc 0x3a
+#define CCP_SPM_gc 0x9d
+#define CCP_IOREG_gc 0xd8
+
+
+; 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
+
+
+; 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
+
+
+; 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.
+
+
+; 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.
+
+
+; 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 index into NVM Address Reg 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
+
+
+; Erases the user signature row.
+.section .text
+.global SP_EraseUserSignatureRow
+
+SP_EraseUserSignatureRow:
+ in r19, RAMPZ ; Save RAMPZ, 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.
+
+
+; Writes the flash buffer to the user signature row.
+.section .text
+.global SP_WriteUserSignatureRow
+
+SP_WriteUserSignatureRow:
+ in r19, RAMPZ ; Save RAMPZ, 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.
+
+
+; Erases the entire application section.
+.section .text
+.global SP_EraseApplicationSection
+
+SP_EraseApplicationSection:
+ in r19, RAMPZ ; Save RAMPZ, 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.
+
+
+; 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.
+.section .text
+.global SP_LoadFlashWord
+
+SP_LoadFlashWord:
+ in r19, RAMPZ ; Save RAMPZ, 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.
+
+
+; 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.
+.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 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 (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
+
+
+; Writes the page buffer to Flash 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.
+.section .text
+.global SP_WriteApplicationPage
+
+SP_WriteApplicationPage:
+ in r19, RAMPZ ; Save RAMPZ, 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.
+
+
+; 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.
+.section .text
+.global SP_EraseWriteApplicationPage
+
+SP_EraseWriteApplicationPage:
+ in r19, RAMPZ ; Save RAMPZ, 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.
+
+
+
+; Locks all further access to SPM operations until next reset.
+.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
+ ; (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
+
+
+; 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.
+.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
+
+
+; 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
+ ; (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
+
+
+; 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
+
+
+; 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.
+.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 (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
--- /dev/null
+/*******************************************************************************
+ * $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.
+ ******************************************************************************/
+
+/*! \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
+ * Support email: avr@atmel.com
+ ******************************************************************************/
+
+#pragma once
+
+#include <avr/io.h>
+
+#include <stdint.h>
+
+#ifndef APP_SECTION_PAGE_SIZE
+#error APP_SECTION_PAGE_SIZE must be defined if not defined in header files.
+#endif
+
+#ifndef APPTABLE_SECTION_START
+#error APPTABLE_SECTION_START must be defined if not defined in header files.
+#endif
+
+
+/*! \brief Read a byte from flash.
+ *
+ * \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.
+ *
+ * \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 Read user signature at given index.
+ *
+ * \param index Index of the byte in the user signature row.
+ * \retval User signature byte
+ */
+uint8_t SP_ReadUserSignatureByte(uint16_t index);
+
+/// Erase user signature row.
+void SP_EraseUserSignatureRow();
+
+/// Write user signature row.
+void SP_WriteUserSignatureRow();
+
+/*! \brief Erase entire application 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();
+
+/*! \brief Erase and write page buffer to application or application table
+ * section at byte address.
+ *
+ * \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.
+ *
+ * \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.
+ *
+ * \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.
+ *
+ * \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);
+
+/// Flush Flash page buffer.
+void SP_EraseFlashBuffer();
+
+/// Disables the use of SPM until the next reset.
+void SP_LockSPM();
+
+/// Waits for the SPM to finish and clears the command register.
+void SP_WaitForSPM();
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2017 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 "i2c.h"
+#include "plan/buffer.h"
+#include "plan/state.h"
+#include "config.h"
+#include "pgmspace.h"
+
+#ifdef __AVR__
+#include <avr/wdt.h>
+#endif
+
+#include <stdio.h>
+#include <string.h>
+#include <ctype.h>
+#include <stdlib.h>
+
+
+static char *_cmd = 0;
+
+
+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_REBOOT: hw_request_hard_reset(); 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;
+}
+
+
+char *_parse_arg(char **p) {
+ char *start = *p;
+ char *next = *p;
+
+ bool inQuote = false;
+ bool escape = false;
+
+ while (**p) {
+ char c = *(*p)++;
+
+ switch (c) {
+ case '\\':
+ if (!escape) {
+ escape = true;
+ continue;
+ }
+ break;
+
+ case ' ': case '\t':
+ if (!inQuote && !escape) goto done;
+ break;
+
+ case '"':
+ if (!escape) {
+ inQuote = !inQuote;
+ continue;
+ }
+ break;
+
+ default: break;
+ }
+
+ *next++ = c;
+ escape = false;
+ }
+
+ done:
+ *next = 0;
+ return start;
+}
+
+
+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
+ char *arg = _parse_arg(&p);
+ if (*arg) argv[argc++] = arg;
+ }
+
+ // 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_is_ready()) return; // Wait for motion planner
+
+ // 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 = " $%-12"PRPSTR" %"PRPSTR"\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);
+#ifdef __AVR__
+ wdt_reset();
+#endif
+ }
+
+ puts_P(PSTR("\nVariables:"));
+ vars_print_help();
+
+ return STAT_OK;
+}
+
+
+uint8_t command_report(int argc, char *argv[]) {
+ if (argc == 2) {
+ vars_report_all(var_parse_bool(argv[1]));
+ return STAT_OK;
+ }
+
+ vars_report_var(argv[1], var_parse_bool(argv[2]));
+ return STAT_OK;
+}
+
+
+uint8_t command_reboot(int argc, char *argv[]) {
+ hw_request_hard_reset();
+ 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 - 2017 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(report, 1, 2, "[var] <enable>. Enable or disable var reporting")
+CMD(reboot, 0, 0, "Reboot the controller")
+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")
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2017 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 - 2017 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"
+
+#ifdef __AVR__
+#include <avr/interrupt.h>
+#endif
+
+
+// Pins
+enum {
+ STALL_X_PIN = PORT_A << 3,
+ STALL_Y_PIN,
+ STALL_Z_PIN,
+ STALL_A_PIN,
+ TOOL_DIR_PIN,
+ TOOL_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
+
+
+#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 10 // number of supported switches
+#define OUTS 5 // number of supported pin outputs
+
+
+// Switch settings. See switch.c
+#define SWITCH_INTLVL PORT_INT0LVL_MED_gc
+
+
+// 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 ((uint16_t)(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 STEP_LOW_LEVEL_ISR ADCB_CH0_vect
+
+#define SEGMENT_USEC 5000.0 // segment time
+#define SEGMENT_SEC (SEGMENT_USEC / 1000000.0)
+#define SEGMENT_TIME (SEGMENT_SEC / 60.0)
+#define SEGMENT_CLOCKS ((uint24_t)(F_CPU * SEGMENT_SEC))
+#define SEGMENT_PERIOD ((uint16_t)(STEP_TIMER_FREQ * SEGMENT_SEC))
+
+
+// DRV8711 settings
+#if 0 // Doug's settings
+#define DRV8711_OFF 48
+#define DRV8711_BLANK (0x80 | DRV8711_BLANK_ABT_bm)
+#define DRV8711_DECAY (DRV8711_DECAY_DECMOD_AUTO_OPT | 6)
+
+#else
+#define DRV8711_OFF 12
+#define DRV8711_BLANK (0x32 | DRV8711_BLANK_ABT_bm)
+#define DRV8711_DECAY (DRV8711_DECAY_DECMOD_MIXED | 16)
+#endif
+
+#define DRV8711_STALL (DRV8711_STALL_SDCNT_2 | DRV8711_STALL_VDIV_4 | 200)
+#define DRV8711_DRIVE (DRV8711_DRIVE_IDRIVEP_50 | \
+ DRV8711_DRIVE_IDRIVEN_100 | DRV8711_DRIVE_TDRIVEP_250 | \
+ DRV8711_DRIVE_TDRIVEN_250 | DRV8711_DRIVE_OCPDEG_2 | \
+ DRV8711_DRIVE_OCPTH_500)
+#define DRV8711_TORQUE DRV8711_TORQUE_SMPLTH_200
+#define DRV8711_CTRL (DRV8711_CTRL_ISGAIN_10 | DRV8711_CTRL_DTIME_450 | \
+ DRV8711_CTRL_EXSTALL_bm)
+
+
+// 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)
+
+
+// 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. 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
+
+
+// Settings ********************************************************************
+
+// Motor settings. See motor.c
+#define MOTOR_IDLE_TIMEOUT 0.25 // secs, motor off after this time
+
+
+#define MIN_HALF_STEP_CORRECTION 4
+#define CHORDAL_TOLERANCE 0.01 // chordal accuracy for arcs
+#define JUNCTION_DEVIATION 0.05 // default value, in mm
+#define JUNCTION_ACCELERATION 100000 // centripetal corner accel
+#define JOG_MIN_VELOCITY 10 // mm/min
+#define CAL_ACCELERATION 500000 // mm/min^2
+#define CURRENT_SENSE_RESISTOR 0.05 // ohms
+#define CURRENT_SENSE_REF 2.75 // volts
+#define MAX_CURRENT 10 // amps
+
+// 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
+
+
+// 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
+#define GCODE_MAX_OPERATOR_DEPTH 16
+#define GCODE_MAX_VALUE_DEPTH 32
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2017 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) {SET_PIN(SWITCH_1_PIN, !x);}
+void coolant_set_flood(bool x) {SET_PIN(SWITCH_2_PIN, !x);}
+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 - 2017 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 - 2017 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 "stepper.h"
+#include "report.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))
+
+
+bool motor_fault = false;
+
+
+typedef struct {
+ float current;
+ uint16_t isgain;
+ uint8_t torque;
+} current_t;
+
+
+typedef struct {
+ uint8_t status;
+ uint16_t flags;
+
+ drv8711_state_t state;
+ current_t drive;
+ current_t idle;
+ float stall_threshold;
+
+ 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 _current_set(current_t *c, float current) {
+ c->current = current;
+
+ float torque_over_gain = current * CURRENT_SENSE_RESISTOR / CURRENT_SENSE_REF;
+
+ float gain = 0;
+ if (torque_over_gain < 1.0 / 40) {
+ c->isgain = DRV8711_CTRL_ISGAIN_40;
+ gain = 40;
+
+ } else if (torque_over_gain < 1.0 / 20) {
+ c->isgain = DRV8711_CTRL_ISGAIN_20;
+ gain = 20;
+
+ } else if (torque_over_gain < 1.0 / 10) {
+ c->isgain = DRV8711_CTRL_ISGAIN_10;
+ gain = 10;
+
+ } else if (torque_over_gain < 1.0 / 5) {
+ c->isgain = DRV8711_CTRL_ISGAIN_5;
+ gain = 5;
+ }
+
+ c->torque = round(torque_over_gain * gain * 256);
+}
+
+
+static bool _driver_get_enabled(int driver) {
+ drv8711_state_t state = drivers[driver].state;
+ return state == DRV8711_IDLE || state == DRV8711_ACTIVE;
+}
+
+
+static float _driver_get_current(int driver) {
+ drv8711_driver_t *drv = &drivers[driver];
+
+ switch (drv->state) {
+ case DRV8711_IDLE: return drv->idle.current;
+ case DRV8711_ACTIVE: return drv->drive.current;
+ default: return 0; // Off
+ }
+}
+
+
+static uint16_t _driver_get_isgain(int driver) {
+ drv8711_driver_t *drv = &drivers[driver];
+
+ switch (drv->state) {
+ case DRV8711_IDLE: return drv->idle.isgain;
+ case DRV8711_ACTIVE: return drv->drive.isgain;
+ default: return 0; // Off
+ }
+}
+
+
+static uint8_t _driver_get_torque(int driver) {
+ drv8711_driver_t *drv = &drivers[driver];
+
+ switch (drv->state) {
+ case DRV8711_IDLE: return drv->idle.torque;
+ case DRV8711_ACTIVE: return drv->drive.torque;
+ default: return 0; // Off
+ }
+}
+
+
+static uint8_t _spi_next_command(uint8_t cmd) {
+ // Process command responses
+ for (int driver = 0; driver < DRIVERS; driver++) {
+ drv8711_driver_t *drv = &drivers[driver];
+ uint16_t command = spi.commands[driver][cmd];
+
+ if (DRV8711_CMD_IS_READ(command))
+ switch (DRV8711_CMD_ADDR(command)) {
+ case DRV8711_STATUS_REG:
+ drv->status = spi.responses[driver];
+
+ if ((drv->status & drv->flags) != drv->status) {
+ drv->flags |= drv->status;
+ report_request();
+ }
+ break;
+
+ case DRV8711_OFF_REG:
+ // We read back the OFF register to test for communication failure.
+ if ((spi.responses[driver] & 0x1ff) != DRV8711_OFF)
+ drv->flags |= DRV8711_COMM_ERROR_bm;
+ break;
+ }
+ }
+
+ // Next command
+ if (++cmd == spi.ncmds) {
+ cmd = 0; // Wrap around
+ st_enable(); // Enable motors
+ }
+
+ // Prep next command
+ for (int driver = 0; driver < DRIVERS; driver++) {
+ drv8711_driver_t *drv = &drivers[driver];
+ uint16_t *command = &spi.commands[driver][cmd];
+
+ switch (DRV8711_CMD_ADDR(*command)) {
+ case DRV8711_STATUS_REG:
+ if (!DRV8711_CMD_IS_READ(*command))
+ // Clear STATUS flags
+ *command = (*command & 0xf000) | (0x0fff & ~(drv->status));
+ break;
+
+ case DRV8711_TORQUE_REG: // Update motor current setting
+ *command = (*command & 0xff00) | _driver_get_torque(driver);
+ break;
+
+ case DRV8711_CTRL_REG: // Set microsteps
+ *command = (*command & 0xfc86) | _driver_get_isgain(driver) |
+ (drv->mode << 3) |
+ (_driver_get_enabled(driver) ? DRV8711_CTRL_ENBL_bm : 0);
+ 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;
+
+ commands[spi.ncmds++] = DRV8711_WRITE(DRV8711_OFF_REG, DRV8711_OFF);
+ commands[spi.ncmds++] = DRV8711_WRITE(DRV8711_BLANK_REG, DRV8711_BLANK);
+ commands[spi.ncmds++] = DRV8711_WRITE(DRV8711_DECAY_REG, DRV8711_DECAY);
+ commands[spi.ncmds++] = DRV8711_WRITE(DRV8711_STALL_REG, DRV8711_STALL);
+ commands[spi.ncmds++] = DRV8711_WRITE(DRV8711_DRIVE_REG, DRV8711_DRIVE);
+ commands[spi.ncmds++] = DRV8711_WRITE(DRV8711_TORQUE_REG, DRV8711_TORQUE);
+ commands[spi.ncmds++] = DRV8711_WRITE(DRV8711_CTRL_REG, DRV8711_CTRL);
+ commands[spi.ncmds++] = DRV8711_READ(DRV8711_OFF_REG);
+ commands[spi.ncmds++] = DRV8711_READ(DRV8711_STATUS_REG);
+ 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() {
+ // 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();
+}
+
+
+drv8711_state_t drv8711_get_state(int driver) {
+ if (driver < 0 || DRIVERS <= driver) return DRV8711_DISABLED;
+ return drivers[driver].state;
+}
+
+
+void drv8711_set_state(int driver, drv8711_state_t state) {
+ if (driver < 0 || DRIVERS <= driver) return;
+ drivers[driver].state = state;
+}
+
+
+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_current(int driver) {
+ if (driver < 0 || DRIVERS <= driver) return 0;
+ return drivers[driver].drive.current;
+}
+
+
+void set_drive_current(int driver, float value) {
+ if (driver < 0 || DRIVERS <= driver || value < 0 || MAX_CURRENT < value)
+ return;
+ _current_set(&drivers[driver].drive, value);
+}
+
+
+float get_idle_current(int driver) {
+ if (driver < 0 || DRIVERS <= driver) return 0;
+ return drivers[driver].idle.current;
+}
+
+
+void set_idle_current(int driver, float value) {
+ if (driver < 0 || DRIVERS <= driver || value < 0 || MAX_CURRENT < value)
+ return;
+
+ _current_set(&drivers[driver].idle, value);
+}
+
+
+float get_active_current(int driver) {
+ if (driver < 0 || DRIVERS <= driver) return 0;
+ return _driver_get_current(driver);
+}
+
+
+bool get_motor_fault() {return motor_fault;}
+
+
+uint16_t get_driver_flags(int driver) {return drivers[driver].flags;}
+
+
+void print_status_flags(uint16_t flags) {
+ bool first = true;
+
+ putchar('"');
+
+ if (DRV8711_STATUS_OTS_bm & flags) {
+ if (!first) printf_P(PSTR(", "));
+ printf_P(PSTR("temp"));
+ first = false;
+ }
+
+ if (DRV8711_STATUS_AOCP_bm & flags) {
+ if (!first) printf_P(PSTR(", "));
+ printf_P(PSTR("current a"));
+ first = false;
+ }
+
+ if (DRV8711_STATUS_BOCP_bm & flags) {
+ if (!first) printf_P(PSTR(", "));
+ printf_P(PSTR("current b"));
+ first = false;
+ }
+
+ if (DRV8711_STATUS_APDF_bm & flags) {
+ if (!first) printf_P(PSTR(", "));
+ printf_P(PSTR("fault a"));
+ first = false;
+ }
+
+ if (DRV8711_STATUS_BPDF_bm & flags) {
+ if (!first) printf_P(PSTR(", "));
+ printf_P(PSTR("fault b"));
+ first = false;
+ }
+
+ if (DRV8711_STATUS_UVLO_bm & flags) {
+ if (!first) printf_P(PSTR(", "));
+ printf_P(PSTR("uvlo"));
+ first = false;
+ }
+
+ if ((DRV8711_STATUS_STD_bm | DRV8711_STATUS_STDLAT_bm) & flags) {
+ if (!first) printf_P(PSTR(", "));
+ printf_P(PSTR("stall"));
+ first = false;
+ }
+
+ if (DRV8711_COMM_ERROR_bm & flags) {
+ if (!first) printf_P(PSTR(", "));
+ printf_P(PSTR("comm"));
+ first = false;
+ }
+
+ putchar('"');
+}
+
+
+uint16_t get_status_strings(int driver) {return get_driver_flags(driver);}
+
+
+// Command callback
+void command_mreset(int argc, char *argv[]) {
+ if (argc == 1)
+ for (int driver = 0; driver < DRIVERS; driver++)
+ drivers[driver].flags = 0;
+
+ else {
+ int driver = atoi(argv[1]);
+ if (driver < DRIVERS) drivers[driver].flags = 0;
+ }
+
+ report_request();
+}
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2017 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,
+ DRV8711_COMM_ERROR_bm = 1 << 8,
+};
+
+
+#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))
+
+
+typedef enum {
+ DRV8711_DISABLED,
+ DRV8711_IDLE,
+ DRV8711_ACTIVE,
+} drv8711_state_t;
+
+
+typedef void (*stall_callback_t)(int driver);
+
+
+void drv8711_init();
+drv8711_state_t drv8711_get_state(int driver);
+void drv8711_set_state(int driver, drv8711_state_t state);
+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 - 2017 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 "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
+ SET_PIN(FAULT_PIN, estop.triggered);
+ 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_stop();
+
+ // Set machine state
+ mp_state_estop();
+
+ // 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 - 2017 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 - 2017 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_expr.h"
+
+#include "gcode_parser.h"
+#include "vars.h"
+
+#include <math.h>
+#include <ctype.h>
+#include <stdlib.h>
+
+
+float parse_gcode_number(char **p) {
+ // Avoid parsing G0X10 as a hexadecimal number
+ if (**p == '0' && toupper(*(*p + 1)) == 'X') {
+ (*p)++; // pointer points to X
+ return 0;
+ }
+
+ // Skip leading zeros so we don't parse as octal
+ while (**p == '0' && isdigit(*(*p + 1))) p++;
+
+ // Parse number
+ char *end;
+ float value = strtod(*p, &end);
+ if (end == *p) {
+ parser.error = STAT_BAD_NUMBER_FORMAT;
+ return 0;
+ }
+ *p = end; // Point to next character after the word
+
+ return value;
+}
+
+
+static float _parse_gcode_var(char **p) {
+ (*p)++; // Skip #
+
+ if (isdigit(**p)) {
+ // TODO numbered parameters
+ parser.error = STAT_GCODE_NUM_PARAM_UNSUPPORTED;
+
+ } else if (**p == '<') {
+ (*p)++;
+
+ // Assigning vars is not supported so the '_' global prefix is optional
+ if (**p == '_') (*p)++;
+
+ char *name = *p;
+ while (**p && **p != '>') (*p)++;
+
+ if (**p != '>') parser.error = STAT_GCODE_UNTERMINATED_VAR;
+ else {
+ *(*p)++ = 0; // Null terminate
+ return vars_get_number(name);
+ }
+ }
+
+ return 0;
+}
+
+
+static float _parse_gcode_func(char **p) {
+ // TODO LinuxCNC supports GCode functions: ATAN, ABS, ACOS, ASIN, COS, EXP,
+ // FIX, FUP, ROUND, LN, SIN, TAN & EXISTS.
+ // See http://linuxcnc.org/docs/html/gcode/overview.html#gcode:functions
+ parser.error = STAT_GCODE_FUNC_UNSUPPORTED;
+ return 0;
+}
+
+
+static int _op_precedence(op_t op) {
+ switch (op) {
+ case OP_INVALID: break;
+ case OP_MINUS: return 6;
+ case OP_EXP: return 5;
+ case OP_MUL: case OP_DIV: case OP_MOD: return 4;
+ case OP_ADD: case OP_SUB: return 3;
+ case OP_EQ: case OP_NE: case OP_GT: case OP_GE: case OP_LT: case OP_LE:
+ return 2;
+ case OP_AND: case OP_OR: case OP_XOR: return 1;
+ }
+ return 0;
+}
+
+
+static op_t _parse_gcode_op(char **_p) {
+ char *p = *_p;
+ op_t op = OP_INVALID;
+
+ switch (toupper(p[0])) {
+ case '*': op = p[1] == '*' ? OP_EXP : OP_MUL; break;
+ case '/': op = OP_DIV; break;
+
+ case 'M':
+ if (toupper(p[1]) == 'O' && toupper(p[1]) == 'O') op = OP_EXP;
+ break;
+
+ case '+': op = OP_ADD; break;
+ case '-': op = OP_SUB; break;
+
+ case 'E': if (toupper(p[1]) == 'Q') op = OP_EQ; break;
+ case 'N': if (toupper(p[1]) == 'E') op = OP_NE; break;
+
+ case 'G':
+ if (toupper(p[1]) == 'T') op = OP_GT;
+ if (toupper(p[1]) == 'E') op = OP_GE;
+ break;
+
+ case 'L':
+ if (toupper(p[1]) == 'T') op = OP_LT;
+ if (toupper(p[1]) == 'E') op = OP_LE;
+ break;
+
+ case 'A':
+ if (toupper(p[1]) == 'N' && toupper(p[2]) == 'D') op = OP_AND;
+ break;
+
+ case 'O': if (toupper(p[1]) == 'R') op = OP_OR; break;
+
+ case 'X':
+ if (toupper(p[1]) == 'O' && toupper(p[2]) == 'R') op = OP_XOR;
+ break;
+ }
+
+ // Advance pointer
+ switch (op) {
+ case OP_INVALID: break;
+ case OP_MINUS: case OP_MUL: case OP_DIV: case OP_ADD:
+ case OP_SUB: *_p += 1; break;
+ case OP_EXP: case OP_EQ: case OP_NE: case OP_GT: case OP_GE: case OP_LT:
+ case OP_LE: case OP_OR: *_p += 2; break;
+ case OP_MOD: case OP_AND: case OP_XOR: *_p += 3; break;
+ }
+
+ return op;
+}
+
+
+static float _apply_binary(op_t op, float left, float right) {
+ switch (op) {
+ case OP_INVALID: case OP_MINUS: return 0;
+
+ case OP_EXP: return pow(left, right);
+
+ case OP_MUL: return left * right;
+ case OP_DIV: return left / right;
+ case OP_MOD: return fmod(left, right);
+
+ case OP_ADD: return left + right;
+ case OP_SUB: return left - right;
+
+ case OP_EQ: return left == right;
+ case OP_NE: return left != right;
+ case OP_GT: return left > right;
+ case OP_GE: return left >= right;
+ case OP_LT: return left > right;
+ case OP_LE: return left <= right;
+
+ case OP_AND: return left && right;
+ case OP_OR: return left || right;
+ case OP_XOR: return (bool)left ^ (bool)right;
+ }
+
+ return 0;
+}
+
+
+static void _val_push(float val) {
+ if (parser.valPtr < GCODE_MAX_VALUE_DEPTH) parser.vals[parser.valPtr++] = val;
+ else parser.error = STAT_EXPR_VALUE_STACK_OVERFLOW;
+}
+
+
+static float _val_pop() {
+ if (parser.valPtr) return parser.vals[--parser.valPtr];
+ parser.error = STAT_EXPR_VALUE_STACK_UNDERFLOW;
+ return 0;
+}
+
+
+static bool _op_empty() {return !parser.opPtr;}
+
+
+static void _op_push(op_t op) {
+ if (parser.opPtr < GCODE_MAX_OPERATOR_DEPTH) parser.ops[parser.opPtr++] = op;
+ else parser.error = STAT_EXPR_OP_STACK_OVERFLOW;
+}
+
+
+static op_t _op_pop() {
+ if (parser.opPtr) return parser.ops[--parser.opPtr];
+ parser.error = STAT_EXPR_OP_STACK_UNDERFLOW;
+ return OP_INVALID;
+}
+
+
+static op_t _op_top() {
+ if (parser.opPtr) return parser.ops[parser.opPtr - 1];
+ parser.error = STAT_EXPR_OP_STACK_UNDERFLOW;
+ return OP_INVALID;
+}
+
+
+static void _op_apply() {
+ op_t op = _op_pop();
+
+ if (op == OP_MINUS) _val_push(-_val_pop());
+
+ else {
+ float right = _val_pop();
+ float left = _val_pop();
+
+ _val_push(_apply_binary(op, left, right));
+ }
+}
+
+
+// Parse expressions with Dijkstra's Shunting Yard Algorithm
+float parse_gcode_expression(char **p) {
+ bool unary = true; // Used to detect unary minus
+
+ while (!parser.error && **p) {
+ switch (**p) {
+ case ' ': case '\n': case '\r': case '\t': (*p)++; break;
+ case '#': _val_push(_parse_gcode_var(p)); unary = false; break;
+ case '[': _op_push(OP_INVALID); (*p)++; unary = true; break;
+
+ case ']':
+ (*p)++;
+
+ while (!parser.error && _op_top() != OP_INVALID)
+ _op_apply();
+
+ _op_pop(); // Pop opening bracket
+ if (_op_empty() && parser.valPtr == 1) return _val_pop();
+ unary = false;
+ break;
+
+ default:
+ if (isdigit(**p) || **p == '.') {
+ _val_push(parse_gcode_number(p));
+ unary = false;
+
+ } else if (isalpha(**p)) {
+ _val_push(_parse_gcode_func(p));
+ unary = false;
+
+ } else {
+ op_t op = _parse_gcode_op(p);
+
+ if (unary && op == OP_ADD) continue; // Ignore it
+ if (unary && op == OP_SUB) {_op_push(OP_MINUS); continue;}
+
+ if (op == OP_INVALID) parser.error = STAT_INVALID_OR_MALFORMED_COMMAND;
+ else {
+ int precedence = _op_precedence(op);
+
+ while (!parser.error && !_op_empty() &&
+ precedence <= _op_precedence(_op_top()))
+ _op_apply();
+
+ _op_push(op);
+ unary = true;
+ }
+ }
+ }
+ }
+
+ return _val_pop();
+}
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2017 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
+
+
+float parse_gcode_number(char **p);
+float parse_gcode_expression(char **p);
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2017 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 "gcode_expr.h"
+#include "machine.h"
+#include "plan/arc.h"
+#include "axis.h"
+#include "util.h"
+
+#include <stdbool.h>
+#include <string.h>
+#include <ctype.h>
+#include <stdlib.h>
+#include <math.h>
+#include <stdio.h>
+
+
+parser_t parser = {{0}};
+
+
+#define SET_MODAL(m, parm, val) \
+ {parser.gn.parm = val; parser.gf.parm = true; parser.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)
+
+
+// NOTE Nested comments are not allowed. E.g. (msg (hello))
+static char *_parse_gcode_comment(char *p) {
+ char *msg = 0;
+
+ p++; // Skip leading paren
+
+ while (isspace(*p)) p++; // skip whitespace
+
+ // Look for "(MSG"
+ if (tolower(*(p + 0)) == 'm' &&
+ tolower(*(p + 1)) == 's' &&
+ tolower(*(p + 2)) == 'g') {
+ p += 3;
+ while (isspace(*p)) p++; // skip whitespace
+ if (*p && *p != ')') msg = p;
+ }
+
+ // Find end
+ while (*p && *p != ')') p++;
+ *p = 0; // Terminate string
+
+ // Queue message
+ if (msg) mach_message(msg);
+
+ return p;
+}
+
+
+static stat_t _parse_gcode_value(char **p, float *value) {
+ while (isspace(**p)) (*p)++; // skip whitespace
+
+ if (**p == '[') *value = parse_gcode_expression(p);
+ else *value = parse_gcode_number(p);
+
+ return parser.error;
+}
+
+
+/// 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 (parser.modals[MODAL_GROUP_G0] && parser.modals[MODAL_GROUP_G1])
+ return STAT_MODAL_GROUP_VIOLATION;
+
+#if 0 // TODO 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 (parser.modals[MODAL_GROUP_G0] || parser.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) // TODO update this
+ * 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_CLEAR_HOME: // G28.2
+ mach_clear_home(parser.gf.target);
+ break;
+ case NEXT_ACTION_SET_HOME: // G28.3
+ mach_set_home(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,
+ parser.modals[MODAL_GROUP_G1],
+ parser.gn.motion_mode);
+ break;
+ case MOTION_MODE_STRAIGHT_PROBE_CLOSE_ERR: // G38.2
+ status = mach_probe(parser.gn.target, parser.gf.target,
+ parser.gn.motion_mode);
+ break;
+ case MOTION_MODE_STRAIGHT_PROBE_CLOSE: // G38.3
+ status = mach_probe(parser.gn.target, parser.gf.target,
+ parser.gn.motion_mode);
+ break;
+ case MOTION_MODE_STRAIGHT_PROBE_OPEN_ERR: // G38.4
+ status = mach_probe(parser.gn.target, parser.gf.target,
+ parser.gn.motion_mode);
+ break;
+ case MOTION_MODE_STRAIGHT_PROBE_OPEN: // G38.5
+ status = mach_probe(parser.gn.target, parser.gf.target,
+ parser.gn.motion_mode);
+ break;
+ case MOTION_MODE_SEEK_CLOSE_ERR: // G38.6
+ status = mach_seek(parser.gn.target, parser.gf.target,
+ parser.gn.motion_mode);
+ break;
+ case MOTION_MODE_SEEK_CLOSE: // G38.7
+ status = mach_seek(parser.gn.target, parser.gf.target,
+ parser.gn.motion_mode);
+ break;
+ case MOTION_MODE_SEEK_OPEN_ERR: // G38.8
+ status = mach_seek(parser.gn.target, parser.gf.target,
+ parser.gn.motion_mode);
+ break;
+ case MOTION_MODE_SEEK_OPEN: // G38.9
+ status = mach_seek(parser.gn.target, parser.gf.target,
+ 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;
+}
+
+
+/// Load the state values in gn (next model state) and set flags in gf (model
+/// state flags).
+static stat_t _process_gcode_word(char letter, float value) {
+ 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_CLEAR_HOME);
+ case 3: SET_NON_MODAL(next_action, NEXT_ACTION_SET_HOME);
+ default: return 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: return STAT_GCODE_COMMAND_UNSUPPORTED;
+ }
+ break;
+
+ case 38:
+ switch (_point(value)) {
+ case 2: SET_MODAL(MODAL_GROUP_G1, motion_mode,
+ MOTION_MODE_STRAIGHT_PROBE_CLOSE_ERR);
+ case 3: SET_MODAL(MODAL_GROUP_G1, motion_mode,
+ MOTION_MODE_STRAIGHT_PROBE_CLOSE);
+ case 4: SET_MODAL(MODAL_GROUP_G1, motion_mode,
+ MOTION_MODE_STRAIGHT_PROBE_OPEN_ERR);
+ case 5: SET_MODAL(MODAL_GROUP_G1, motion_mode,
+ MOTION_MODE_STRAIGHT_PROBE_OPEN);
+ case 6: SET_MODAL(MODAL_GROUP_G1, motion_mode,
+ MOTION_MODE_SEEK_CLOSE_ERR);
+ case 7: SET_MODAL(MODAL_GROUP_G1, motion_mode, MOTION_MODE_SEEK_CLOSE);
+ case 8: SET_MODAL(MODAL_GROUP_G1, motion_mode,
+ MOTION_MODE_SEEK_OPEN_ERR);
+ case 9: SET_MODAL(MODAL_GROUP_G1, motion_mode, MOTION_MODE_SEEK_OPEN);
+ default: return 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: return 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:
+ 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: return 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: return 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: return 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: return 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: return 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: return STAT_GCODE_COMMAND_UNSUPPORTED;
+ }
+
+ return STAT_OK;
+}
+
+
+/// 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) {
+#ifdef DEBUG
+ printf("GCODE: %s\n", block);
+#endif
+
+ // Delete block if it starts with /
+ if (*block == '/') return STAT_NOOP;
+
+ // Set initial state for new block
+ // 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
+ memset(&parser, 0, sizeof(parser)); // clear all parser values
+
+ // get motion mode from previous block
+ parser.gn.motion_mode = mach_get_motion_mode();
+
+ // Parse words
+ for (char *p = block; *p;) {
+ switch (*p) {
+ case ' ': case '\t': case '\r': case '\n': p++; break; // Skip whitespace
+ case '(': p = _parse_gcode_comment(p); break;
+ case ';': *p = 0; break; // Comment
+
+ default: {
+ char letter = toupper(*p++);
+ float value = 0;
+ if (!isalpha(letter)) return STAT_INVALID_OR_MALFORMED_COMMAND;
+ RITORNO(_parse_gcode_value(&p, &value));
+ RITORNO(_process_gcode_word(letter, value));
+ }
+ }
+ }
+
+ RITORNO(_validate_gcode_block());
+
+ return _execute_gcode_block();
+}
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2017 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 enum {
+ OP_INVALID,
+ OP_MINUS,
+ OP_EXP,
+ OP_MUL, OP_DIV, OP_MOD,
+ OP_ADD, OP_SUB,
+ OP_EQ, OP_NE, OP_GT,OP_GE, OP_LT, OP_LE,
+ OP_AND, OP_OR, OP_XOR,
+} op_t;
+
+
+typedef struct {
+ gcode_state_t gn; // gcode input values
+ gcode_flags_t gf; // gcode input flags
+
+ uint8_t modals[MODAL_GROUP_COUNT]; // collects modal groups in a block
+
+ op_t ops[GCODE_MAX_OPERATOR_DEPTH];
+ float vals[GCODE_MAX_VALUE_DEPTH];
+ int opPtr;
+ int valPtr;
+
+ stat_t error;
+} 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 - 2017 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"
+
+
+static const char INVALID_PGMSTR[] PROGMEM = "INVALID";
+
+static const char INCHES_PGMSTR[] PROGMEM = "IN";
+static const char MILLIMETERS_PGMSTR[] PROGMEM = "MM";
+static const char DEGREES_PGMSTR[] PROGMEM = "DEG";
+
+static const char INVERSE_TIME_MODE_PGMSTR[] PROGMEM = "INVERSE TIME";
+static const char UNITS_PER_MINUTE_MODE_PGMSTR[] PROGMEM = "PER MIN";
+static const char UNITS_PER_REVOLUTION_MODE_PGMSTR[] PROGMEM = "PER REV";
+
+static const char PLANE_XY_PGMSTR[] PROGMEM = "XY";
+static const char PLANE_XZ_PGMSTR[] PROGMEM = "XZ";
+static const char PLANE_YZ_PGMSTR[] PROGMEM = "YZ";
+
+static const char ABSOLUTE_COORDS_PGMSTR[] PROGMEM = "ABS";
+static const char G54_PGMSTR[] PROGMEM = "G54";
+static const char G55_PGMSTR[] PROGMEM = "G55";
+static const char G56_PGMSTR[] PROGMEM = "G56";
+static const char G57_PGMSTR[] PROGMEM = "G57";
+static const char G58_PGMSTR[] PROGMEM = "G58";
+static const char G59_PGMSTR[] PROGMEM = "G59";
+
+static const char PATH_EXACT_PATH_PGMSTR[] PROGMEM = "EXACT PATH";
+static const char PATH_EXACT_STOP_PGMSTR[] PROGMEM = "EXACT STOP";
+static const char PATH_CONTINUOUS_PGMSTR[] PROGMEM = "CONTINUOUS";
+
+static const char ABSOLUTE_MODE_PGMSTR[] PROGMEM = "ABSOLUTE";
+static const char INCREMENTAL_MODE_PGMSTR[] PROGMEM = "INCREMENTAL";
+
+
+PGM_P gs_get_units_pgmstr(units_t mode) {
+ switch (mode) {
+ case INCHES: return INCHES_PGMSTR;
+ case MILLIMETERS: return MILLIMETERS_PGMSTR;
+ case DEGREES: return DEGREES_PGMSTR;
+ }
+
+ return INVALID_PGMSTR;
+}
+
+
+units_t gs_parse_units(const char *s) {
+ if (!strcmp_P(s, INCHES_PGMSTR)) return INCHES;
+ if (!strcmp_P(s, MILLIMETERS_PGMSTR)) return MILLIMETERS;
+ if (!strcmp_P(s, DEGREES_PGMSTR)) return DEGREES;
+ return -1;
+}
+
+
+PGM_P gs_get_feed_mode_pgmstr(feed_mode_t mode) {
+ switch (mode) {
+ case INVERSE_TIME_MODE: return INVERSE_TIME_MODE_PGMSTR;
+ case UNITS_PER_MINUTE_MODE: return UNITS_PER_MINUTE_MODE_PGMSTR;
+ case UNITS_PER_REVOLUTION_MODE: return UNITS_PER_REVOLUTION_MODE_PGMSTR;
+ }
+
+ return INVALID_PGMSTR;
+}
+
+
+feed_mode_t gs_parse_feed_mode(const char *s) {
+ if (!strcmp_P(s, INVERSE_TIME_MODE_PGMSTR)) return INVERSE_TIME_MODE;
+ if (!strcmp_P(s, UNITS_PER_MINUTE_MODE_PGMSTR)) return UNITS_PER_MINUTE_MODE;
+ if (!strcmp_P(s, UNITS_PER_REVOLUTION_MODE_PGMSTR))
+ return UNITS_PER_REVOLUTION_MODE;
+ return -1;
+}
+
+
+PGM_P gs_get_plane_pgmstr(plane_t plane) {
+ switch (plane) {
+ case PLANE_XY: return PLANE_XY_PGMSTR;
+ case PLANE_XZ: return PLANE_XZ_PGMSTR;
+ case PLANE_YZ: return PLANE_YZ_PGMSTR;
+ }
+
+ return INVALID_PGMSTR;
+}
+
+
+plane_t gs_parse_plane(const char *s) {
+ if (!strcmp_P(s, PLANE_XY_PGMSTR)) return PLANE_XY;
+ if (!strcmp_P(s, PLANE_XZ_PGMSTR)) return PLANE_XZ;
+ if (!strcmp_P(s, PLANE_YZ_PGMSTR)) return PLANE_YZ;
+ return -1;
+}
+
+
+PGM_P gs_get_coord_system_pgmstr(coord_system_t cs) {
+ switch (cs) {
+ case ABSOLUTE_COORDS: return ABSOLUTE_COORDS_PGMSTR;
+ case G54: return G54_PGMSTR;
+ case G55: return G55_PGMSTR;
+ case G56: return G56_PGMSTR;
+ case G57: return G57_PGMSTR;
+ case G58: return G58_PGMSTR;
+ case G59: return G59_PGMSTR;
+ }
+
+ return INVALID_PGMSTR;
+}
+
+
+coord_system_t gs_parse_coord_system(const char *s) {
+ if (!strcmp_P(s, ABSOLUTE_COORDS_PGMSTR)) return ABSOLUTE_COORDS;
+ if (!strcmp_P(s, G54_PGMSTR)) return G54;
+ if (!strcmp_P(s, G55_PGMSTR)) return G55;
+ if (!strcmp_P(s, G56_PGMSTR)) return G56;
+ if (!strcmp_P(s, G57_PGMSTR)) return G57;
+ if (!strcmp_P(s, G58_PGMSTR)) return G58;
+ if (!strcmp_P(s, G59_PGMSTR)) return G59;
+ return -1;
+}
+
+
+PGM_P gs_get_path_mode_pgmstr(path_mode_t mode) {
+ switch (mode) {
+ case PATH_EXACT_PATH: return PATH_EXACT_PATH_PGMSTR;
+ case PATH_EXACT_STOP: return PATH_EXACT_STOP_PGMSTR;
+ case PATH_CONTINUOUS: return PATH_CONTINUOUS_PGMSTR;
+ }
+
+ return INVALID_PGMSTR;
+}
+
+
+path_mode_t gs_parse_path_mode(const char *s) {
+ if (!strcmp_P(s, PATH_EXACT_PATH_PGMSTR)) return PATH_EXACT_PATH;
+ if (!strcmp_P(s, PATH_EXACT_STOP_PGMSTR)) return PATH_EXACT_STOP;
+ if (!strcmp_P(s, PATH_CONTINUOUS_PGMSTR)) return PATH_CONTINUOUS;
+ return -1;
+}
+
+
+PGM_P gs_get_distance_mode_pgmstr(distance_mode_t mode) {
+ switch (mode) {
+ case ABSOLUTE_MODE: return ABSOLUTE_MODE_PGMSTR;
+ case INCREMENTAL_MODE: return INCREMENTAL_MODE_PGMSTR;
+ }
+
+ return INVALID_PGMSTR;
+}
+
+
+distance_mode_t gs_parse_distance_mode(const char *s) {
+ if (!strcmp_P(s, ABSOLUTE_MODE_PGMSTR)) return ABSOLUTE_MODE;
+ if (!strcmp_P(s, INCREMENTAL_MODE_PGMSTR)) return INCREMENTAL_MODE;
+ return -1;
+}
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2017 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 - 2017 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 "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
+ */
+
+typedef enum {
+ NEXT_ACTION_DEFAULT, // Must be zero (invokes motion modes)
+ NEXT_ACTION_DWELL, // G4
+ NEXT_ACTION_SET_COORD_DATA, // G10
+ NEXT_ACTION_GOTO_G28_POSITION, // G28 go to machine position
+ NEXT_ACTION_SET_G28_POSITION, // G28.1 set position in abs coordinates
+ NEXT_ACTION_CLEAR_HOME, // G28.3 clear axis home
+ NEXT_ACTION_SET_HOME, // G28.3 set axis home position
+ NEXT_ACTION_GOTO_G30_POSITION, // G30
+ NEXT_ACTION_SET_G30_POSITION, // G30.1
+ 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_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_STRAIGHT_PROBE_CLOSE_ERR, // G38.2
+ MOTION_MODE_STRAIGHT_PROBE_CLOSE, // G38.3
+ MOTION_MODE_STRAIGHT_PROBE_OPEN_ERR, // G38.4
+ MOTION_MODE_STRAIGHT_PROBE_OPEN, // G38.5
+ MOTION_MODE_SEEK_CLOSE_ERR, // G38.6
+ MOTION_MODE_SEEK_CLOSE, // G38.7
+ MOTION_MODE_SEEK_OPEN_ERR, // G38.8
+ MOTION_MODE_SEEK_OPEN, // G38.9
+ MOTION_MODE_CANCEL_MOTION_MODE, // G80
+ 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, man 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 {
+ UNITS_PER_MINUTE_MODE, // G94
+ INVERSE_TIME_MODE, // G93
+ 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;
+
+
+/* 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);
+units_t gs_parse_units(const char *units);
+PGM_P gs_get_feed_mode_pgmstr(feed_mode_t mode);
+feed_mode_t gs_parse_feed_mode(const char *mode);
+PGM_P gs_get_plane_pgmstr(plane_t plane);
+plane_t gs_parse_plane(const char *plane);
+PGM_P gs_get_coord_system_pgmstr(coord_system_t cs);
+coord_system_t gs_parse_coord_system(const char *cs);
+PGM_P gs_get_path_mode_pgmstr(path_mode_t mode);
+path_mode_t gs_parse_path_mode(const char *mode);
+PGM_P gs_get_distance_mode_pgmstr(distance_mode_t mode);
+distance_mode_t gs_parse_distance_mode(const char *mode);
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2017 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 "pgmspace.h"
+
+#include <avr/interrupt.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]
+
+
+static void _init_clock() {
+#if 0 // 32Mhz Int RC
+ OSC.CTRL |= OSC_RC32MEN_bm | OSC_RC32KEN_bm; // Enable 32MHz & 32KHz osc
+ while (!(OSC.STATUS & OSC_RC32KRDY_bm)); // Wait for 32Khz oscillator
+ while (!(OSC.STATUS & OSC_RC32MRDY_bm)); // Wait for 32MHz oscillator
+
+ // Defaults to calibrate against internal 32Khz clock
+ DFLLRC32M.CTRL = DFLL_ENABLE_bm; // Enable DFLL
+ CCP = CCP_IOREG_gc; // Disable register security
+ CLK.CTRL = CLK_SCLKSEL_RC32M_gc; // Switch to 32MHz clock
+
+#else
+ // 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
+#endif
+
+ OSC.CTRL &= ~OSC_RC2MEN_bm; // disable internal 2 MHz clock
+}
+
+
+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 (!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 - 2017 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 - 2017 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 "pgmspace.h"
+
+#include <avr/io.h>
+#include <avr/interrupt.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_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;
+ spindle_mode_t mode;
+ float speed;
+
+ float actual_freq;
+ float actual_current;
+ uint16_t actual_rpm;
+ 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) {SET_PIN(RS485_RW_PIN, x);}
+
+
+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.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_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) {
+ 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_stop() {
+ huanyang_set(SPINDLE_OFF, 0);
+ huanyang_reset();
+}
+
+
+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_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 - 2017 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_stop();
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2017 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 - 2017 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_REBOOT,
+} 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 - 2017 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 "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 = {
+ .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;}
+bool mach_in_absolute_mode() {return mach.gm.distance_mode == ABSOLUTE_MODE;}
+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 unit conversion
+ * is performed.
+ */
+float mach_get_axis_position(uint8_t axis) {return mach.position[axis];}
+
+
+/* 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 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
+ * 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;
+
+ // TODO should set work position, accounting for offsets
+
+ mach.position[axis] = position;
+ mp_set_axis_position(axis, position);
+ mp_runtime_set_axis_position(axis, position);
+ mp_runtime_set_steps_from_position();
+}
+
+
+/// Do not call this function while machine is moving or queue is not empty
+void mach_set_position_from_runtime() {
+ for (int axis = 0; axis < AXES; axis++) {
+ mach.position[axis] = mp_runtime_get_work_position(axis);
+ mp_set_axis_position(axis, mach.position[axis]);
+ }
+}
+
+
+/*** 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[], bool absolute) {
+ for (int axis = 0; axis < AXES; axis++) {
+ target[axis] = mach.position[axis];
+ if (!flags[axis] || !axis_is_enabled(axis)) continue;
+
+ target[axis] = absolute ? 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
+ // TODO This should depend on the axis mode
+ else if (AXIS_Z < axis) target[axis] += values[axis];
+ else target[axis] += TO_MM(values[axis]);
+ }
+}
+
+
+/*** Return error code if soft limit is exceeded
+ *
+ * Tests for soft limit for any axis if min and max are different values. You
+ * can set min and max to 0 to disable soft limits for an axis.
+ */
+stat_t mach_test_soft_limits(float target[]) {
+ for (int axis = 0; axis < AXES; axis++) {
+ if (!axis_is_enabled(axis) || !axis_get_homed(axis)) continue;
+
+ 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 (target[axis] < min || max < target[axis])
+ 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) {
+ if (plane != (plane_t)-1) mach.gm.plane = plane;
+}
+
+
+/// G20, G21
+void mach_set_units(units_t mode) {
+ if (mode != (units_t)-1) mach.gm.units = mode;
+}
+
+
+/// G90, G91
+void mach_set_distance_mode(distance_mode_t mode) {
+ if (mode != (distance_mode_t)-1) mach.gm.distance_mode = mode;
+}
+
+
+/// G90.1, G91.1
+void mach_set_arc_distance_mode(distance_mode_t mode) {
+ if (mode != (distance_mode_t)-1) 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 cs) {
+ if (cs != (coord_system_t)-1) mach.gm.coord_system = cs;
+}
+
+
+// G28.3 functions and support
+static stat_t _exec_home(mp_buffer_t *bf) {
+ const float *target = bf->target;
+ const float *flags = bf->unit;
+
+ for (int axis = 0; axis < AXES; axis++)
+ if (flags[axis]) {
+ mp_runtime_set_axis_position(axis, target[axis]);
+ axis_set_homed(axis, true);
+ }
+
+ 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.
+ */
+void mach_set_home(float origin[], bool flags[]) {
+ mp_buffer_t *bf = mp_queue_get_tail();
+
+ // Compute target position
+ mach_calc_target(bf->target, origin, flags, true);
+
+ for (int axis = 0; axis < AXES; axis++)
+ if (flags[axis] && isfinite(origin[axis])) {
+ bf->target[axis] -= mach_get_active_coord_offset(axis);
+ mach.position[axis] = bf->target[axis];
+ mp_set_axis_position(axis, bf->target[axis]); // set mm position
+ bf->unit[axis] = true;
+
+ } else bf->unit[axis] = false;
+
+ // Synchronized update of runtime position
+ mp_queue_push_nonstop(_exec_home, mach_get_line());
+}
+
+
+void mach_clear_home(bool flags[]) {
+ for (int axis = 0; axis < AXES; axis++)
+ if (flags[axis]) axis_set_homed(axis, false);
+}
+
+
+/* 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[]) {
+ 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]);
+
+ mach_update_work_offsets(); // update resolved offsets
+}
+
+
+/// G92.1
+void mach_reset_origin_offsets() {
+ mach.origin_offset_enable = false;
+
+ for (int axis = 0; axis < AXES; axis++)
+ mach.origin_offset[axis] = 0;
+
+ mach_update_work_offsets(); // update resolved offsets
+}
+
+
+/// G92.2
+void mach_suspend_origin_offsets() {
+ mach.origin_offset_enable = false;
+ mach_update_work_offsets(); // update resolved offsets
+}
+
+
+/// G92.3
+void mach_resume_origin_offsets() {
+ mach.origin_offset_enable = true;
+ mach_update_work_offsets(); // update resolved offsets
+}
+
+
+stat_t mach_plan_line(float target[], switch_id_t sw) {
+ buffer_flags_t flags = 0;
+
+ switch (mach_get_motion_mode()) {
+ case MOTION_MODE_STRAIGHT_PROBE_CLOSE_ERR:
+ case MOTION_MODE_SEEK_CLOSE_ERR:
+ flags |= BUFFER_SEEK_CLOSE | BUFFER_SEEK_ERROR | BUFFER_EXACT_STOP;
+ break;
+
+ case MOTION_MODE_STRAIGHT_PROBE_CLOSE:
+ case MOTION_MODE_SEEK_CLOSE:
+ flags |= BUFFER_SEEK_CLOSE | BUFFER_EXACT_STOP;
+ break;
+
+ case MOTION_MODE_STRAIGHT_PROBE_OPEN_ERR:
+ case MOTION_MODE_SEEK_OPEN_ERR:
+ flags |= BUFFER_SEEK_OPEN | BUFFER_SEEK_ERROR | BUFFER_EXACT_STOP;
+ break;
+
+ case MOTION_MODE_STRAIGHT_PROBE_OPEN:
+ case MOTION_MODE_SEEK_OPEN:
+ flags |= BUFFER_SEEK_OPEN | BUFFER_EXACT_STOP;
+ break;
+
+ default: break;
+ }
+
+ if (mach_is_rapid()) flags |= BUFFER_RAPID;
+ if (mach_is_inverse_time_mode()) flags |= BUFFER_INVERSE_TIME;
+ if (mach_is_exact_stop()) flags |= BUFFER_EXACT_STOP;
+
+ return mp_aline(target, flags, sw, 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[], switch_id_t sw) {
+ // Trap inverse time mode wo/ feed rate
+ if (!mach_is_rapid() && mach.gm.feed_mode == INVERSE_TIME_MODE &&
+ !parser.gf.feed_rate)
+ return STAT_GCODE_FEEDRATE_NOT_SPECIFIED;
+
+ // Compute target position
+ float target[AXES];
+ mach_calc_target(target, values, flags, mach_in_absolute_mode());
+
+ // 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
+ RITORNO(mach_plan_line(target, sw));
+ copy_vector(mach.position, target); // update model position
+
+ return STAT_OK;
+}
+
+
+/// G0 linear rapid
+stat_t mach_rapid(float values[], bool flags[]) {
+ mach_set_motion_mode(MOTION_MODE_RAPID);
+ return _feed(values, flags, 0);
+}
+
+
+/// 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);
+}
+
+
+stat_t mach_probe(float values[], bool flags[], motion_mode_t mode) {
+ mach_set_motion_mode(mode);
+ return _feed(values, flags, SW_PROBE);
+}
+
+
+stat_t _exec_set_seek_position(mp_buffer_t *bf) {
+ mach_set_position_from_runtime();
+ mp_pause_queue(false);
+ return STAT_NOOP; // No move queued
+}
+
+
+stat_t mach_seek(float target[], bool flags[], motion_mode_t mode) {
+ mach_set_motion_mode(mode);
+
+ switch_id_t sw = SW_PROBE;
+
+ for (int axis = 0; axis < AXES; axis++)
+ if (flags[axis] && isfinite(target[axis])) {
+ // Convert to incremental move
+ if (mach.gm.distance_mode == ABSOLUTE_MODE)
+ target[axis] += mach.position[axis];
+
+ if (!axis_is_enabled(axis)) return STAT_SEEK_AXIS_DISABLED;
+ if (sw != SW_PROBE) return STAT_SEEK_MULTIPLE_AXES;
+ if (fp_EQ(target[axis], mach.position[axis])) return STAT_SEEK_ZERO_MOVE;
+
+ bool min = target[axis] < mach.position[axis];
+
+ if (mode == MOTION_MODE_SEEK_OPEN_ERR ||
+ mode == MOTION_MODE_SEEK_OPEN) min = !min;
+
+ switch (axis) {
+ case AXIS_X: sw = min ? SW_MIN_X : SW_MAX_X; break;
+ case AXIS_Y: sw = min ? SW_MIN_Y : SW_MAX_Y; break;
+ case AXIS_Z: sw = min ? SW_MIN_Z : SW_MAX_Z; break;
+ case AXIS_A: sw = min ? SW_MIN_A : SW_MAX_A; break;
+ }
+
+ if (!switch_is_enabled(sw)) return STAT_SEEK_SWITCH_DISABLED;
+ }
+
+ if (sw == SW_PROBE) return STAT_SEEK_MISSING_AXIS;
+
+ RITORNO(_feed(target, flags, sw));
+
+ mp_pause_queue(true);
+ mp_queue_push_nonstop(_exec_set_seek_position, mach_get_line());
+
+ return STAT_OK;
+}
+
+
+// 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 (mode == (feed_mode_t)-1 || 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) {
+ if (mode != (path_mode_t)-1) mach.gm.path_mode = mode;
+}
+
+
+// Machining Functions (4.3.6) see also arc.c
+
+/// G1
+stat_t mach_feed(float values[], bool flags[]) {
+ mach_set_motion_mode(MOTION_MODE_FEED);
+ return _feed(values, flags, 0);
+}
+
+
+/// G4, P parameter (seconds)
+stat_t mach_dwell(float seconds) {return mp_dwell(seconds, mach_get_line());}
+
+
+// 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
+ */
+
+void mach_set_feed_override(float value) {
+ mach.gm.feed_override = value;
+ mach.gm.feed_override_enable = !fp_ZERO(value);
+}
+
+
+void mach_set_spindle_override(float value) {
+ mach.gm.spindle_override = value;
+ mach.gm.spindle_override_enable = !fp_ZERO(value);
+}
+
+
+/// 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 - 2017 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"
+#include "switch.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();
+bool mach_in_absolute_mode();
+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);
+void mach_set_axis_position(unsigned axis, float position);
+void mach_set_position_from_runtime();
+
+// Critical helpers
+void mach_calc_target(float target[], const float values[], const bool flags[],
+ bool absolute);
+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_coord_system(coord_system_t coord_system);
+
+void mach_set_home(float origin[], bool flags[]);
+void mach_clear_home(bool flags[]);
+
+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[], switch_id_t sw);
+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[]);
+stat_t mach_probe(float target[], bool flags[], motion_mode_t mode);
+stat_t mach_seek(float target[], bool flags[], motion_mode_t mode);
+
+// 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_set_feed_override(float override);
+void mach_set_spindle_override(float override);
+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 - 2017 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 "i2c.h"
+#include "pgmspace.h"
+#include "outputs.h"
+
+#include "plan/planner.h"
+#include "plan/arc.h"
+#include "plan/state.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
+ outputs_init(); // output pins
+ 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"));
+
+ // Main loop
+ while (true) {
+ hw_reset_handler(); // handle hard reset requests
+ if (!estop_triggered()) {
+ mp_state_callback();
+ mach_arc_callback(); // arc generation runs
+ }
+ 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 - 2017 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")
+STAT_MSG(EXPR_VALUE_STACK_OVERFLOW, "Expression parser value overflow")
+STAT_MSG(EXPR_VALUE_STACK_UNDERFLOW, "Expression parser value underflow")
+STAT_MSG(EXPR_OP_STACK_OVERFLOW, "Expression parser operator overflow")
+STAT_MSG(EXPR_OP_STACK_UNDERFLOW, "Expression parser operator underflow")
+STAT_MSG(GCODE_FUNC_UNSUPPORTED, "GCode function call unsupported")
+STAT_MSG(GCODE_NUM_PARAM_UNSUPPORTED, "GCode numbered parameters unsupported")
+STAT_MSG(GCODE_UNTERMINATED_VAR, "GCode variable reference untermiated")
+
+// 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")
+
+// Seeking
+STAT_MSG(SEEK_MULTIPLE_AXES, "Seek error - multiple axes specified")
+STAT_MSG(SEEK_MISSING_AXIS, "Seek error - no axis specified")
+STAT_MSG(SEEK_AXIS_DISABLED, "Seek error - specified axis is disabled")
+STAT_MSG(SEEK_SWITCH_DISABLED, "Seek error - target switch is disabled")
+STAT_MSG(SEEK_ZERO_MOVE, "Seek error - axis move too short or zero")
+STAT_MSG(SEEK_SWTICH_NOT_FOUND, "Seek error - move end before switch change")
+
+// End of stats marker
+STAT_MSG(MAX, "")
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2017 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 "pgmspace.h"
+
+#include "plan/runtime.h"
+
+#include <util/delay.h>
+
+#include <string.h>
+#include <math.h>
+#include <stdio.h>
+#include <stdlib.h>
+
+
+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;
+
+ // Computed
+ float steps_per_unit;
+
+ // Runtime state
+ uint32_t power_timeout;
+ int32_t commanded;
+ int32_t encoder;
+ int16_t error;
+ bool last_negative;
+ uint8_t last_clock;
+
+ // Move prep
+ bool prepped;
+ uint8_t timer_clock;
+ uint16_t timer_period;
+ bool negative;
+ int32_t position;
+} motor_t;
+
+
+static motor_t motors[MOTORS] = {
+ {
+ .axis = AXIS_X,
+ .step_pin = STEP_X_PIN,
+ .dir_pin = DIR_X_PIN,
+ .timer = &M1_TIMER,
+ .dma = &M1_DMA_CH,
+ .dma_trigger = M1_DMA_TRIGGER,
+ }, {
+ .axis = AXIS_Y,
+ .step_pin = STEP_Y_PIN,
+ .dir_pin = DIR_Y_PIN,
+ .timer = &M2_TIMER,
+ .dma = &M2_DMA_CH,
+ .dma_trigger = M2_DMA_TRIGGER,
+ }, {
+ .axis = AXIS_Z,
+ .step_pin = STEP_Z_PIN,
+ .dir_pin = DIR_Z_PIN,
+ .timer = &M3_TIMER,
+ .dma = &M3_DMA_CH,
+ .dma_trigger = M3_DMA_TRIGGER,
+ }, {
+ .axis = AXIS_A,
+ .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;
+
+
+static void _update_config(int motor) {
+ motor_t *m = &motors[motor];
+
+ m->steps_per_unit = 360.0 * m->microsteps / m->travel_rev / m->step_angle;
+}
+
+
+void motor_init() {
+ // Enable DMA
+ DMA.CTRL = DMA_RESET_bm;
+ DMA.CTRL = DMA_ENABLE_bm;
+ DMA.INTFLAGS = 0xff; // clear all pending interrupts
+
+ for (int motor = 0; motor < MOTORS; motor++) {
+ motor_t *m = &motors[motor];
+
+ _update_config(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;
+
+ // 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->TRFCNT = 0xffff;
+ m->dma->REPCNT = 0;
+ m->dma->CTRLB = 0;
+ m->dma->CTRLA = DMA_CH_SINGLE_bm | DMA_CH_BURSTLEN_1BYTE_gc;
+
+ drv8711_set_microsteps(motor, m->microsteps);
+ }
+}
+
+
+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_axis(int motor, uint8_t axis) {
+ if (MOTORS <= motor || AXES <= axis || axis == motors[motor].axis) return;
+ axis_set_motor(motors[motor].axis, -1);
+ motors[motor].axis = axis;
+ axis_set_motor(axis, motor);
+}
+
+
+float motor_get_steps_per_unit(int motor) {return motors[motor].steps_per_unit;}
+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;
+ _update_config(motor);
+ drv8711_set_microsteps(motor, microsteps);
+}
+
+
+void motor_set_position(int motor, int32_t position) {
+ //if (st_is_busy()) ALARM(STAT_INTERNAL_ERROR); TODO
+
+ motor_t *m = &motors[motor];
+
+ m->commanded = m->encoder = m->position = position << 1; // We use half steps
+ m->error = 0;
+}
+
+
+int32_t motor_get_position(int motor) {
+ return motors[motor].position >> 1; // Convert from half to full steps
+}
+
+
+static void _update_power(int motor) {
+ motor_t *m = &motors[motor];
+
+ switch (m->power_mode) {
+ case MOTOR_POWERED_ONLY_WHEN_MOVING:
+ case MOTOR_POWERED_IN_CYCLE:
+ if (rtc_expired(m->power_timeout)) {
+ drv8711_set_state(motor, DRV8711_IDLE);
+ break;
+ }
+ // Fall through
+
+ case MOTOR_ALWAYS_POWERED:
+ // TODO is ~5ms enough time to enable the motor?
+ drv8711_set_state(motor, DRV8711_ACTIVE);
+ break;
+
+ default: // Disabled
+ drv8711_set_state(motor, DRV8711_DISABLED);
+ }
+}
+
+
+/// 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++)
+ _update_power(motor);
+
+ return STAT_OK;
+}
+
+
+void motor_end_move(int motor) {
+ motor_t *m = &motors[motor];
+
+ if (!m->timer->CTRLA) return;
+
+ // Stop clock
+ m->timer->CTRLA = 0;
+
+ // Get actual step count from DMA channel
+ const int24_t half_steps = 0xffff - m->dma->TRFCNT;
+
+ // Accumulate encoder
+ m->encoder += m->last_negative ? -half_steps : half_steps;
+
+ // Compute error
+ m->error = m->commanded - m->encoder;
+}
+
+
+void motor_load_move(int motor) {
+ motor_t *m = &motors[motor];
+
+ ASSERT(m->prepped);
+
+ motor_end_move(motor);
+
+ // Set direction, compensating for polarity
+ const bool counterclockwise = m->negative ^ m->reverse;
+ SET_PIN(m->dir_pin, counterclockwise);
+
+ // Adjust clock count
+ if (m->last_clock) {
+ uint24_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 >> 1;
+
+ m->timer->CNT = count;
+
+ } else m->timer->CNT = m->timer_period >> 1;
+
+ // Reset DMA channel counter
+ m->dma->CTRLA &= ~DMA_CH_ENABLE_bm;
+ m->dma->TRFCNT = 0xffff;
+ m->dma->CTRLA |= DMA_CH_ENABLE_bm;
+
+ // Set 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; // Save clock value
+ m->timer_clock = 0; // Clear clock
+ m->last_negative = m->negative;
+ m->commanded = m->position;
+
+ // Clear move
+ m->prepped = false;
+}
+
+
+void motor_prep_move(int motor, float time, int32_t target) {
+ motor_t *m = &motors[motor];
+
+ // Validate input
+ ASSERT(0 <= motor && motor < MOTORS);
+ ASSERT(!m->prepped);
+
+ // We count in half steps
+ target = target << 1;
+
+ // Compute travel in steps
+ int24_t half_steps = target - m->position;
+ m->position = target;
+
+ // Error correction
+ int16_t correction = abs(m->error);
+ if (MIN_HALF_STEP_CORRECTION <= correction) {
+ // Allowed step correction is proportional to velocity
+ int24_t positive_half_steps = half_steps < 0 ? -half_steps : half_steps;
+ int16_t max_correction = (positive_half_steps >> 5) + 1;
+ if (max_correction < correction) correction = max_correction;
+
+ if (m->error < 0) correction = -correction;
+
+ half_steps += correction;
+ m->error -= correction;
+ }
+
+ // Positive steps from here on
+ m->negative = half_steps < 0;
+ if (m->negative) half_steps = -half_steps;
+
+ // Find the fastest clock rate that will fit the required number of steps.
+ // Note, clock toggles step line so we need two clocks per step.
+ uint24_t seg_clocks = time * F_CPU * 60;
+ uint24_t ticks_per_step = seg_clocks / half_steps + 1; // Round up
+ 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)) + 1; // Round up
+
+ if (!m->timer_period || !half_steps) m->timer_clock = 0;
+
+ // Power motor
+ switch (m->power_mode) {
+ case MOTOR_POWERED_ONLY_WHEN_MOVING:
+ if (!m->timer_clock) break; // Not moving
+ // Fall through
+
+ case MOTOR_ALWAYS_POWERED: case MOTOR_POWERED_IN_CYCLE:
+ // Reset timeout
+ m->power_timeout = rtc_get_time() + MOTOR_IDLE_TIMEOUT * 1000;
+ break;
+
+ default: break;
+ }
+ _update_power(motor);
+
+ // Queue move
+ m->prepped = true;
+}
+
+
+// Var callbacks
+float get_step_angle(int motor) {return motors[motor].step_angle;}
+
+
+void set_step_angle(int motor, float value) {
+ motors[motor].step_angle = value;
+ _update_config(motor);
+}
+
+
+float get_travel(int motor) {return motors[motor].travel_rev;}
+
+
+void set_travel(int motor, float value) {
+ motors[motor].travel_rev = value;
+ _update_config(motor);
+}
+
+
+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;}
+char get_motor_axis(int motor) {return motors[motor].axis;}
+void set_motor_axis(int motor, uint8_t axis) {motor_set_axis(motor, axis);}
+
+
+uint8_t get_power_mode(int motor) {return motors[motor].power_mode;}
+
+
+void set_power_mode(int motor, uint8_t value) {
+ if (value <= MOTOR_POWERED_ONLY_WHEN_MOVING)
+ motors[motor].power_mode = value;
+ else motors[motor].power_mode = MOTOR_DISABLED;
+}
+
+
+int32_t get_encoder(int m) {return motors[m].encoder;}
+int32_t get_error(int m) {return motors[m].error;}
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2017 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_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_t;
+
+
+void motor_init();
+
+bool motor_is_enabled(int motor);
+int motor_get_axis(int motor);
+float motor_get_steps_per_unit(int motor);
+uint16_t motor_get_microsteps(int motor);
+void motor_set_microsteps(int motor, uint16_t microsteps);
+void motor_set_position(int motor, int32_t position);
+int32_t motor_get_position(int motor);
+
+stat_t motor_rtc_callback();
+
+void motor_end_move(int motor);
+void motor_load_move(int motor);
+void motor_prep_move(int motor, float time, int32_t target);
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2017 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 "outputs.h"
+#include "config.h"
+
+
+typedef struct {
+ uint8_t pin;
+ bool active;
+ output_state_t state;
+ output_mode_t mode;
+} output_t;
+
+
+output_t outputs[OUTS] = {
+ {TOOL_ENABLE_PIN},
+ {TOOL_DIR_PIN},
+ {SWITCH_1_PIN},
+ {SWITCH_2_PIN},
+ {FAULT_PIN},
+};
+
+
+static output_t *_get_output(uint8_t pin) {
+ switch (pin) {
+ case TOOL_ENABLE_PIN: return &outputs[0];
+ case TOOL_DIR_PIN: return &outputs[1];
+ case SWITCH_1_PIN: return &outputs[2];
+ case SWITCH_2_PIN: return &outputs[3];
+ case FAULT_PIN: return &outputs[4];
+ }
+
+ return 0;
+}
+
+
+static void _update_state(output_t *output) {
+ switch (output->mode) {
+ case OUT_DISABLED: output->state = OUT_TRI; break;
+ case OUT_LO_HI: output->state = output->active ? OUT_HI : OUT_LO; break;
+ case OUT_HI_LO: output->state = output->active ? OUT_LO : OUT_HI; break;
+ case OUT_TRI_LO: output->state = output->active ? OUT_LO : OUT_TRI; break;
+ case OUT_TRI_HI: output->state = output->active ? OUT_HI : OUT_TRI; break;
+ case OUT_LO_TRI: output->state = output->active ? OUT_TRI : OUT_LO; break;
+ case OUT_HI_TRI: output->state = output->active ? OUT_TRI : OUT_HI; break;
+ }
+
+ switch (output->state) {
+ case OUT_TRI: DIRCLR_PIN(output->pin); break;
+ case OUT_HI: OUTSET_PIN(output->pin); DIRSET_PIN(output->pin); break;
+ case OUT_LO: OUTCLR_PIN(output->pin); DIRSET_PIN(output->pin); break;
+ }
+}
+
+
+void outputs_init() {
+ for (int i = 0; i < OUTS; i++) _update_state(&outputs[i]);
+}
+
+
+bool outputs_is_active(uint8_t pin) {
+ output_t *output = _get_output(pin);
+ return output ? output->active : false;
+}
+
+
+void outputs_set_active(uint8_t pin, bool active) {
+ output_t *output = _get_output(pin);
+ if (!output) return;
+
+ output->active = active;
+ _update_state(output);
+}
+
+
+void outputs_set_mode(uint8_t pin, output_mode_t mode) {
+ output_t *output = _get_output(pin);
+ if (!output) return;
+ output->mode = mode;
+}
+
+
+output_state_t outputs_get_state(uint8_t pin) {
+ output_t *output = _get_output(pin);
+ if (output) return OUT_TRI;
+ return output->state;
+}
+
+
+uint8_t get_output_state(uint8_t id) {
+ return OUTS <= id ? OUT_TRI : outputs[id].state;
+}
+
+
+uint8_t get_output_mode(uint8_t id) {
+ return OUTS <= id ? OUT_DISABLED : outputs[id].mode;
+}
+
+
+void set_output_mode(uint8_t id, uint8_t mode) {
+ if (OUTS <= id) return;
+ outputs[id].mode = mode;
+ _update_state(&outputs[id]);
+}
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2017 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>
+
+
+typedef enum {
+ OUT_LO,
+ OUT_HI,
+ OUT_TRI,
+} output_state_t;
+
+
+// OUT_inactive_active
+typedef enum {
+ OUT_DISABLED,
+ OUT_LO_HI,
+ OUT_HI_LO,
+ OUT_TRI_LO,
+ OUT_TRI_HI,
+ OUT_LO_TRI,
+ OUT_HI_TRI,
+} output_mode_t;
+
+
+void outputs_init();
+bool outputs_is_active(uint8_t pin);
+void outputs_set_active(uint8_t pin, bool active);
+void outputs_set_mode(uint8_t pin, output_mode_t mode);
+output_state_t outputs_get_state(uint8_t pin);
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2017 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
+
+#ifdef __AVR__
+
+#include <avr/pgmspace.h>
+#define PRPSTR "S"
+
+#else // __AVR__
+
+#define PRPSTR "s"
+#define PROGMEM
+#define PGM_P char *
+#define PSTR(X) X
+#define vfprintf_P vfprintf
+#define printf_P printf
+#define puts_P puts
+#define sprintf_P sprintf
+#define strcmp_P strcmp
+#define pgm_read_ptr(x) *(x)
+#define pgm_read_word(x) *(x)
+#define pgm_read_byte(x) *(x)
+
+#endif // __AVR__
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2017 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 - 2017 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
+
+enum {PORT_A = 1, PORT_B, PORT_C, PORT_D, PORT_E, PORT_F};
+
+#define PORT(PIN) pin_ports[(PIN >> 3) - 1]
+#define BM(PIN) (1 << (PIN & 7))
+
+#ifdef __AVR__
+#include <avr/io.h>
+
+extern PORT_t *pin_ports[];
+
+#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])
+
+#define SET_PIN(PIN, X) \
+ do {if (X) OUTSET_PIN(PIN); else OUTCLR_PIN(PIN);} while (0);
+
+#endif // __AVR__
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2017 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 <plan/state.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_velocity_max(arc.plane_axis_0),
+ planar_travel / axis_get_velocity_max(arc.plane_axis_1),
+ fabs(linear_travel) / axis_get_velocity_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, mach_in_absolute_mode());
+
+ // 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
+ mp_pause_queue(true); // Hold queue until arc is done
+ 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, 0);
+
+ if (!--arc.segments) mach_abort_arc();
+ }
+}
+
+
+/// Stop arc movement without maintaining position
+/// OK to call if no arc is running
+void mach_abort_arc() {
+ arc.running = false;
+ mp_pause_queue(false);
+}
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2017 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();
+void mach_abort_arc();
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2017 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 {
+ uint16_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_buffer_validate(mb.tail);
+ 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();
+}
+
+
+#ifdef DEBUG
+void mp_queue_dump() {
+ mp_buffer_t *bf = mp_queue_get_head();
+ if (!bf) return;
+ mp_buffer_t *bp = bf;
+
+ do {
+ if (bp != bf) putchar(',');
+ mp_buffer_print(bp);
+ bp = mp_buffer_next(bp);
+ } while (bp != bf && bp->state != BUFFER_OFF);
+
+ if (bp != bf) mp_buffer_print(bp);
+
+ putchar('\n');
+}
+
+
+void mp_buffer_print(const mp_buffer_t *bf) {
+ printf("{"
+ "\"ts\":%d,"
+ "\"line\":%d,"
+ "\"state\":%d,"
+ "\"replannable\":%s,"
+ "\"hold\":%s,"
+ "\"value\":%0.2f,"
+ "\"target\":[%0.2f, %0.2f, %0.2f, %0.2f],"
+ "\"unit\":[%0.2f, %0.2f, %0.2f, %0.2f],"
+ "\"length\":%0.2f,"
+ "\"head_length\":%0.2f,"
+ "\"body_length\":%0.2f,"
+ "\"tail_length\":%0.2f,"
+ "\"entry_velocity\":%0.2f,"
+ "\"cruise_velocity\":%0.2f,"
+ "\"exit_velocity\":%0.2f,"
+ "\"braking_velocity\":%0.2f,"
+ "\"entry_vmax\":%0.2f,"
+ "\"cruise_vmax\":%0.2f,"
+ "\"exit_vmax\":%0.2f,"
+ "\"delta_vmax\":%0.2f,"
+ "\"jerk\":%0.2f,"
+ "\"cbrt_jerk\":%0.2f"
+ "}", bf->ts, bf->line, bf->state,
+ (bf->flags & BUFFER_REPLANNABLE) ? "true" : "false",
+ (bf->flags & BUFFER_HOLD) ? "true" : "false",
+ bf->value, bf->target[0], bf->target[1],
+ bf->target[2], bf->target[3], bf->unit[0], bf->unit[1], bf->unit[2],
+ bf->unit[3], bf->length, bf->head_length, bf->body_length,
+ bf->tail_length, bf->entry_velocity, bf->cruise_velocity,
+ bf->exit_velocity, bf->braking_velocity, bf->entry_vmax,
+ bf->cruise_vmax, bf->exit_vmax, bf->delta_vmax, bf->jerk,
+ bf->cbrt_jerk);
+}
+#endif // DEBUG
+
+
+void mp_buffer_validate(const mp_buffer_t *bp) {
+ ASSERT(bp);
+
+ if (!(bp->flags & BUFFER_LINE)) return; // Only check line buffers
+
+ ASSERT(isfinite(bp->value));
+
+ ASSERT(isfinite(bp->target[0]) && isfinite(bp->target[1]) &&
+ isfinite(bp->target[2]) && isfinite(bp->target[3]));
+ ASSERT(isfinite(bp->unit[0]) && isfinite(bp->unit[1]) &&
+ isfinite(bp->unit[2]) && isfinite(bp->unit[3]));
+
+ ASSERT(isfinite(bp->length));
+ ASSERT(isfinite(bp->head_length));
+ ASSERT(isfinite(bp->body_length));
+ ASSERT(isfinite(bp->tail_length));
+
+ ASSERT(isfinite(bp->entry_velocity));
+ ASSERT(isfinite(bp->cruise_velocity));
+ ASSERT(isfinite(bp->exit_velocity));
+ ASSERT(isfinite(bp->braking_velocity));
+
+ ASSERT(isfinite(bp->jerk));
+ ASSERT(isfinite(bp->cbrt_jerk));
+}
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2017 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;
+
+
+typedef enum {
+ BUFFER_REPLANNABLE = 1 << 0,
+ BUFFER_HOLD = 1 << 1,
+ BUFFER_SEEK_CLOSE = 1 << 2,
+ BUFFER_SEEK_OPEN = 1 << 3,
+ BUFFER_SEEK_ERROR = 1 << 4,
+ BUFFER_RAPID = 1 << 5,
+ BUFFER_INVERSE_TIME = 1 << 6,
+ BUFFER_EXACT_STOP = 1 << 7,
+ BUFFER_LINE = 1 << 8,
+} buffer_flags_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
+ buffer_flags_t flags; // buffer flags
+ switch_id_t sw; // Switch to seek
+
+ float value; // used in dwell and other callbacks
+
+ float target[AXES]; // XYZABC where the move should go in mm
+ 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();
+
+void mp_queue_dump();
+
+void mp_buffer_print(const mp_buffer_t *bp);
+void mp_buffer_validate(const mp_buffer_t *bp);
+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 - 2017 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>
+#include <inttypes.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 = 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_position(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_position(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);
+
+ 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 - 2017 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 - 2017 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 - 2017 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>
+
+
+stat_t mp_dwell(float seconds, int32_t line);
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2017 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 "stepper.h"
+#include "motor.h"
+#include "rtc.h"
+#include "util.h"
+#include "velocity_curve.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;
+
+ uint24_t segment_count; // count of running segments
+ uint24_t segment; // current segment
+ float segment_time;
+ float segment_velocity; // computed velocity for segment
+ float segment_start[AXES];
+ float segment_delta;
+ float segment_dist;
+ 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}};
+
+/// Common code for head and tail sections
+static stat_t _exec_aline_section(float length, float Vi, float Vt) {
+ if (ex.section_new) {
+ ASSERT(isfinite(length));
+
+ if (fp_ZERO(length)) return STAT_NOOP; // end the section
+
+ ASSERT(isfinite(Vi) && isfinite(Vt));
+ ASSERT(0 <= Vi && 0 <= Vt);
+ ASSERT(Vi || Vt);
+
+ // len / avg. velocity
+ const float move_time = 2 * length / (Vi + Vt); // in mins
+ const float segments = ceil(move_time / SEGMENT_TIME);
+ ex.segment_count = segments;
+ ex.segment_time = move_time / segments; // in mins
+ ex.segment = 0;
+ ex.segment_dist = 0;
+
+ for (int axis = 0; axis < AXES; axis++)
+ ex.segment_start[axis] = mp_runtime_get_axis_position(axis);
+
+ if (Vi == Vt) {
+ ex.segment_delta = length / segments;
+ ex.segment_velocity = Vi;
+
+ } else ex.segment_delta = 1 / (segments + 1);
+
+ ex.section_new = false;
+ }
+
+ float target[AXES];
+ ex.segment++;
+
+ // 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 section waypoint compute target from segment time and velocity. Don't
+ // do waypoint correction when going into a hold.
+ if (ex.segment == ex.segment_count && !ex.section_new && !ex.hold_planned)
+ copy_vector(target, ex.waypoint[ex.section]);
+
+ else {
+ if (Vi == Vt) ex.segment_dist += ex.segment_delta;
+ else {
+ // Compute quintic Bezier curve
+ ex.segment_velocity =
+ velocity_curve(Vi, Vt, ex.segment * ex.segment_delta);
+ ex.segment_dist += ex.segment_velocity * ex.segment_time;
+ }
+
+ // Avoid overshoot
+ if (length < ex.segment_dist) ex.segment_dist = length;
+
+ for (int axis = 0; axis < AXES; axis++)
+ target[axis] = ex.segment_start[axis] + ex.unit[axis] * ex.segment_dist;
+ }
+
+ mp_runtime_set_velocity(ex.segment_velocity);
+ RITORNO(mp_runtime_move_to_target(ex.segment_time, target));
+
+ // Return EAGAIN to continue or OK if this segment is done
+ return ex.segment < ex.segment_count ? STAT_EAGAIN : STAT_OK;
+}
+
+
+/// 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;
+ }
+}
+
+
+/// 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());
+ // Velocity left to shed to brake to zero
+ float braking_velocity = ex.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.
+ 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 if (HOLD_VELOCITY_TOLERANCE < braking_velocity) {
+ // 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);
+
+ } else { // Were're close enough
+ ex.tail_length = available_length;
+ ex.exit_velocity = 0;
+ }
+
+ // Don't error if seek was stopped
+ bf->flags &= ~BUFFER_SEEK_ERROR;
+}
+
+
+/// Initializes move runtime with a new planner buffer
+static stat_t _exec_aline_init(mp_buffer_t *bf) {
+#ifdef DEBUG
+ printf("buffer:");
+ mp_buffer_print(bf);
+ putchar('\n');
+#endif
+
+ // 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;
+
+ // Enforce min cruise velocity
+ // TODO How does cruise_velocity ever end up zero when length is non-zero?
+ if (ex.cruise_velocity < 10) ex.cruise_velocity = 10;
+
+ 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;
+ }
+
+ // If seeking, end move when switch is in target state.
+ if ((((bf->flags & BUFFER_SEEK_CLOSE) && switch_is_active(bf->sw)) ||
+ ((bf->flags & BUFFER_SEEK_OPEN) && !switch_is_active(bf->sw))) &&
+ !ex.hold_planned) {
+ if (!fp_ZERO(mp_runtime_get_velocity())) _plan_hold();
+ else {
+ mp_runtime_set_velocity(0);
+ bf->flags &= ~BUFFER_SEEK_ERROR;
+ return STAT_NOOP;
+ }
+ }
+
+ // 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;
+ }
+
+ // Exiting
+ if (status != STAT_EAGAIN) {
+ // Set runtime velocity on exit
+ mp_runtime_set_velocity(ex.exit_velocity);
+
+ // If seeking, switch was not found. Signal error if necessary.
+ if ((bf->flags & (BUFFER_SEEK_CLOSE | BUFFER_SEEK_OPEN)) &&
+ (bf->flags & BUFFER_SEEK_ERROR))
+ return STAT_SEEK_SWTICH_NOT_FOUND;
+ }
+
+ 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);
+ if (mp_get_state() == STATE_STOPPING) mp_state_holding();
+
+ 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->flags &= ~BUFFER_REPLANNABLE;
+
+ // 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->flags & BUFFER_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)->flags &= ~BUFFER_REPLANNABLE;
+
+ 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 - 2017 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 - 2017 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 "velocity_curve.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 {
+ float delta;
+ float t;
+ bool changed;
+
+ int sign;
+ float velocity;
+ float accel;
+ float next;
+ float initial;
+ float target;
+} jog_axis_t;
+
+
+typedef struct {
+ bool writing;
+ bool done;
+
+ float Vi;
+ float Vt;
+
+ jog_axis_t axes[AXES];
+} jog_runtime_t;
+
+
+static jog_runtime_t jr;
+
+
+static bool _next_axis_velocity(int axis) {
+ jog_axis_t *a = &jr.axes[axis];
+
+ float Vn = a->next * axis_get_velocity_max(axis);
+ float Vi = a->velocity;
+ float Vt = a->target;
+
+ if (JOG_MIN_VELOCITY < fabs(Vn)) jr.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) return false; // No change
+
+ a->target = Vn;
+ if (Vn) a->sign = Vn < 0 ? -1 : 1;
+
+ return true;
+}
+
+
+static float _compute_axis_velocity(int axis) {
+ jog_axis_t *a = &jr.axes[axis];
+
+ float V = fabs(a->velocity);
+ float Vt = fabs(a->target);
+
+ if (JOG_MIN_VELOCITY < Vt) jr.done = false;
+
+ if (fp_EQ(V, Vt)) return Vt;
+
+ // Compute axis max jerk
+ float jerk = axis_get_jerk_max(axis) * JERK_MULTIPLIER;
+
+ // Compute target accel
+ float accel = sqrt(jerk * fabs(Vt - V)) * (Vt < V ? -1 : 1);
+
+ // TODO apply max accel here
+
+ // Compute max delta accel
+ float deltaAccel = jerk * SEGMENT_TIME;
+
+ // Update accel
+ if (a->accel < accel) {
+ if (accel < a->accel + deltaAccel) a->accel = accel;
+ else a->accel += deltaAccel;
+
+ } else if (accel < a->accel) {
+ if (a->accel - deltaAccel < accel) a->accel = accel;
+ else a->accel -= deltaAccel;
+ }
+
+ return V + a->accel * SEGMENT_TIME;
+}
+
+
+#if 0
+static float _axis_velocity_at_limits(int axis) {
+ float V = jr.axes[axis].velocity;
+
+ if (axis_get_homed(axis)) {
+ float min = axis_get_travel_min(axis);
+ float max = axis_get_travel_max(axis);
+
+ if (position <= min) return 0;
+ if (max <= position) return 0;
+
+ float position = mp_runtime_get_axis_position(axis);
+ float jerk = axis_get_jerk_max(axis) * JERK_MULTIPLIER;
+ float decelDist = mp_get_target_length(V, 0, jerk);
+
+ if (position < min + decelDist) {
+ }
+
+ if (max - decelDist < position) {
+ }
+ }
+
+ return V;
+}
+#endif
+
+
+static stat_t _exec_jog(mp_buffer_t *bf) {
+ // Load next velocity
+ jr.done = true;
+
+ if (!jr.writing)
+ for (int axis = 0; axis < AXES; axis++) {
+ if (!axis_is_enabled(axis)) continue;
+ jr.axes[axis].changed = _next_axis_velocity(axis);
+ }
+
+ float velocity_sqr = 0;
+
+ // Compute per axis velocities
+ for (int axis = 0; axis < AXES; axis++) {
+ if (!axis_is_enabled(axis)) continue;
+ float V = _compute_axis_velocity(axis);
+ velocity_sqr += square(V);
+ jr.axes[axis].velocity = V * jr.axes[axis].sign;
+ if (JOG_MIN_VELOCITY < V) jr.done = false;
+ }
+
+ // Check if we are done
+ if (jr.done) {
+ // Update machine position
+ mach_set_position_from_runtime();
+ mp_set_cycle(CYCLE_MACHINING); // Default cycle
+ mp_pause_queue(false);
+
+ 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.axes[axis].velocity * SEGMENT_TIME;
+
+ // Set velocity and target
+ mp_runtime_set_velocity(sqrt(velocity_sqr));
+ stat_t status = mp_runtime_move_to_target(SEGMENT_TIME, target);
+ if (status != STAT_OK) return status;
+
+ return STAT_EAGAIN;
+}
+
+
+uint8_t command_jog(int argc, char *argv[]) {
+ if (mp_get_cycle() != CYCLE_JOGGING &&
+ (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_get_cycle() != CYCLE_JOGGING) memset(&jr, 0, sizeof(jr));
+
+ jr.writing = true;
+ for (int axis = 0; axis < AXES; axis++)
+ jr.axes[axis].next = velocity[axis];
+ jr.writing = false;
+
+ if (mp_get_cycle() != CYCLE_JOGGING) {
+ mp_set_cycle(CYCLE_JOGGING);
+ mp_pause_queue(true);
+ mp_queue_push_nonstop(_exec_jog, -1);
+ }
+
+ return STAT_OK;
+}
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2017 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
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2017 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"
+
+#include <stdio.h>
+#include <float.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 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(dot(a, b) / (norm(a) * norm(b)));
+ * 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 FLT_MAX; // 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] * JUNCTION_DEVIATION);
+ b_delta += square(b_unit[axis] * JUNCTION_DEVIATION);
+ }
+
+ if (!a_delta || !b_delta) return 0; // One or both unit vectors are null
+
+ float delta = (sqrt(a_delta) + sqrt(b_delta)) / 2;
+ float sin_half_theta = sqrt((1 - costheta) / 2);
+ float radius = delta * sin_half_theta / (1 - sin_half_theta);
+ 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.
+ */
+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_jerk_max(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);
+
+ ASSERT(isfinite(unit[axis]) && unit[axis]);
+
+ // 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) {
+ ASSERT(0 < move_time && isfinite(move_time));
+
+ bf->cruise_vmax = bf->length / move_time; // target velocity requested
+
+ float junction_velocity = FLT_MAX;
+
+ mp_buffer_t *prev = mp_buffer_prev(bf);
+ while (prev->state != BUFFER_OFF)
+ if (prev->flags & BUFFER_LINE) {
+ junction_velocity = _get_junction_vmax(prev->unit, bf->unit);
+ break;
+
+ } else prev = mp_buffer_prev(prev);
+
+ 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;
+}
+
+
+/* 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) {
+ ASSERT(0 < 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]) / axis_get_velocity_max(axis);
+ if (max_time < time) max_time = time;
+ }
+
+ return max_time < SEGMENT_TIME ? 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[], buffer_flags_t flags, switch_id_t sw,
+ float feed_rate, float feed_override, int32_t line) {
+ DEBUG_CALL("(%f, %f, %f, %f), %s%s%s, %f, %f, %d",
+ target[0], target[1], target[2], target[3],
+ (flags & BUFFER_RAPID) ? "rapid|" : "",
+ (flags & BUFFER_INVERSE_TIME) ? "inverse_time|" : "",
+ (flags & BUFFER_EXACT_STOP) ? "exact_stop|" : "",
+ feed_rate, feed_override, line);
+
+ // Trap zero feed rate condition
+ if (!(flags & BUFFER_RAPID) && fp_ZERO(feed_rate))
+ return STAT_GCODE_FEEDRATE_NOT_SPECIFIED;
+
+ // 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, flags & BUFFER_RAPID,
+ flags & BUFFER_INVERSE_TIME, feed_rate,
+ feed_override);
+ _calc_max_velocities(bf, time, flags & BUFFER_EXACT_STOP);
+
+ flags |= BUFFER_LINE;
+ if (!(flags & BUFFER_EXACT_STOP)) flags |= BUFFER_REPLANNABLE;
+
+ // Note, the following lines must remain in order.
+ bf->line = line; // Planner needs this when planning steps
+ bf->flags = flags; // Move flags
+ bf->sw = sw; // Seek switch, if any
+ mp_plan(bf); // Plan block list
+ mp_set_position(target); // Set planner position before pushing 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 - 2017 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 "buffer.h"
+
+#include <stdbool.h>
+#include <stdint.h>
+
+
+stat_t mp_aline(const float target[], buffer_flags_t flags, switch_id_t sw,
+ 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 - 2017 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 \
+ (SEGMENT_TIME * (bf->cruise_velocity + bf->entry_velocity))
+#define MIN_TAIL_LENGTH \
+ (SEGMENT_TIME * (bf->cruise_velocity + bf->exit_velocity))
+#define MIN_BODY_LENGTH (SEGMENT_TIME * 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. 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 lowered.
+ * 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) {
+ 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 (isfinite(naive_move_time) && naive_move_time < SEGMENT_TIME) {
+ bf->cruise_velocity = bf->length / SEGMENT_TIME;
+ 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 (isfinite(naive_move_time) && naive_move_time <= SEGMENT_TIME) {
+ bf->entry_velocity = mp_buffer_prev(bf)->exit_velocity;
+
+ if (!fp_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 ((fabs(bf->cruise_velocity - bf->entry_velocity) <
+ TRAPEZOID_VELOCITY_TOLERANCE) &&
+ (fabs(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->tail_length < bf->head_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 if (bf->head_length + bf->tail_length) {
+ 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);
+
+ } else break;
+
+ } while (TRAPEZOID_ITERATION_ERROR_PERCENT <
+ (fabs(bf->cruise_velocity - computed_velocity) /
+ computed_velocity));
+
+ // 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_ZERO(bf->body_length)) {
+ if (!fp_ZERO(bf->head_length)) {
+ if (!fp_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
+void mp_print_buffer(mp_buffer_t *bp) {
+ printf_P(PSTR("{\"msg\":\""));
+ printf_P(PSTR("%d,"), bp->flags & BUFFER_REPLANNABLE);
+ printf_P(PSTR("0x%04lx,"), (intptr_t)bp->cb);
+ printf_P(PSTR("%0.2f,"), bp->length);
+ printf_P(PSTR("%0.2f,"), bp->head_length);
+ printf_P(PSTR("%0.2f,"), bp->body_length);
+ printf_P(PSTR("%0.2f,"), bp->tail_length);
+ printf_P(PSTR("%0.2f,"), bp->entry_velocity);
+ printf_P(PSTR("%0.2f,"), bp->cruise_velocity);
+ printf_P(PSTR("%0.2f,"), bp->exit_velocity);
+ printf_P(PSTR("%0.2f,"), bp->braking_velocity);
+ printf_P(PSTR("%0.2f,"), bp->entry_vmax);
+ printf_P(PSTR("%0.2f,"), bp->cruise_vmax);
+ printf_P(PSTR("%0.2f"), bp->exit_vmax);
+ printf_P(PSTR("\"}\n"));
+
+ while (!usart_tx_empty()) continue;
+}
+
+
+/// 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\":\"replannable,callback,"
+ "length,head_length,body_length,tail_length,"
+ "entry_velocity,cruise_velocity,exit_velocity,braking_velocity,"
+ "entry_vmax,cruise_vmax,exit_vmax\"}\n"));
+
+ mp_buffer_t *bp = bf;
+ while (bp) {
+ mp_print_buffer(bp);
+ bp = mp_buffer_prev(bp);
+ if (bp == bf || bp->state == BUFFER_OFF) break;
+ }
+}
+#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->flags - replanable, hold, probe, etc [Note 1]
+ * 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->flags - replanable
+ * 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->flags
+ * BUFFER_REPLANNABLE bit. 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 *bf) {
+ mp_buffer_t *bp = bf;
+
+ // 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)) != bf) {
+ if (!(bp->flags & BUFFER_REPLANNABLE)) break;
+
+ // TODO what about non-move buffers?
+ 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 bf.
+ mp_buffer_t *prev = bp;
+ while ((bp = mp_buffer_next(bp)) != bf) {
+ mp_buffer_t *next = mp_buffer_next(bp);
+
+ bp->entry_velocity = prev == bf ? 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->flags |= BUFFER_HOLD;
+
+ } else bp->flags &= ~BUFFER_HOLD;
+
+ 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->flags & BUFFER_REPLANNABLE) &&
+ fp_EQ(bp->exit_velocity, (bp->entry_velocity + bp->delta_vmax))))
+ bp->flags &= ~BUFFER_REPLANNABLE;
+
+ prev = bp;
+ }
+
+ // Finish last block
+ bf->entry_velocity = prev->exit_velocity;
+ bf->cruise_velocity = bf->cruise_vmax;
+ bf->exit_velocity = 0;
+
+ mp_calculate_trapezoid(bf);
+}
+
+
+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->flags |= BUFFER_REPLANNABLE;
+ 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;
+ bp->flags |= BUFFER_REPLANNABLE;
+
+ 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) {
+ ASSERT(0 <= Vi);
+ ASSERT(0 <= Vf);
+ ASSERT(isfinite(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 equation c) solved for Vf. We use f) since
+ * it is more simple.
+ *
+ * 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) {
+ ASSERT(0 <= Vi);
+ ASSERT(0 <= L);
+ ASSERT(isfinite(bf->jerk));
+ ASSERT(isfinite(bf->cbrt_jerk));
+
+ if (!L) return Vi;
+
+ // 0 iterations (a reasonable estimate)
+ float x = pow(L, 0.66666666) * bf->cbrt_jerk + Vi; // First estimate
+
+#if GET_VELOCITY_ITERATIONS
+ 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 - 2017 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
+
+/// 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
+#define HOLD_VELOCITY_TOLERANCE 60 // In mm/min
+
+
+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_print_buffer(mp_buffer_t *bp);
+
+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 - 2017 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
+
+#if 0 // TODO These are not currently being set
+ 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;
+#endif
+} 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_position(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, 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(const 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 time, const float target[]) {
+ ASSERT(isfinite(target[0]) && isfinite(target[1]) &&
+ isfinite(target[2]) && isfinite(target[3]));
+
+ // Convert target position to steps.
+ float steps[MOTORS];
+ mp_kinematics(target, steps);
+
+ // Call the stepper prep function
+ RITORNO(st_prep_line(time, steps));
+
+ // Update positions
+ mp_runtime_set_position(target);
+
+ return STAT_OK;
+}
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2017 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, float position);
+float mp_runtime_get_axis_position(uint8_t axis);
+
+float *mp_runtime_get_position();
+void mp_runtime_set_position(const 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 time, const float target[]);
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2017 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 "spindle.h"
+
+#include "report.h"
+
+#include <stdbool.h>
+
+
+typedef struct {
+ mp_state_t state;
+ mp_cycle_t cycle;
+ mp_hold_reason_t hold_reason;
+ bool pause;
+
+ 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();
+}
+
+
+bool mp_is_ready() {
+ return mp_queue_get_room() && !mp_is_resuming() && !ps.pause;
+}
+
+
+void mp_pause_queue(bool x) {ps.pause = x;}
+
+
+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);
+ mp_pause_queue(false);
+}
+
+
+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.
+ mach_set_position_from_runtime();
+ }
+
+ // Stop spindle
+ spindle_stop();
+
+ // 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 - 2017 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 "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();
+bool mp_is_ready();
+void mp_pause_queue(bool x);
+
+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 - 2017 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 "velocity_curve.h"
+
+#include <math.h>
+
+
+/// We are using a quintic (fifth-degree) Bezier polynomial for the velocity
+/// curve. This yields a constant pop; with pop being the sixth derivative
+/// of position:
+///
+/// 1st - velocity
+/// 2nd - acceleration
+/// 3rd - jerk
+/// 4th - snap
+/// 5th - crackle
+/// 6th - pop
+///
+/// The Bezier curve takes the form:
+///
+/// f(t) = P_0(1 - t)^5 + 5P_1(1 - t)^4 t + 10P_2(1 - t)^3 t^2 +
+/// 10P_3(1 - t)^2 t^3 + 5P_4(1 - t)t^4 + P_5t^5
+///
+/// Where 0 <= t <= 1, f(t) is the velocity and P_0 through P_5 are the control
+/// points. In our case:
+///
+/// P_0 = P_1 = P2 = Vi
+/// P_3 = P_4 = P5 = Vt
+///
+/// Where Vi is the initial velocity and Vt is the target velocity.
+///
+/// After substitution, expanding the polynomial and collecting terms we have:
+///
+/// f(t) = (Vt - Vi)(6t^5 - 15t^4 + 10t^3) + Vi
+///
+/// Computing this directly using 32bit float-point on a 32MHz AtXMega processor
+/// takes about 60uS or about 1,920 clocks. The code was compiled with avr-gcc
+/// v4.9.2 with -O3.
+float velocity_curve(float Vi, float Vt, float t) {
+ // If the change is small enough just do a linear velocity transition.
+ // TODO revisit this.
+ if (fabs(Vt - Vi) < 200) return Vi + (Vt - Vi) * t;
+
+ const float t2 = t * t;
+ const float t3 = t2 * t;
+
+ return (Vt - Vi) * (6 * t2 - 15 * t + 10) * t3 + Vi;
+}
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2017 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
+
+float velocity_curve(float Vi, float Vt, float t);
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2017 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"
+#include "estop.h"
+#include "outputs.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 pwm_invert;
+} pwm_spindle_t;
+
+
+static pwm_spindle_t spindle = {0};
+
+
+static void _set_enable(bool enable) {
+ outputs_set_active(TOOL_ENABLE_PIN, enable);
+}
+
+
+static void _set_dir(bool clockwise) {
+ outputs_set_active(TOOL_DIR_PIN, !clockwise);
+}
+
+
+static void _set_pwm(spindle_mode_t mode, float speed) {
+ if (mode == SPINDLE_OFF || speed < spindle.min_rpm || estop_triggered()) {
+ TIMER_PWM.CTRLA = 0;
+ OUTCLR_PIN(SPIN_PWM_PIN);
+ _set_enable(false);
+ return;
+ }
+
+ // Invert PWM
+ if (spindle.pwm_invert) PINCTRL_PIN(SPIN_PWM_PIN) |= PORT_INVEN_bm;
+ else PINCTRL_PIN(SPIN_PWM_PIN) &= ~PORT_INVEN_bm;
+
+ // 100% duty
+ if (spindle.max_rpm <= speed && spindle.max_duty == 1) {
+ TIMER_PWM.CTRLB = 0;
+ OUTSET_PIN(SPIN_PWM_PIN);
+ return;
+ }
+
+ // Clamp speed
+ if (spindle.max_rpm < speed) speed = spindle.max_rpm;
+ if (speed < spindle.min_rpm) speed = 0;
+
+ // Set clock period and optimal prescaler value
+ float prescale = (float)(F_CPU >> 16) / 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;
+
+ // Configure clock
+ TIMER_PWM.CTRLB = TC1_CCAEN_bm | TC_WGMODE_SINGLESLOPE_gc;
+ TIMER_PWM.CCA = TIMER_PWM.PER * duty;
+}
+
+
+void pwm_spindle_init() {
+ // Configure IO
+ _set_dir(true);
+ _set_enable(false);
+
+ DIRSET_PIN(SPIN_PWM_PIN); // Output
+}
+
+
+void pwm_spindle_set(spindle_mode_t mode, float speed) {
+ if (mode != SPINDLE_OFF) _set_dir(mode == SPINDLE_CW);
+ _set_pwm(mode, speed);
+ _set_enable(mode != SPINDLE_OFF);
+}
+
+
+void pwm_spindle_stop() {pwm_spindle_set(SPINDLE_OFF, 0);}
+
+
+// TODO these need more effort and should work with the huanyang spindle too
+float get_max_spin() {return spindle.max_rpm;}
+void set_max_spin(float value) {spindle.max_rpm = value;}
+float get_min_spin() {return spindle.min_rpm;}
+void set_min_spin(float value) {spindle.min_rpm = value;}
+float get_spin_min_duty() {return spindle.min_duty * 100;}
+void set_spin_min_duty(float value) {spindle.min_duty = value / 100;}
+float get_spin_max_duty() {return spindle.max_duty * 100;}
+void set_spin_max_duty(float value) {spindle.max_duty = value / 100;}
+float get_spin_up() {return 0;} // TODO
+void set_spin_up(float value) {} // TODO
+float get_spin_down() {return 0;} // TODO
+void set_spin_down(float value) {} // TODO
+uint16_t get_spin_freq() {return spindle.freq;}
+void set_spin_freq(uint16_t value) {spindle.freq = value;}
+bool get_pwm_invert() {return spindle.pwm_invert;}
+void set_pwm_invert(bool value) {spindle.pwm_invert = value;}
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2017 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_stop();
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2017 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 "pgmspace.h"
+
+#include <stdio.h>
+#include <stdbool.h>
+
+
+static bool _requested = false;
+static bool _full = false;
+static uint32_t _last = 0;
+
+
+void report_request() {_requested = true;}
+void report_request_full() {_requested = _full = true;}
+
+
+void report_callback() {
+ if (_requested && usart_tx_empty()) {
+ uint32_t now = rtc_get_time();
+ if (now - _last < 250) return;
+ _last = now;
+
+ vars_report(_full);
+ _requested = _full = false;
+ }
+}
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2017 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 - 2017 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 - 2017 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 - 2017 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 - 2017 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_HUANYANG,
+ SPINDLE_TYPE_PWM,
+} spindle_type_t;
+
+
+typedef struct {
+ spindle_type_t type;
+ spindle_mode_t mode;
+ float speed;
+ bool reversed;
+} spindle_t;
+
+
+static spindle_t spindle = {0};
+
+
+void spindle_init() {
+ pwm_spindle_init();
+ huanyang_init();
+}
+
+
+void _spindle_set(spindle_mode_t mode, float speed) {
+ if (speed < 0) speed = 0;
+ if (mode != SPINDLE_CW && mode != SPINDLE_CCW) mode = SPINDLE_OFF;
+
+ spindle.mode = mode;
+ spindle.speed = speed;
+
+ if (spindle.reversed) {
+ if (mode == SPINDLE_CW) mode = SPINDLE_CCW;
+ else if (mode == SPINDLE_CCW) mode = SPINDLE_CW;
+ }
+
+ 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_set(mode, spindle.speed);}
+void spindle_set_speed(float speed) {_spindle_set(spindle.mode, speed);}
+spindle_mode_t spindle_get_mode() {return spindle.mode;}
+float spindle_get_speed() {return spindle.speed;}
+
+
+void spindle_stop() {
+ switch (spindle.type) {
+ case SPINDLE_TYPE_PWM: pwm_spindle_stop(); break;
+ case SPINDLE_TYPE_HUANYANG: huanyang_stop(); 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);
+ }
+}
+
+
+bool spindle_is_reversed() {return spindle.reversed;}
+bool get_spin_reversed() {return spindle.reversed;}
+
+
+void set_spin_reversed(bool reversed) {
+ spindle.reversed = reversed;
+ _spindle_set(spindle.mode, spindle.speed);
+}
+
+
+PGM_P get_spin_mode() {
+ switch (spindle.mode) {
+ case SPINDLE_CW: return PSTR("Clockwise");
+ case SPINDLE_CCW: return PSTR("Counterclockwise");
+ default: break;
+ }
+ return PSTR("Off");
+}
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2017 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_stop();
+bool spindle_is_reversed();
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2017 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\":\"%"PRPSTR"\", \"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\": \"%"PRPSTR"\""), 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\": \"%"PRPSTR"\""), 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, const char *msg) {
+ status_message_P(location, STAT_LEVEL_ERROR, code, msg);
+ estop_trigger(code);
+ while (!usart_tx_empty()) continue;
+ return code;
+}
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2017 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 "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, const char *msg);
+
+#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_DEBUG, 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, 0)
+#define ASSERT(COND) \
+ do { \
+ if (!(COND)) \
+ status_alarm(STATUS_LOCATION, STAT_INTERNAL_ERROR, PSTR(#COND)); \
+ } while (0)
+
+
+#ifdef DEBUG
+#define DEBUG_CALL(FMT, ...) \
+ printf("%s(" FMT ")\n", __FUNCTION__, ##__VA_ARGS__)
+#else // DEBUG
+#define DEBUG_CALL(...)
+#endif // DEBUG
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2017 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 "report.h"
+
+#include <string.h>
+#include <stdio.h>
+
+
+typedef enum {
+ MOVE_TYPE_NULL, // null move - does a no-op
+ MOVE_TYPE_LINE, // acceleration planned line
+ MOVE_TYPE_DWELL, // delay with no movement
+} move_type_t;
+
+
+typedef struct {
+ // Runtime
+ bool busy;
+ bool requesting;
+ float dwell;
+
+ // Move prep
+ bool move_ready; // prepped move ready for loader
+ bool move_queued; // prepped move queued
+ move_type_t move_type;
+ float prep_dwell;
+ uint16_t clock_period;
+} stepper_t;
+
+
+static stepper_t st = {0};
+
+
+void stepper_init() {
+ // Motor enable
+ OUTSET_PIN(MOTOR_ENABLE_PIN); // Low (disabled)
+ DIRSET_PIN(MOTOR_ENABLE_PIN); // Output
+
+ // 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 rate
+ TIMER_STEP.CTRLA = STEP_TIMER_ENABLE; // start step timer
+
+ st.clock_period = STEP_TIMER_POLL;
+}
+
+
+void st_shutdown() {
+ OUTCLR_PIN(MOTOR_ENABLE_PIN);
+ st.dwell = 0;
+ st.move_type = MOVE_TYPE_NULL;
+}
+
+
+void st_enable() {
+ if (!estop_triggered()) OUTSET_PIN(MOTOR_ENABLE_PIN); // Active high
+ report_request();
+}
+
+
+/// 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(STEP_LOW_LEVEL_ISR) {
+ 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 "software" interrupt to trigger next move exec as low interrupt.
+ ADCB_CH0_INTCTRL = ADC_CH_INTLVL_LO_gc;
+ ADCB_CTRLA = ADC_ENABLE_bm | ADC_CH0START_bm;
+}
+
+
+static void _end_move() {
+ for (int motor = 0; motor < MOTORS; motor++)
+ motor_end_move(motor);
+}
+
+
+/// Dwell or dequeue and load next move.
+static void _load_move() {
+ // Check EStop
+ if (estop_triggered()) {
+ st.move_type = MOVE_TYPE_NULL;
+ _end_move();
+ return;
+ }
+
+ // New clock period
+ TIMER_STEP.PER = st.clock_period;
+
+ // Dwell
+ if (0 < st.dwell) {
+ st.dwell -= 0.001; // 1ms
+ return;
+ } else st.dwell = 0;
+
+ // If the next move is not ready try to load it
+ if (!st.move_ready) {
+ _request_exec_move();
+ _end_move();
+ return;
+ }
+
+ // Start move
+ if (st.move_type == MOVE_TYPE_LINE)
+ for (int motor = 0; motor < MOTORS; motor++)
+ motor_load_move(motor);
+
+ else _end_move();
+
+ if (st.move_type != MOVE_TYPE_NULL) {
+ st.busy = true;
+
+ // Start dwell
+ st.dwell = st.prep_dwell;
+ }
+
+ // We are done with this move
+ st.move_type = MOVE_TYPE_NULL;
+ st.prep_dwell = 0; // clear dwell
+ st.move_ready = false; // flip the flag back
+ st.clock_period = STEP_TIMER_POLL;
+
+ // 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();
+}
+
+
+/// Step timer interrupt routine.
+ISR(STEP_TIMER_ISR) {
+ _load_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.
+ */
+stat_t st_prep_line(float time, const float target[]) {
+ // Trap conditions that would prevent queueing the line
+ ASSERT(!st.move_ready);
+
+ // Setup segment parameters
+ st.move_type = MOVE_TYPE_LINE;
+ st.clock_period = round(time * STEP_TIMER_FREQ * 60);
+
+ // Prepare motor moves
+ for (int motor = 0; motor < MOTORS; motor++) {
+ ASSERT(isfinite(target[motor]));
+ motor_prep_move(motor, time, round(target[motor]));
+ }
+
+ 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) {
+ ASSERT(!st.move_ready);
+ st.move_type = MOVE_TYPE_DWELL;
+ st.prep_dwell = seconds;
+ st.move_queued = true; // signal prep buffer ready
+}
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2017 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();
+void st_enable();
+bool st_is_busy();
+stat_t st_prep_line(float time, const float target[]);
+void st_prep_dwell(float seconds);
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2017 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 "switch.h"
+#include "config.h"
+
+#include <avr/interrupt.h>
+
+#include <stdbool.h>
+
+
+typedef struct {
+ uint8_t pin;
+ switch_type_t type;
+
+ switch_callback_t cb;
+ bool state;
+ bool triggered;
+} switch_t;
+
+
+// Order must match indices in var functions below
+static switch_t switches[SWITCHES] = {
+ {.pin = MIN_X_PIN, .type = SW_DISABLED},
+ {.pin = MAX_X_PIN, .type = SW_DISABLED},
+ {.pin = MIN_Y_PIN, .type = SW_DISABLED},
+ {.pin = MAX_Y_PIN, .type = SW_DISABLED},
+ {.pin = MIN_Z_PIN, .type = SW_DISABLED},
+ {.pin = MAX_Z_PIN, .type = SW_DISABLED},
+ {.pin = MIN_A_PIN, .type = SW_DISABLED},
+ {.pin = MAX_A_PIN, .type = SW_DISABLED},
+ {.pin = ESTOP_PIN, .type = SW_DISABLED},
+ {.pin = PROBE_PIN, .type = SW_DISABLED},
+};
+
+
+static bool _read_state(const switch_t *s) {return IN_PIN(s->pin);}
+
+
+static void _switch_isr() {
+ for (int i = 0; i < SWITCHES; i++) {
+ switch_t *s = &switches[i];
+ if (s->type == SW_DISABLED || s->triggered) continue;
+ s->triggered = _read_state(s) != s->state;
+ if (s->triggered) 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) {
+ s->triggered = false;
+ s->state = _read_state(s); // Initialize state
+ PORT(s->pin)->INT0MASK |= BM(s->pin); // Enable INT0
+
+ } 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->triggered) continue;
+
+ bool state = _read_state(s);
+ s->triggered = false;
+ PORT(s->pin)->INT0MASK |= BM(s->pin); // Reenable INT0
+
+ if (state != s->state) {
+ s->state = state;
+ if (s->cb) s->cb(i, switch_is_active(i));
+ }
+ }
+}
+
+
+bool switch_is_active(int index) {
+ switch (switches[index].type) {
+ case SW_DISABLED: break; // A disabled switch cannot be active
+ case SW_NORMALLY_OPEN: return !switches[index].state;
+ case SW_NORMALLY_CLOSED: return switches[index].state;
+ }
+ return false;
+}
+
+
+bool switch_is_enabled(int index) {
+ return switch_get_type(index) != SW_DISABLED;
+}
+
+
+switch_type_t switch_get_type(int index) {
+ return (index < 0 || SWITCHES <= index) ? SW_DISABLED : switches[index].type;
+}
+
+
+void switch_set_type(int index, switch_type_t type) {
+ if (index < 0 || SWITCHES <= index) return;
+ 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_min_sw_mode(int index) {return switch_get_type(MIN_SWITCH(index));}
+
+
+void set_min_sw_mode(int index, uint8_t value) {
+ switch_set_type(MIN_SWITCH(index), value);
+}
+
+
+uint8_t get_max_sw_mode(int index) {return switch_get_type(MAX_SWITCH(index));}
+
+
+void set_max_sw_mode(int index, uint8_t value) {
+ switch_set_type(MAX_SWITCH(index), value);
+}
+
+
+uint8_t get_estop_mode() {return switch_get_type(SW_ESTOP);}
+void set_estop_mode(uint8_t value) {switch_set_type(SW_ESTOP, value);}
+uint8_t get_probe_mode() {return switch_get_type(SW_PROBE);}
+void set_probe_mode(uint8_t value) {switch_set_type(SW_PROBE, value);}
+
+
+static uint8_t _get_state(int index) {
+ if (!switch_is_enabled(index)) return 2; // Disabled
+ return switches[index].state;
+}
+
+
+uint8_t get_min_switch(int index) {return _get_state(MIN_SWITCH(index));}
+uint8_t get_max_switch(int index) {return _get_state(MAX_SWITCH(index));}
+uint8_t get_estop_switch() {return _get_state(SW_ESTOP);}
+uint8_t get_probe_switch() {return _get_state(SW_PROBE);}
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2017 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 - 2017 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 - 2017 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 - 2017 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"
+
+#include <stdint.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 x) {
+ // evil floating point bit level hacking
+ union {
+ float f;
+ int32_t i;
+ } u;
+
+ const float xhalf = x * 0.5f;
+ u.f = x;
+ u.i = 0x5f3759df - (u.i >> 1); // what the fuck?
+ u.f = u.f * (1.5f - xhalf * u.f * u.f); // 1st iteration
+ u.f = u.f * (1.5f - xhalf * u.f * u.f); // 2nd iteration, can be removed
+
+ return u.f;
+}
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2017 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 static float min(float a, float b) {return a < b ? a : b;}
+inline static float max(float a, float b) {return a < b ? b : a;}
+inline static float min3(float a, float b, float c) {return min(min(a, b), c);}
+inline static float max3(float a, float b, float c) {return max(max(a, b), c);}
+inline static float min4(float a, float b, float c, float d)
+{return min(min(a, b), min(c, d));}
+inline static float max4(float a, float b, float c, float d)
+{return max(max(a, b), max(c, d));}
+
+float invsqrt(float number);
+
+#ifndef __AVR__
+inline static float square(float x) {return x * x;}
+#endif
+
+// Floating-point utilities
+#define EPSILON 0.00001 // allowable rounding error for floats
+inline static bool fp_EQ(float a, float b) {return fabs(a - b) < EPSILON;}
+inline static bool fp_NE(float a, float b) {return fabs(a - b) > EPSILON;}
+inline static bool fp_ZERO(float a) {return fabs(a) < EPSILON;}
+inline static bool fp_FALSE(float a) {return fp_ZERO(a);}
+inline static 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
+
+// 24bit integers
+#ifdef __AVR__
+typedef __int24 int24_t;
+typedef __uint24 uint24_t;
+#else
+typedef int32_t int24_t;
+typedef uint32_t uint24_t;
+#endif
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2017 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 "machine.h"
+#include "spindle.h"
+#include "coolant.h"
+#include "plan/runtime.h"
+#include "plan/state.h"
+
+// Axis
+float get_axis_mach_coord(int axis) {return mp_runtime_get_axis_position(axis);}
+
+
+void set_axis_mach_coord(int axis, float position) {
+ mach_set_axis_position(axis, position);
+}
+
+
+float get_axis_work_coord(int axis) {return mp_runtime_get_work_position(axis);}
+
+
+// GCode getters
+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();}
+
+
+// GCode setters
+void set_unit(const char *units) {mach_set_units(gs_parse_units(units));}
+void set_speed(float speed) {spindle_set_speed(speed);}
+void set_feed(float feed) {mach_set_feed_rate(feed);}
+
+
+void set_tool(uint8_t tool) {
+ mp_runtime_set_tool(tool);
+ mach_select_tool(tool);
+}
+
+
+void set_feed_mode(const char *mode) {
+ mach_set_feed_mode(gs_parse_feed_mode(mode));
+}
+
+
+void set_plane(const char *plane) {mach_set_plane(gs_parse_plane(plane));}
+
+
+void set_coord_system(const char *cs) {
+ mach_set_coord_system(gs_parse_coord_system(cs));
+}
+
+
+void set_abs_override(bool enable) {mach_set_absolute_mode(enable);}
+
+
+void set_path_mode(const char *mode) {
+ mach_set_path_mode(gs_parse_path_mode(mode));
+}
+
+
+void set_distance_mode(const char *mode) {
+ mach_set_distance_mode(gs_parse_distance_mode(mode));
+}
+
+
+void set_arc_dist_mode(const char *mode) {
+ mach_set_arc_distance_mode(gs_parse_distance_mode(mode));
+}
+
+
+void set_feed_override(float value) {mach_set_feed_override(value);}
+void set_speed_override(float value) {mach_set_spindle_override(value);}
+void set_mist_coolant(bool enable) {coolant_set_mist(enable);}
+void set_flood_coolant(bool enable) {coolant_set_flood(enable);}
+
+
+// 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 - 2017 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 "axis.h"
+#include "pgmspace.h"
+
+#include <stdint.h>
+#include <stdbool.h>
+#include <stdio.h>
+#include <string.h>
+#include <stdlib.h>
+#include <ctype.h>
+#include <math.h>
+#include <inttypes.h>
+
+
+typedef uint16_t flags_t;
+typedef const char *string;
+typedef const 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);
+
+
+// Eq functions
+#define EQ_FUNC(TYPE) \
+ inline static bool var_eq_##TYPE(const TYPE a, const TYPE b) {return a == b;}
+MAP(EQ_FUNC, SEMI, flags_t, string, pstring, uint8_t, uint16_t, int32_t, char);
+
+
+// String
+static void var_print_string(string s) {printf_P(PSTR("\"%s\""), s);}
+static float var_string_to_float(string s) {return 0;}
+
+
+// Program string
+static void var_print_pstring(pstring s) {printf_P(PSTR("\"%"PRPSTR"\""), s);}
+static const char *var_parse_pstring(const char *value) {return value;}
+static float var_pstring_to_float(pstring s) {return 0;}
+
+
+// Flags
+static void var_print_flags_t(flags_t x) {
+ extern void print_status_flags(flags_t x);
+ print_status_flags(x);
+}
+
+static float var_flags_t_to_float(flags_t x) {return x;}
+
+
+// Float
+static bool var_eq_float(float a, float b) {
+ return a == b || (isnan(a) && isnan(b));
+}
+
+
+static void var_print_float(float x) {
+ if (isnan(x)) printf_P(PSTR("\"nan\""));
+ else if (isinf(x)) printf_P(PSTR("\"%cinf\""), x < 0 ? '-' : '+');
+
+ else {
+ char buf[20];
+
+ int len = sprintf_P(buf, PSTR("%.3f"), 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);}
+static float var_float_to_float(float x) {return x;}
+
+
+// Bool
+inline static bool var_eq_bool(float a, float b) {return a == b;}
+static void var_print_bool(bool x) {printf_P(x ? PSTR("true") : PSTR("false"));}
+
+
+bool var_parse_bool(const char *value) {
+ return !strcasecmp(value, "true") || var_parse_float(value);
+}
+
+static float var_bool_to_float(bool x) {return x;}
+
+
+// Char
+#if 0
+static void var_print_char(char x) {putchar('"'); putchar(x); putchar('"');}
+static char var_parse_char(const char *value) {return value[1];}
+static float var_char_to_float(char x) {return x;}
+#endif
+
+
+// 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 float var_int8_t_to_float(int8_t x) {return x;}
+#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 float var_uint8_t_to_float(uint8_t x) {return x;}
+
+
+// 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 float var_uint16_t_to_float(uint16_t x) {return x;}
+
+
+// int32
+static void var_print_int32_t(int32_t x) {printf_P(PSTR("%"PRIi32), x);}
+static float var_int32_t_to_float(int32_t x) {return x;}
+
+
+// Ensure no code is used more than once
+enum {
+#define VAR(NAME, CODE, ...) var_code_##CODE,
+#include "vars.def"
+#undef VAR
+ var_code_count
+};
+
+// 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 & help
+#define VAR(NAME, CODE, TYPE, INDEX, SET, REPORT, HELP) \
+ static const char NAME##_name[] PROGMEM = #NAME; \
+ static const char NAME##_help[] PROGMEM = HELP;
+
+#include "vars.def"
+#undef VAR
+
+// Last value
+#define VAR(NAME, CODE, TYPE, INDEX, ...) \
+ static TYPE NAME##_state IF(INDEX)([INDEX]);
+
+#include "vars.def"
+#undef VAR
+
+// Report
+static uint8_t _report_var[(var_code_count >> 3) + 1] = {0,};
+
+
+static bool _get_report_var(int index) {
+ return _report_var[index >> 3] & (1 << (index & 7));
+}
+
+
+static void _set_report_var(int index, bool enable) {
+ if (enable) _report_var[index >> 3] |= 1 << (index & 7);
+ else _report_var[index >> 3] &= ~(1 << (index & 7));
+}
+
+
+static int _find_code(const char *code) {
+#define VAR(NAME, CODE, TYPE, INDEX, ...) \
+ if (!strcmp(code, #CODE)) return var_code_##CODE; \
+
+#include "vars.def"
+#undef VAR
+ return -1;
+}
+
+
+void vars_init() {
+ // Initialize var state
+#define VAR(NAME, CODE, TYPE, INDEX, ...) \
+ IF(INDEX)(for (int i = 0; i < INDEX; i++)) \
+ (NAME##_state)IF(INDEX)([i]) = get_##NAME(IF(INDEX)(i));
+
+#include "vars.def"
+#undef VAR
+
+// Report
+#define VAR(NAME, CODE, TYPE, INDEX, SET, REPORT, ...) \
+ _set_report_var(var_code_##CODE, REPORT);
+
+#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]); \
+ bool report = _get_report_var(var_code_##CODE); \
+ \
+ if ((report && !var_eq_##TYPE(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);
+}
+
+
+void vars_report_all(bool enable) {
+#define VAR(NAME, CODE, TYPE, INDEX, SET, REPORT, ...) \
+ _set_report_var(var_code_##CODE, enable);
+
+#include "vars.def"
+#undef VAR
+}
+
+
+void vars_report_var(const char *code, bool enable) {
+ int index = _find_code(code);
+ if (index != -1) _set_report_var(index, enable);
+}
+
+
+static char *_resolve_name(const char *_name) {
+ unsigned len = strlen(_name);
+
+ if (!len || 4 < len) return 0;
+
+ static char name[5];
+ strncpy(name, _name, 4);
+ name[4] = 0;
+
+ // Handle axis to motor mapping
+ if (2 < len && name[1] == '.') {
+ int axis = axis_get_id(name[0]);
+ if (axis < 0) return 0;
+ int motor = axis_get_motor(axis);
+ if (motor < 0) return 0;
+
+ name[0] = MOTORS_LABEL[motor];
+ for (int i = 1; _name[i]; i++)
+ name[i] = _name[i + 1];
+ }
+
+ return name;
+}
+
+
+bool vars_print(const char *_name) {
+ char *name = _resolve_name(_name);
+ if (!name) return false;
+
+ int i;
+#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); \
+ \
+ printf("{\"%s\":", _name); \
+ 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) {
+ char *name = _resolve_name(_name);
+ if (!name) return false;
+
+ int i;
+#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;
+}
+
+
+float vars_get_number(const char *_name) {
+ char *name = _resolve_name(_name);
+ if (!name) return 0;
+
+ int i;
+#define VAR(NAME, CODE, TYPE, INDEX, SET, ...) \
+ if (!strcmp(IF_ELSE(INDEX)(name + 1, name), #CODE)) { \
+ IF(INDEX) \
+ (i = strchr(INDEX##_LABEL, name[0]) - INDEX##_LABEL; \
+ if (INDEX <= i) return 0); \
+ \
+ TYPE x = get_##NAME(IF(INDEX)(i)); \
+ return var_##TYPE##_to_float(x); \
+ } \
+
+#include "vars.def"
+#undef VAR
+
+ return 0;
+}
+
+
+int vars_parser(char *vars) {
+ if (!*vars || *vars != '{') return STAT_OK;
+ vars++;
+
+ 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 %-20"PRPSTR" %-16"PRPSTR" %"PRPSTR"\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);
+}
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2017 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 OUTS_LABEL "ed12f"
+
+// VAR(name, code, type, index, settable, report, help)
+
+// Motor
+VAR(motor_axis, an, uint8_t, MOTORS, 1, 1, "Maps motor to axis")
+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_current, dc, float, MOTORS, 1, 1, "Max motor drive current")
+VAR(idle_current, ic, float, MOTORS, 1, 1, "Motor idle current")
+VAR(active_current, ac, float, MOTORS, 0, 1, "Motor current now")
+VAR(driver_flags, df, uint16_t, MOTORS, 0, 1, "Motor driver status flags")
+VAR(status_strings, ds, flags_t, MOTORS, 0, 1, "Motor driver status strings")
+VAR(encoder, en, int32_t, MOTORS, 0, 0, "Motor encoder")
+VAR(error, ee, int32_t, MOTORS, 0, 0, "Motor position error")
+
+VAR(motor_fault, fa, bool, 0, 0, 1, "Motor fault status")
+
+VAR(velocity_max, vm, float, MOTORS, 1, 1, "Maxium velocity in mm/min")
+VAR(jerk_max, jm, float, MOTORS, 1, 1, "Maxium jerk in mm/min^3")
+VAR(radius, ra, float, MOTORS, 1, 1, "Axis radius or zero")
+
+// Switches
+VAR(travel_min, tn, float, MOTORS, 1, 1, "Minimum soft limit")
+VAR(travel_max, tm, float, MOTORS, 1, 1, "Maximum soft limit")
+VAR(min_sw_mode, ls, uint8_t, MOTORS, 1, 1, "Minimum switch mode")
+VAR(max_sw_mode, xs, uint8_t, MOTORS, 1, 1, "Maximum switch mode")
+VAR(estop_mode, et, uint8_t, 0, 1, 1, "Estop switch mode")
+VAR(probe_mode, pt, uint8_t, 0, 1, 1, "Probe switch mode")
+VAR(min_switch, lw, uint8_t, MOTORS, 0, 1, "Minimum switch state")
+VAR(max_switch, xw, uint8_t, MOTORS, 0, 1, "Maximum switch state")
+VAR(estop_switch, ew, uint8_t, 0, 0, 1, "Estop switch state")
+VAR(probe_switch, pw, uint8_t, 0, 0, 1, "Probe switch state")
+
+// Homing
+VAR(homing_mode, ho, uint8_t, MOTORS, 1, 1, "Homing type")
+VAR(homing_dir, hd, float, MOTORS, 0, 1, "Homing direction")
+VAR(home, hp, float, MOTORS, 0, 1, "Home position")
+VAR(homed, h, bool, MOTORS, 1, 1, "True if axis is homed")
+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 backoff")
+VAR(zero_backoff, zb, float, MOTORS, 1, 1, "Homing zero backoff")
+
+// Axis
+VAR(axis_mach_coord, p, float, AXES, 1, 1, "Axis machine coordinate")
+VAR(axis_work_coord, w, float, AXES, 0, 1, "Axis work coordinate")
+VAR(axis_can_home, ch, bool, AXES, 0, 1, "Is axis configured for homing")
+
+// Outputs
+VAR(output_state, os, uint8_t, OUTS, 0, 1, "Output pin state")
+VAR(output_mode, om, uint8_t, OUTS, 1, 1, "Output pin mode")
+
+// Spindle
+VAR(spindle_type, st, uint8_t, 0, 1, 1, "PWM=0 or HUANYANG=1")
+VAR(spin_reversed, 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_duty, nd, float, 0, 1, 1, "Minimum PWM duty cycle")
+VAR(spin_max_duty, md, float, 0, 1, 1, "Maximum PWM duty cycle")
+VAR(spin_up, su, float, 0, 1, 1, "Spin up velocity")
+VAR(spin_down, sd, float, 0, 1, 1, "Spin down velocity")
+VAR(spin_freq, sf, uint16_t, 0, 1, 1, "Spindle PWM frequency")
+VAR(spin_mode, ss, pstring, 0, 0, 1, "Spindle mode")
+
+// PWM spindle
+VAR(pwm_invert, pi, bool, 0, 1, 1, "Inverted spindle PWM")
+
+// Huanyang spindle
+VAR(huanyang_id, hi, uint8_t, 0, 1, 1, "Huanyang ID")
+VAR(huanyang_freq, hz, float, 0, 0, 1, "Huanyang actual freq")
+VAR(huanyang_current, hc, float, 0, 0, 1, "Huanyang actual current")
+VAR(huanyang_rpm, hr, uint16_t, 0, 0, 1, "Huanyang actual RPM")
+VAR(huanyang_temp, ht, uint16_t, 0, 0, 1, "Huanyang temperature")
+VAR(huanyang_max_freq, hx, float, 0, 0, 1, "Huanyang max freq")
+VAR(huanyang_min_freq, hm, float, 0, 0, 1, "Huanyang min freq")
+VAR(huanyang_rated_rpm, hq, uint16_t, 0, 0, 1, "Huanyang rated RPM")
+VAR(huanyang_status, hs, uint8_t, 0, 0, 1, "Huanyang status flags")
+VAR(huanyang_debug, hb, bool, 0, 1, 1, "Huanyang debugging")
+VAR(huanyang_connected, he, bool, 0, 0, 1, "Huanyang connected")
+
+// GCode
+VAR(line, ln, int32_t, 0, 0, 1, "Last GCode line executed")
+VAR(unit, u, pstring, 0, 1, 1, "Current unit of measure")
+VAR(speed, s, float, 0, 1, 1, "Current spindle speed")
+VAR(feed, f, float, 0, 1, 1, "Current feed rate")
+VAR(tool, t, uint8_t, 0, 1, 1, "Current tool")
+VAR(feed_mode, fm, pstring, 0, 1, 1, "Current feed rate mode")
+VAR(plane, pa, pstring, 0, 1, 1, "Current plane")
+VAR(coord_system, cs, pstring, 0, 1, 1, "Current coordinate system")
+VAR(abs_override, ao, bool, 0, 1, 1, "Absolute override enabled")
+VAR(path_mode, pc, pstring, 0, 1, 1, "Current path control mode")
+VAR(distance_mode, dm, pstring, 0, 1, 1, "Current distance mode")
+VAR(arc_dist_mode, ad, pstring, 0, 1, 1, "Current arc distance mode")
+VAR(feed_override, fo, float, 0, 1, 1, "Feed rate override")
+VAR(speed_override, so, float, 0, 1, 1, "Spindle speed override")
+VAR(mist_coolant, mc, bool, 0, 1, 1, "Mist coolant enabled")
+VAR(flood_coolant, fc, bool, 0, 1, 1, "Flood coolant enabled")
+
+// System
+VAR(velocity, v, float, 0, 0, 1, "Current velocity")
+VAR(hw_id, id, string, 0, 0, 1, "Hardware ID")
+VAR(echo, ec, bool, 0, 1, 1, "Enable or disable echo")
+VAR(estop, es, bool, 0, 1, 1, "Emergency stop")
+VAR(estop_reason, er, pstring, 0, 0, 1, "Emergency stop reason")
+VAR(state, x, pstring, 0, 0, 1, "Machine state")
+VAR(cycle, c, pstring, 0, 0, 1, "Machine cycle")
+VAR(hold_reason, pr, pstring, 0, 0, 1, "Machine pause reason")
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2017 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 var_parse_bool(const char *value);
+
+void vars_init();
+
+void vars_report(bool full);
+void vars_report_all(bool enable);
+void vars_report_var(const char *code, bool enable);
+bool vars_print(const char *name);
+bool vars_set(const char *name, const char *value);
+float vars_get_number(const char *name);
+int vars_parser(char *vars);
+void vars_print_help();
--- /dev/null
+#include "cpp_magic.h"
+{
+#define VAR(NAME, CODE, TYPE, INDEX, SET, REPORT, HELP) \
+#CODE: { \
+ "name": #NAME, \
+ "type": #TYPE, \
+ "index": IF_ELSE(INDEX)(true, false), \
+ "setable": IF_ELSE(SET)(true, false), \
+ "report": IF_ELSE(REPORT)(true, false), \
+ "help": HELP \
+ },
+#include "vars.def"
+#undef VAR
+ "_": {}
+}
--- /dev/null
+TESTS=planner-test
+
+PLANNER_TEST_SRC = gcode_parser.c machine.c status.c util.c axis.c report.c \
+ homing.c probing.c command.c vars.c
+PLANNER_TEST_SRC := $(patsubst %,../src/%,$(PLANNER_TEST_SRC))
+PLANNER_TEST_SRC += $(wildcard ../src/plan/*.c) planner-test.c hal.c
+
+CFLAGS = -I../src -Wall -Werror -DDEBUG -g -std=gnu99
+CFLAGS += -MD -MP -MT $@ -MF .dep/$(@F).d
+CFLAGS += -DF_CPU=320000000
+LDFLAGS = -lm
+
+all: $(TESTS)
+
+planner-test: $(PLANNER_TEST_SRC)
+ gcc -o $@ $(PLANNER_TEST_SRC) $(CFLAGS) $(LDFLAGS)
+
+%.csv: %.gc planner-test
+ ./planner-test < $< | grep -E '^-?[0-9.]+,'
+
+%-test: %.gc planner-test
+ ./planner-test < $<
+
+%-plot: %.gc planner-test
+ ./planner-test < $< | grep -E '^-?[0-9.]+,' | ./plot.py
+
+# Clean
+tidy:
+ rm -f $(shell find -name \*~ -o -name \#\*) .dep
+
+clean: tidy
+ rm -rf $(TESTS)
+
+.PHONY: tidy clean all
+
+# Dependencies
+-include $(shell mkdir -p .dep) $(wildcard .dep/*)
--- /dev/null
+$resume
+
+$0vm=10000
+$1vm=10000
+$2vm=10000
+$0jm=50
+$1jm=50
+$2jm=50
+
+G3 I2 J2 F10000
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2017 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 "spindle.h"
+#include "i2c.h"
+#include "cpp_magic.h"
+#include "plan/buffer.h"
+
+#include <stdint.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <math.h>
+
+
+typedef uint8_t flags_t;
+typedef const char *string;
+typedef const char *pstring;
+
+
+#define VAR(NAME, CODE, TYPE, INDEX, SET, ...) \
+ TYPE get_##NAME(IF(INDEX)(int index)) __attribute__((weak)); \
+ TYPE get_##NAME(IF(INDEX)(int index)) { \
+ DEBUG_CALL(); \
+ return 0; \
+ } \
+ IF(SET) \
+ (void set_##NAME(IF(INDEX)(int index,) TYPE value) __attribute__((weak)); \
+ void set_##NAME(IF(INDEX)(int index,) TYPE value) { \
+ DEBUG_CALL(); \
+ })
+
+#include "vars.def"
+#undef VAR
+
+
+void command_mreset(int argc, char *argv[]) {}
+void command_home(int argc, char *argv[]) {}
+void i2c_set_read_callback(i2c_read_cb_t cb) {}
+void print_status_flags(uint8_t flags) {DEBUG_CALL();}
+uint8_t hw_disable_watchdog() {return 0;}
+void hw_restore_watchdog(uint8_t state) {}
+
+
+bool estop = false;
+
+
+void estop_trigger(stat_t reason) {
+ DEBUG_CALL("%s", status_to_pgmstr(reason));
+ mp_queue_dump();
+ estop = true;
+ abort();
+}
+
+
+void estop_clear() {
+ DEBUG_CALL();
+ estop = false;
+}
+
+
+bool estop_triggered() {return estop;}
+
+
+void hw_request_hard_reset() {
+ DEBUG_CALL();
+ exit(0);
+}
+
+
+bool usart_tx_empty() {return true;}
+bool usart_tx_full() {return false;}
+
+
+char *usart_readline() {
+ static char *cmd = 0;
+
+ if (cmd) {
+ free(cmd);
+ cmd = 0;
+ }
+
+ size_t n = 0;
+ if (getline(&cmd, &n, stdin) == -1) {
+ free(cmd);
+ cmd = 0;
+ }
+
+ return cmd;
+}
+
+
+void coolant_init() {}
+
+
+void coolant_set_mist(bool x) {
+ DEBUG_CALL("%s", x ? "true" : "false");
+}
+
+
+void coolant_set_flood(bool x) {
+ DEBUG_CALL("%s", x ? "true" : "false");
+}
+
+
+void spindle_init() {}
+
+
+void spindle_set_speed(float speed) {
+ DEBUG_CALL("%f", speed);
+}
+
+
+void spindle_set_mode(spindle_mode_t mode) {
+ DEBUG_CALL("%d", mode);
+}
+
+
+void spindle_stop() {}
+
+
+void motor_set_position(int motor, int32_t position) {
+ DEBUG_CALL("%d, %d", motor, position);
+}
+
+
+bool switch_is_active(int index) {
+ DEBUG_CALL("%d", index);
+ return false;
+}
+
+
+bool switch_is_enabled(int index) {
+ DEBUG_CALL("%d", index);
+ return false;
+}
+
+
+static uint32_t ticks = 0;
+
+
+uint32_t rtc_get_time() {return ticks;}
+
+
+bool rtc_expired(uint32_t t) {
+ return true;
+ return 0 <= (int32_t)(ticks - t);
+}
+
+
+bool motor_is_enabled(int motor) {return true;}
+int motor_get_axis(int motor) {return motor;}
+
+
+#define MICROSTEPS 32
+#define TRAVEL_REV 5
+#define STEP_ANGLE 1.8
+
+
+float motor_position[MOTORS] = {0};
+
+
+float motor_get_steps_per_unit(int motor) {
+ return 360 * MICROSTEPS / TRAVEL_REV / STEP_ANGLE;
+}
+
+
+int32_t motor_get_encoder(int motor) {
+ DEBUG_CALL("%d", motor);
+ return 0;
+}
+
+
+void motor_end_move(int motor) {
+ DEBUG_CALL("%d", motor);
+}
+
+
+int32_t motor_get_error(int motor) {return 0;}
+int32_t motor_get_position(int motor) {return motor_position[motor];}
+
+
+bool st_is_busy() {return false;}
+
+
+float square(float x) {return x * x;}
+
+
+stat_t st_prep_line(float time, const float target[]) {
+ DEBUG_CALL("%f, (%f, %f, %f, %f)",
+ time, target[0], target[1], target[2], target[3]);
+
+ float p[MOTORS] = {0, 0, 0, 0};
+
+ for (int i = 0; i < MOTORS; i++) {
+ motor_position[i] = target[i];
+ p[i] = target[i] / motor_get_steps_per_unit(i);
+ }
+
+ printf("%0.10f, %0.10f, %0.10f, %0.10f\n", time, p[0], p[1], p[2]);
+
+ return STAT_OK;
+}
+
+
+void st_prep_dwell(float seconds) {
+ DEBUG_CALL("%f", seconds);
+}
--- /dev/null
+$resume
+
+$0vm=10000
+$1vm=10000
+$2vm=10000
+$0jm=50
+$1jm=50
+$2jm=50
+
+G0 X500Y500
+G0 X0Y0
+G0 X-500Y0
+G0 X-500Y-500
+
+G0 Z-100
+G0 Z0
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2017 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 "machine.h"
+#include "command.h"
+#include "plan/arc.h"
+#include "plan/planner.h"
+#include "plan/exec.h"
+#include "plan/state.h"
+
+#include <stdio.h>
+#include <stdlib.h>
+
+
+int main(int argc, char *argv[]) {
+ mp_init(); // motion planning
+ machine_init(); // gcode machine
+ for (int i = 0; i < 4; i++) axis_set_motor(i, i);
+
+ stat_t status = STAT_OK;
+
+ while (true) {
+ mach_arc_callback(); // arc generation runs
+
+ bool reading = !feof(stdin);
+
+ if (reading && mp_queue_get_room()) {
+ mp_state_callback();
+ command_callback();
+
+ if (mp_queue_get_room()) continue;
+ }
+
+ status = mp_exec_move();
+ printf("EXEC: %s\n", status_to_pgmstr(status));
+
+ switch (status) {
+ case STAT_NOOP: 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;
+ continue;
+
+ default:
+ printf("ERROR: %s\n", status_to_pgmstr(status));
+ }
+
+ if (!reading) break;
+ }
+
+ printf("STATE: %s\n", mp_get_state_pgmstr(mp_get_state()));
+
+ return 0;
+}
--- /dev/null
+#!/usr/bin/env python3
+
+import sys
+import csv
+import matplotlib.pyplot as plt
+import numpy as np
+import math
+
+
+def compute_velocity(rows):
+ p = [0, 0, 0]
+
+ for row in rows:
+ t = float(row[0])
+ d = 0.0
+
+ for i in range(3):
+ x = float(row[i + 1]) - p[i]
+ d += x * x
+ p[i] = float(row[i + 1])
+
+ d = math.sqrt(d)
+
+ yield d / t
+
+
+time_step = 0.005
+
+data = list(csv.reader(sys.stdin))
+
+velocity = list(compute_velocity(data))
+acceleration = np.diff(velocity)
+jerk = np.diff(acceleration)
+
+t = np.cumsum([float(row[0]) for row in data])
+x = [float(row[1]) for row in data]
+y = [float(row[2]) for row in data]
+z = [float(row[3]) for row in data]
+
+rows = 3
+row = 1
+if np.sum(x): rows += 1
+if np.sum(y): rows += 1
+if np.sum(z): rows += 1
+
+def next_subplot():
+ global row
+ plt.subplot(rows * 100 + 10 + row)
+ row += 1
+
+if np.sum(x):
+ next_subplot()
+ plt.title('X')
+ plt.plot(t, x, 'r')
+ plt.ylabel(r'$mm$')
+
+if np.sum(y):
+ next_subplot()
+ plt.title('X')
+ plt.title('Y')
+ plt.plot(t, [row[2] for row in data], 'g')
+ plt.ylabel(r'$mm$')
+
+if np.sum(z):
+ next_subplot()
+ plt.title('Z')
+ plt.plot(t, [row[3] for row in data], 'b')
+ plt.ylabel(r'$mm$')
+
+next_subplot()
+plt.title('Velocity')
+plt.plot(t, velocity, 'g')
+plt.ylabel(r'$\frac{mm}{min}$')
+
+next_subplot()
+plt.title('Acceleration')
+plt.plot(t[1:], acceleration, 'b')
+plt.ylabel(r'$\frac{mm}{min^2}$')
+
+next_subplot()
+plt.title('Jerk')
+plt.plot(t[2:], jerk, 'r')
+plt.ylabel(r'$\frac{mm}{min^3}$')
+plt.xlabel('Seconds')
+
+plt.tight_layout()
+plt.subplots_adjust(hspace = 0.5)
+mng = plt.get_current_fig_manager()
+mng.resize(*mng.window.maxsize())
+plt.show()
--- /dev/null
+$resume
+
+$0vm=10000
+$1vm=10000
+$2vm=10000
+$0jm=50
+$1jm=50
+$2jm=50
+
+G0 X0.0
+G0 X0.1
+G0 X0.2
+G0 X0.3
+G0 X0.4
+G0 X0.5
+G0 X0.6
+G0 X0.7
+G0 X0.8
+G0 X0.9
+G0 X1.0
+G0 X1.1
+G0 X1.2
+G0 X1.3
+G0 X1.4
+G0 X1.5
+G0 X1.6
+G0 X1.7
+G0 X1.8
+G0 X1.9
+G0 X2.0
+G0 X2.1
+G0 X2.2
+G0 X2.3
+G0 X2.4
+G0 X2.5
+G0 X2.6
+G0 X2.7
+G0 X2.8
+G0 X2.9
+G0 X3.0
+G0 X3.1
+G0 X3.2
+G0 X3.3
+G0 X3.4
+G0 X3.5
+G0 X3.6
+G0 X3.7
+G0 X3.8
+G0 X3.9
+G0 X4.0
+G0 X4.1
+G0 X4.2
+G0 X4.3
+G0 X4.4
+G0 X4.5
+G0 X4.6
+G0 X4.7
+G0 X4.8
+G0 X4.9
projects / bbctrl-firmware / commitdiff