From: Joseph Coffland Date: Fri, 21 Jul 2017 22:14:15 +0000 (-0700) Subject: Moved avr code to src X-Git-Url: https://git.buildbotics.com/?a=commitdiff_plain;h=7776302fbb1f076c842bf4bb8fd06e00ffa4999b;p=bbctrl-firmware Moved avr code to src --- diff --git a/MANIFEST.in b/MANIFEST.in index 0277ea2..9dec892 100644 --- a/MANIFEST.in +++ b/MANIFEST.in @@ -1,3 +1,3 @@ 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 diff --git a/Makefile b/Makefile index 1f21775..1f51761 100644 --- a/Makefile +++ b/Makefile @@ -17,7 +17,7 @@ STATIC := $(shell find src/resources -type f) 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)) @@ -47,7 +47,7 @@ pkg: all $(AVR_FIRMWARE) .PHONY: $(AVR_FIRMWARE) $(AVR_FIRMWARE): - $(MAKE) -C avr $(shell basename $@) + $(MAKE) -C src/avr $(shell basename $@) publish: pkg echo -n $(VERSION) > dist/latest.txt diff --git a/avr/.gitignore b/avr/.gitignore deleted file mode 100755 index bc78328..0000000 --- a/avr/.gitignore +++ /dev/null @@ -1,12 +0,0 @@ -# Backup files -*~ -\#* - -build -.dep - -/*.eep -/*.hex -/*.elf -/*.lss -/*.map diff --git a/avr/BezierMath.md b/avr/BezierMath.md deleted file mode 100644 index f9da75a..0000000 --- a/avr/BezierMath.md +++ /dev/null @@ -1,56 +0,0 @@ -# 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 diff --git a/avr/CODE_TAG b/avr/CODE_TAG deleted file mode 100644 index a0a7364..0000000 --- a/avr/CODE_TAG +++ /dev/null @@ -1,22 +0,0 @@ - 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 . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" diff --git a/avr/LICENSE b/avr/LICENSE deleted file mode 100644 index b08b657..0000000 --- a/avr/LICENSE +++ /dev/null @@ -1,380 +0,0 @@ -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 -. - -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. - - - Copyright (C) - - 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. - - , 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. diff --git a/avr/Makefile b/avr/Makefile deleted file mode 100644 index 9c6c0d2..0000000 --- a/avr/Makefile +++ /dev/null @@ -1,160 +0,0 @@ -# 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/*) diff --git a/avr/MoveLifecycle.md b/avr/MoveLifecycle.md deleted file mode 100644 index edb3101..0000000 --- a/avr/MoveLifecycle.md +++ /dev/null @@ -1,99 +0,0 @@ -# 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. diff --git a/avr/MoveLifecycleCalls.md b/avr/MoveLifecycleCalls.md deleted file mode 100644 index b982100..0000000 --- a/avr/MoveLifecycleCalls.md +++ /dev/null @@ -1,76 +0,0 @@ -# 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 diff --git a/avr/README.md b/avr/README.md deleted file mode 100644 index 24ba38a..0000000 --- a/avr/README.md +++ /dev/null @@ -1,22 +0,0 @@ -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 diff --git a/avr/data_usage.py b/avr/data_usage.py deleted file mode 100755 index 8ba92f0..0000000 --- a/avr/data_usage.py +++ /dev/null @@ -1,36 +0,0 @@ -#!/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) diff --git a/avr/src/axis.c b/avr/src/axis.c deleted file mode 100644 index 0697aaf..0000000 --- a/avr/src/axis.c +++ /dev/null @@ -1,197 +0,0 @@ -/******************************************************************************\ - - 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 . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - - -#include "axis.h" -#include "motor.h" -#include "plan/planner.h" - -#include -#include -#include - - -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)); -} diff --git a/avr/src/axis.h b/avr/src/axis.h deleted file mode 100644 index af7f658..0000000 --- a/avr/src/axis.h +++ /dev/null @@ -1,71 +0,0 @@ -/******************************************************************************\ - - 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 . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -#pragma once - -#include "config.h" - -#include - - -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); diff --git a/avr/src/boot/boot.c b/avr/src/boot/boot.c deleted file mode 100644 index a0ac310..0000000 --- a/avr/src/boot/boot.c +++ /dev/null @@ -1,448 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2017 Buildbotics LLC - Copyright (c) 2010 Alex Forencich - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -#include "boot.h" -#include "sp_driver.h" - -#include -#include - -#include -#include - -#include - - -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"); -} diff --git a/avr/src/boot/boot.h b/avr/src/boot/boot.h deleted file mode 100644 index 2af4be1..0000000 --- a/avr/src/boot/boot.h +++ /dev/null @@ -1,111 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2017 Buildbotics LLC - Copyright (c) 2010 Alex Forencich - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -#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 = '?', -}; diff --git a/avr/src/boot/sp_driver.S b/avr/src/boot/sp_driver.S deleted file mode 100644 index bbb1ee7..0000000 --- a/avr/src/boot/sp_driver.S +++ /dev/null @@ -1,403 +0,0 @@ -;****************************************************************************** -;* $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 - -; 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 diff --git a/avr/src/boot/sp_driver.h b/avr/src/boot/sp_driver.h deleted file mode 100644 index 01aeaee..0000000 --- a/avr/src/boot/sp_driver.h +++ /dev/null @@ -1,173 +0,0 @@ -/******************************************************************************* - * $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 - -#include - -#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(); diff --git a/avr/src/command.c b/avr/src/command.c deleted file mode 100644 index 3939751..0000000 --- a/avr/src/command.c +++ /dev/null @@ -1,331 +0,0 @@ -/******************************************************************************\ - - 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 . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -#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 -#endif - -#include -#include -#include -#include - - -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; -} diff --git a/avr/src/command.def b/avr/src/command.def deleted file mode 100644 index b11766f..0000000 --- a/avr/src/command.def +++ /dev/null @@ -1,36 +0,0 @@ -/******************************************************************************\ - - 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 . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -// Name Min, Max args, Help -CMD(help, 0, 1, "Print this help screen") -CMD(report, 1, 2, "[var] . 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") diff --git a/avr/src/command.h b/avr/src/command.h deleted file mode 100644 index 67397ac..0000000 --- a/avr/src/command.h +++ /dev/null @@ -1,50 +0,0 @@ -/******************************************************************************\ - - 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 . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -#pragma once - - -#include - - -#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(); diff --git a/avr/src/config.h b/avr/src/config.h deleted file mode 100644 index 91fa972..0000000 --- a/avr/src/config.h +++ /dev/null @@ -1,263 +0,0 @@ -/******************************************************************************\ - - 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 . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -#pragma once - -#include "pins.h" - -#ifdef __AVR__ -#include -#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 diff --git a/avr/src/coolant.c b/avr/src/coolant.c deleted file mode 100644 index 46ec802..0000000 --- a/avr/src/coolant.c +++ /dev/null @@ -1,45 +0,0 @@ -/******************************************************************************\ - - 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 . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -#include "coolant.h" -#include "config.h" - -#include - - -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);} diff --git a/avr/src/coolant.h b/avr/src/coolant.h deleted file mode 100644 index 0d9b074..0000000 --- a/avr/src/coolant.h +++ /dev/null @@ -1,37 +0,0 @@ -/******************************************************************************\ - - 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 . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -#pragma once - -#include - - -void coolant_init(); -void coolant_set_mist(bool x); -void coolant_set_flood(bool x); -bool coolant_get_mist(); -bool coolant_get_flood(); diff --git a/avr/src/cpp_magic.h b/avr/src/cpp_magic.h deleted file mode 100644 index c7abc59..0000000 --- a/avr/src/cpp_magic.h +++ /dev/null @@ -1,510 +0,0 @@ -/******************************************************************************\ - - 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 - diff --git a/avr/src/drv8711.c b/avr/src/drv8711.c deleted file mode 100644 index 18cd95d..0000000 --- a/avr/src/drv8711.c +++ /dev/null @@ -1,497 +0,0 @@ -/******************************************************************************\ - - 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 . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -#include "drv8711.h" -#include "status.h" -#include "stepper.h" -#include "report.h" - -#include -#include - -#include -#include -#include - - -#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(); -} diff --git a/avr/src/drv8711.h b/avr/src/drv8711.h deleted file mode 100644 index 8356322..0000000 --- a/avr/src/drv8711.h +++ /dev/null @@ -1,179 +0,0 @@ -/******************************************************************************\ - - 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 . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -#pragma once - - -#include "config.h" -#include "status.h" -#include "motor.h" - -#include -#include - -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); diff --git a/avr/src/estop.c b/avr/src/estop.c deleted file mode 100644 index 99ed0f5..0000000 --- a/avr/src/estop.c +++ /dev/null @@ -1,140 +0,0 @@ -/******************************************************************************\ - - 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 . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -#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 - - -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()); -} diff --git a/avr/src/estop.h b/avr/src/estop.h deleted file mode 100644 index 757e03f..0000000 --- a/avr/src/estop.h +++ /dev/null @@ -1,38 +0,0 @@ -/******************************************************************************\ - - 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 . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -#pragma once - -#include "status.h" - -#include - - -void estop_init(); -bool estop_triggered(); -void estop_trigger(stat_t reason); -void estop_clear(); diff --git a/avr/src/gcode_expr.c b/avr/src/gcode_expr.c deleted file mode 100644 index 8ac1a50..0000000 --- a/avr/src/gcode_expr.c +++ /dev/null @@ -1,296 +0,0 @@ -/******************************************************************************\ - - 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 . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -#include "gcode_expr.h" - -#include "gcode_parser.h" -#include "vars.h" - -#include -#include -#include - - -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(); -} diff --git a/avr/src/gcode_expr.h b/avr/src/gcode_expr.h deleted file mode 100644 index d7a963a..0000000 --- a/avr/src/gcode_expr.h +++ /dev/null @@ -1,33 +0,0 @@ -/******************************************************************************\ - - 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 . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -#pragma once - - -float parse_gcode_number(char **p); -float parse_gcode_expression(char **p); diff --git a/avr/src/gcode_parser.c b/avr/src/gcode_parser.c deleted file mode 100644 index 97fbe40..0000000 --- a/avr/src/gcode_parser.c +++ /dev/null @@ -1,518 +0,0 @@ -/******************************************************************************\ - - 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 . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -#include "gcode_parser.h" - -#include "gcode_expr.h" -#include "machine.h" -#include "plan/arc.h" -#include "axis.h" -#include "util.h" - -#include -#include -#include -#include -#include -#include - - -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(); -} diff --git a/avr/src/gcode_parser.h b/avr/src/gcode_parser.h deleted file mode 100644 index dc880ef..0000000 --- a/avr/src/gcode_parser.h +++ /dev/null @@ -1,87 +0,0 @@ -/******************************************************************************\ - - 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 . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -#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); diff --git a/avr/src/gcode_state.c b/avr/src/gcode_state.c deleted file mode 100644 index 7807e56..0000000 --- a/avr/src/gcode_state.c +++ /dev/null @@ -1,179 +0,0 @@ -/******************************************************************************\ - - 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 . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -#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; -} diff --git a/avr/src/gcode_state.def b/avr/src/gcode_state.def deleted file mode 100644 index 5d73cf4..0000000 --- a/avr/src/gcode_state.def +++ /dev/null @@ -1,69 +0,0 @@ -/******************************************************************************\ - - 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 . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -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 diff --git a/avr/src/gcode_state.h b/avr/src/gcode_state.h deleted file mode 100644 index 4dc2665..0000000 --- a/avr/src/gcode_state.h +++ /dev/null @@ -1,206 +0,0 @@ -/******************************************************************************\ - - 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 . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -#pragma once - -#include "config.h" -#include "pgmspace.h" - -#include -#include - - -/* 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); diff --git a/avr/src/hardware.c b/avr/src/hardware.c deleted file mode 100644 index bd3de2b..0000000 --- a/avr/src/hardware.c +++ /dev/null @@ -1,179 +0,0 @@ -/******************************************************************************\ - - 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 . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -#include "hardware.h" -#include "rtc.h" -#include "usart.h" -#include "huanyang.h" -#include "config.h" -#include "pgmspace.h" - -#include -#include -#include - -#include -#include - - -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; -} diff --git a/avr/src/hardware.h b/avr/src/hardware.h deleted file mode 100644 index 56ea39e..0000000 --- a/avr/src/hardware.h +++ /dev/null @@ -1,45 +0,0 @@ -/******************************************************************************\ - - 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 . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -#pragma once - -#include "status.h" - -#include - - -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); diff --git a/avr/src/huanyang.c b/avr/src/huanyang.c deleted file mode 100644 index f43bf11..0000000 --- a/avr/src/huanyang.c +++ /dev/null @@ -1,523 +0,0 @@ -/******************************************************************************\ - - 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 . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -#include "huanyang.h" -#include "config.h" -#include "rtc.h" -#include "report.h" -#include "pgmspace.h" - -#include -#include -#include - -#include -#include -#include - - -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;} diff --git a/avr/src/huanyang.h b/avr/src/huanyang.h deleted file mode 100644 index 1e52258..0000000 --- a/avr/src/huanyang.h +++ /dev/null @@ -1,37 +0,0 @@ -/******************************************************************************\ - - 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 . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -#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(); diff --git a/avr/src/i2c.c b/avr/src/i2c.c deleted file mode 100644 index 7ed4510..0000000 --- a/avr/src/i2c.c +++ /dev/null @@ -1,128 +0,0 @@ -/******************************************************************************\ - - 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 . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -#include "i2c.h" - -#include - -#include - - -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;} diff --git a/avr/src/i2c.h b/avr/src/i2c.h deleted file mode 100644 index 766396d..0000000 --- a/avr/src/i2c.h +++ /dev/null @@ -1,56 +0,0 @@ -/******************************************************************************\ - - 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 . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -#pragma once - -#include "config.h" - -#include - - -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); diff --git a/avr/src/machine.c b/avr/src/machine.c deleted file mode 100644 index 5c5be2c..0000000 --- a/avr/src/machine.c +++ /dev/null @@ -1,934 +0,0 @@ -/******************************************************************************\ - - 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 . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -/* 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); -} diff --git a/avr/src/machine.h b/avr/src/machine.h deleted file mode 100644 index 51e0480..0000000 --- a/avr/src/machine.h +++ /dev/null @@ -1,142 +0,0 @@ -/******************************************************************************\ - - 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 . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -#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(); diff --git a/avr/src/main.c b/avr/src/main.c deleted file mode 100644 index 6250827..0000000 --- a/avr/src/main.c +++ /dev/null @@ -1,95 +0,0 @@ -/******************************************************************************\ - - 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 . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -#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 - -#include -#include - - -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; -} diff --git a/avr/src/messages.def b/avr/src/messages.def deleted file mode 100644 index c69933b..0000000 --- a/avr/src/messages.def +++ /dev/null @@ -1,129 +0,0 @@ -/******************************************************************************\ - - 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 . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -// 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, "") diff --git a/avr/src/motor.c b/avr/src/motor.c deleted file mode 100644 index 4e421ff..0000000 --- a/avr/src/motor.c +++ /dev/null @@ -1,432 +0,0 @@ -/******************************************************************************\ - - 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 . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -#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 - -#include -#include -#include -#include - - -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;} diff --git a/avr/src/motor.h b/avr/src/motor.h deleted file mode 100644 index ce568b4..0000000 --- a/avr/src/motor.h +++ /dev/null @@ -1,59 +0,0 @@ -/******************************************************************************\ - - 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 . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -#pragma once - -#include "status.h" - -#include -#include - - -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); diff --git a/avr/src/outputs.c b/avr/src/outputs.c deleted file mode 100644 index a01536f..0000000 --- a/avr/src/outputs.c +++ /dev/null @@ -1,129 +0,0 @@ -/******************************************************************************\ - - 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 . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -#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]); -} diff --git a/avr/src/outputs.h b/avr/src/outputs.h deleted file mode 100644 index a705069..0000000 --- a/avr/src/outputs.h +++ /dev/null @@ -1,57 +0,0 @@ -/******************************************************************************\ - - 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 . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -#pragma once - -#include -#include - - -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); diff --git a/avr/src/pgmspace.h b/avr/src/pgmspace.h deleted file mode 100644 index f0b1cd0..0000000 --- a/avr/src/pgmspace.h +++ /dev/null @@ -1,50 +0,0 @@ -/******************************************************************************\ - - 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 . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -#pragma once - -#ifdef __AVR__ - -#include -#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__ diff --git a/avr/src/pins.c b/avr/src/pins.c deleted file mode 100644 index 65fb849..0000000 --- a/avr/src/pins.c +++ /dev/null @@ -1,31 +0,0 @@ -/******************************************************************************\ - - 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 . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -#include "pins.h" - - -PORT_t *pin_ports[] = {&PORTA, &PORTB, &PORTC, &PORTD, &PORTE, &PORTF}; diff --git a/avr/src/pins.h b/avr/src/pins.h deleted file mode 100644 index d122e1b..0000000 --- a/avr/src/pins.h +++ /dev/null @@ -1,52 +0,0 @@ -/******************************************************************************\ - - 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 . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -#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 - -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__ diff --git a/avr/src/plan/arc.c b/avr/src/plan/arc.c deleted file mode 100644 index 2cb1fcc..0000000 --- a/avr/src/plan/arc.c +++ /dev/null @@ -1,510 +0,0 @@ -/******************************************************************************\ - - 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 . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -/* 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 - -#include -#include -#include - - -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); -} diff --git a/avr/src/plan/arc.h b/avr/src/plan/arc.h deleted file mode 100644 index 13a047f..0000000 --- a/avr/src/plan/arc.h +++ /dev/null @@ -1,47 +0,0 @@ -/******************************************************************************\ - - 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 . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -#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(); diff --git a/avr/src/plan/buffer.c b/avr/src/plan/buffer.c deleted file mode 100644 index 6a1ad0a..0000000 --- a/avr/src/plan/buffer.c +++ /dev/null @@ -1,240 +0,0 @@ -/******************************************************************************\ - - 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 . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -/* 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 -#include -#include - - -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)); -} diff --git a/avr/src/plan/buffer.h b/avr/src/plan/buffer.h deleted file mode 100644 index de1d4d5..0000000 --- a/avr/src/plan/buffer.h +++ /dev/null @@ -1,119 +0,0 @@ -/******************************************************************************\ - - 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 . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -#pragma once - -#include "machine.h" -#include "config.h" - -#include - - -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;} diff --git a/avr/src/plan/calibrate.c b/avr/src/plan/calibrate.c deleted file mode 100644 index 1f7c6e8..0000000 --- a/avr/src/plan/calibrate.c +++ /dev/null @@ -1,176 +0,0 @@ -/******************************************************************************\ - - 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 . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - - -#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 -#include -#include -#include -#include - - -#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; -} diff --git a/avr/src/plan/calibrate.h b/avr/src/plan/calibrate.h deleted file mode 100644 index 00a9190..0000000 --- a/avr/src/plan/calibrate.h +++ /dev/null @@ -1,35 +0,0 @@ -/******************************************************************************\ - - 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 . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -#pragma once - -#include -#include - - -bool calibrate_busy(); -void calibrate_set_stallguard(int motor, uint16_t sg); diff --git a/avr/src/plan/dwell.c b/avr/src/plan/dwell.c deleted file mode 100644 index 2d5af43..0000000 --- a/avr/src/plan/dwell.c +++ /dev/null @@ -1,54 +0,0 @@ -/******************************************************************************\ - - 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 . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -#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; -} diff --git a/avr/src/plan/dwell.h b/avr/src/plan/dwell.h deleted file mode 100644 index 0345084..0000000 --- a/avr/src/plan/dwell.h +++ /dev/null @@ -1,35 +0,0 @@ -/******************************************************************************\ - - 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 . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -#pragma once - -#include "status.h" - -#include - - -stat_t mp_dwell(float seconds, int32_t line); diff --git a/avr/src/plan/exec.c b/avr/src/plan/exec.c deleted file mode 100644 index ec1cff3..0000000 --- a/avr/src/plan/exec.c +++ /dev/null @@ -1,482 +0,0 @@ -/******************************************************************************\ - - 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 . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -#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 -#include -#include -#include - - -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; - } -} diff --git a/avr/src/plan/exec.h b/avr/src/plan/exec.h deleted file mode 100644 index 409f62d..0000000 --- a/avr/src/plan/exec.h +++ /dev/null @@ -1,35 +0,0 @@ -/******************************************************************************\ - - 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 . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -#pragma once - -#include "buffer.h" - - -stat_t mp_exec_move(); -void mp_exec_abort(); -stat_t mp_exec_aline(mp_buffer_t *bf); diff --git a/avr/src/plan/jog.c b/avr/src/plan/jog.c deleted file mode 100644 index 38315b6..0000000 --- a/avr/src/plan/jog.c +++ /dev/null @@ -1,232 +0,0 @@ -/******************************************************************************\ - - 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 . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -#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 -#include -#include -#include -#include - - -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; -} diff --git a/avr/src/plan/jog.h b/avr/src/plan/jog.h deleted file mode 100644 index f2bfa69..0000000 --- a/avr/src/plan/jog.h +++ /dev/null @@ -1,28 +0,0 @@ -/******************************************************************************\ - - 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 . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -#pragma once diff --git a/avr/src/plan/line.c b/avr/src/plan/line.c deleted file mode 100644 index 1a0a34c..0000000 --- a/avr/src/plan/line.c +++ /dev/null @@ -1,437 +0,0 @@ -/******************************************************************************\ - - 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 . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -#include "line.h" - -#include "axis.h" -#include "planner.h" -#include "exec.h" -#include "buffer.h" - -#include -#include - - -/* 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; -} diff --git a/avr/src/plan/line.h b/avr/src/plan/line.h deleted file mode 100644 index e1f3b35..0000000 --- a/avr/src/plan/line.h +++ /dev/null @@ -1,39 +0,0 @@ -/******************************************************************************\ - - 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 . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -#pragma once - -#include "status.h" -#include "buffer.h" - -#include -#include - - -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[]); diff --git a/avr/src/plan/planner.c b/avr/src/plan/planner.c deleted file mode 100644 index d8da7d9..0000000 --- a/avr/src/plan/planner.c +++ /dev/null @@ -1,766 +0,0 @@ -/******************************************************************************\ - - 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 . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -/* 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 -#include -#include - - -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 VeVx sufficient length exists for all parts (corner - * case: HBT') - * HB VeVx enter at full speed & decelerate (corner case: T') - * HT Ve & Vx perfect fit HT (very rare). May be symmetric or - * asymmetric - * H VeVx 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)Vx Ve is degraded (velocity step). Vx is met - * B" line is very short but drawable; is treated as a - * body only. - * F 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; -} diff --git a/avr/src/plan/planner.h b/avr/src/plan/planner.h deleted file mode 100644 index bdc2c35..0000000 --- a/avr/src/plan/planner.h +++ /dev/null @@ -1,86 +0,0 @@ -/******************************************************************************\ - - 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 . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -#pragma once - - -#include "machine.h" // used for gcode_state_t -#include "buffer.h" -#include "util.h" -#include "config.h" - -#include - - -// 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); diff --git a/avr/src/plan/runtime.c b/avr/src/plan/runtime.c deleted file mode 100644 index a9c1bf1..0000000 --- a/avr/src/plan/runtime.c +++ /dev/null @@ -1,171 +0,0 @@ -/******************************************************************************\ - - 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 . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -#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 -#include -#include -#include - - -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; -} diff --git a/avr/src/plan/runtime.h b/avr/src/plan/runtime.h deleted file mode 100644 index a5c0fc2..0000000 --- a/avr/src/plan/runtime.h +++ /dev/null @@ -1,58 +0,0 @@ -/******************************************************************************\ - - 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 . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -#pragma once - -#include "status.h" - -#include - - -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[]); diff --git a/avr/src/plan/state.c b/avr/src/plan/state.c deleted file mode 100644 index 29c161b..0000000 --- a/avr/src/plan/state.c +++ /dev/null @@ -1,277 +0,0 @@ -/******************************************************************************\ - - 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 . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -#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 - - -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); - } - } -} diff --git a/avr/src/plan/state.h b/avr/src/plan/state.h deleted file mode 100644 index a287c42..0000000 --- a/avr/src/plan/state.h +++ /dev/null @@ -1,95 +0,0 @@ -/******************************************************************************\ - - 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 . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -#pragma once - -#include "pgmspace.h" - -#include - - -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(); diff --git a/avr/src/plan/velocity_curve.c b/avr/src/plan/velocity_curve.c deleted file mode 100644 index 7ef1009..0000000 --- a/avr/src/plan/velocity_curve.c +++ /dev/null @@ -1,75 +0,0 @@ -/******************************************************************************\ - - 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 . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -#include "velocity_curve.h" - -#include - - -/// 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; -} diff --git a/avr/src/plan/velocity_curve.h b/avr/src/plan/velocity_curve.h deleted file mode 100644 index 8d1e497..0000000 --- a/avr/src/plan/velocity_curve.h +++ /dev/null @@ -1,32 +0,0 @@ -/******************************************************************************\ - - 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 . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -#pragma once - -float velocity_curve(float Vi, float Vt, float t); diff --git a/avr/src/pwm_spindle.c b/avr/src/pwm_spindle.c deleted file mode 100644 index 305251a..0000000 --- a/avr/src/pwm_spindle.c +++ /dev/null @@ -1,151 +0,0 @@ -/******************************************************************************\ - - 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 . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - - -#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;} diff --git a/avr/src/pwm_spindle.h b/avr/src/pwm_spindle.h deleted file mode 100644 index 10006a6..0000000 --- a/avr/src/pwm_spindle.h +++ /dev/null @@ -1,35 +0,0 @@ -/******************************************************************************\ - - 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 . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -#pragma once - -#include "machine.h" - - -void pwm_spindle_init(); -void pwm_spindle_set(spindle_mode_t mode, float speed); -void pwm_spindle_stop(); diff --git a/avr/src/report.c b/avr/src/report.c deleted file mode 100644 index bb04f63..0000000 --- a/avr/src/report.c +++ /dev/null @@ -1,57 +0,0 @@ -/******************************************************************************\ - - 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 . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -#include "report.h" -#include "config.h" -#include "usart.h" -#include "rtc.h" -#include "vars.h" -#include "pgmspace.h" - -#include -#include - - -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; - } -} diff --git a/avr/src/report.h b/avr/src/report.h deleted file mode 100644 index dd0106c..0000000 --- a/avr/src/report.h +++ /dev/null @@ -1,35 +0,0 @@ -/******************************************************************************\ - - 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 . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -#pragma once - - -#include "status.h" - -void report_request(); -void report_request_full(); -void report_callback(); diff --git a/avr/src/ringbuf.def b/avr/src/ringbuf.def deleted file mode 100644 index 5228fd0..0000000 --- a/avr/src/ringbuf.def +++ /dev/null @@ -1,158 +0,0 @@ -/******************************************************************************\ - - 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 . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -/* 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 _init(); - * int _empty(); - * int _full(); - * _peek(); - * void _pop(); - * void _push( data); - * - * Where is defined by RING_BUF_NAME and 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 - -#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 diff --git a/avr/src/rtc.c b/avr/src/rtc.c deleted file mode 100644 index e09c584..0000000 --- a/avr/src/rtc.c +++ /dev/null @@ -1,73 +0,0 @@ -/******************************************************************************\ - - 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 . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -#include "rtc.h" - -#include "switch.h" -#include "huanyang.h" -#include "motor.h" - -#include -#include - -#include - - -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);} diff --git a/avr/src/rtc.h b/avr/src/rtc.h deleted file mode 100644 index 2c040bb..0000000 --- a/avr/src/rtc.h +++ /dev/null @@ -1,38 +0,0 @@ -/******************************************************************************\ - - 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 . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -#pragma once - - -#include -#include - -void rtc_init(); -uint32_t rtc_get_time(); -int32_t rtc_diff(uint32_t t); -bool rtc_expired(uint32_t t); diff --git a/avr/src/spindle.c b/avr/src/spindle.c deleted file mode 100644 index d6ae802..0000000 --- a/avr/src/spindle.c +++ /dev/null @@ -1,124 +0,0 @@ -/******************************************************************************\ - - 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 . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -#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"); -} diff --git a/avr/src/spindle.h b/avr/src/spindle.h deleted file mode 100644 index 9dbe5a5..0000000 --- a/avr/src/spindle.h +++ /dev/null @@ -1,40 +0,0 @@ -/******************************************************************************\ - - 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 . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -#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(); diff --git a/avr/src/status.c b/avr/src/status.c deleted file mode 100644 index 8d1a0a8..0000000 --- a/avr/src/status.c +++ /dev/null @@ -1,123 +0,0 @@ -/******************************************************************************\ - - 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 . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -#include "status.h" -#include "estop.h" -#include "usart.h" - -#include -#include - - -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; -} diff --git a/avr/src/status.h b/avr/src/status.h deleted file mode 100644 index 5e2b91d..0000000 --- a/avr/src/status.h +++ /dev/null @@ -1,99 +0,0 @@ -/******************************************************************************\ - - 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 . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -#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 diff --git a/avr/src/stepper.c b/avr/src/stepper.c deleted file mode 100644 index 2396f07..0000000 --- a/avr/src/stepper.c +++ /dev/null @@ -1,242 +0,0 @@ -/******************************************************************************\ - - 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 . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -#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 -#include - - -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 -} diff --git a/avr/src/stepper.h b/avr/src/stepper.h deleted file mode 100644 index 44f2d45..0000000 --- a/avr/src/stepper.h +++ /dev/null @@ -1,43 +0,0 @@ -/******************************************************************************\ - - 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 . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -#pragma once - -#include "status.h" - -#include -#include - - -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); diff --git a/avr/src/switch.c b/avr/src/switch.c deleted file mode 100644 index 489c554..0000000 --- a/avr/src/switch.c +++ /dev/null @@ -1,195 +0,0 @@ -/******************************************************************************\ - - 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 . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -#include "switch.h" -#include "config.h" - -#include - -#include - - -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);} diff --git a/avr/src/switch.h b/avr/src/switch.h deleted file mode 100644 index aad8ad1..0000000 --- a/avr/src/switch.h +++ /dev/null @@ -1,69 +0,0 @@ -/******************************************************************************\ - - 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 . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -#pragma once - - -#include "config.h" - -#include -#include - - -// 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); diff --git a/avr/src/usart.c b/avr/src/usart.c deleted file mode 100644 index be88d8b..0000000 --- a/avr/src/usart.c +++ /dev/null @@ -1,282 +0,0 @@ -/******************************************************************************\ - - 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 . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -#include "usart.h" -#include "cpp_magic.h" -#include "config.h" - -#include -#include - -#include -#include - -// 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(); -} diff --git a/avr/src/usart.h b/avr/src/usart.h deleted file mode 100644 index 915d362..0000000 --- a/avr/src/usart.h +++ /dev/null @@ -1,77 +0,0 @@ -/******************************************************************************\ - - 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 . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -#pragma once - - -#include -#include - -#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();} diff --git a/avr/src/util.c b/avr/src/util.c deleted file mode 100644 index 1a76542..0000000 --- a/avr/src/util.c +++ /dev/null @@ -1,51 +0,0 @@ -/******************************************************************************\ - - 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 . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -#include "util.h" - -#include - - -/// 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; -} diff --git a/avr/src/util.h b/avr/src/util.h deleted file mode 100644 index 3ec44b8..0000000 --- a/avr/src/util.h +++ /dev/null @@ -1,80 +0,0 @@ -/******************************************************************************\ - - 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 . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -#pragma once - - -#include "config.h" - -#include -#include -#include -#include - - -// 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 diff --git a/avr/src/varcb.c b/avr/src/varcb.c deleted file mode 100644 index 1a59471..0000000 --- a/avr/src/varcb.c +++ /dev/null @@ -1,141 +0,0 @@ -/******************************************************************************\ - - 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 . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -#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()); -} diff --git a/avr/src/vars.c b/avr/src/vars.c deleted file mode 100644 index 815bcc1..0000000 --- a/avr/src/vars.c +++ /dev/null @@ -1,456 +0,0 @@ -/******************************************************************************\ - - 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 . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -#include "vars.h" - -#include "cpp_magic.h" -#include "status.h" -#include "hardware.h" -#include "config.h" -#include "axis.h" -#include "pgmspace.h" - -#include -#include -#include -#include -#include -#include -#include -#include - - -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 = ""; -#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); -} diff --git a/avr/src/vars.def b/avr/src/vars.def deleted file mode 100644 index 8346d7c..0000000 --- a/avr/src/vars.def +++ /dev/null @@ -1,141 +0,0 @@ -/******************************************************************************\ - - 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 . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -#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") diff --git a/avr/src/vars.h b/avr/src/vars.h deleted file mode 100644 index 4ad37b7..0000000 --- a/avr/src/vars.h +++ /dev/null @@ -1,45 +0,0 @@ -/******************************************************************************\ - - 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 . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -#pragma once - -#include "status.h" - -#include - -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(); diff --git a/avr/src/vars.json.in b/avr/src/vars.json.in deleted file mode 100644 index 03195c7..0000000 --- a/avr/src/vars.json.in +++ /dev/null @@ -1,15 +0,0 @@ -#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 - "_": {} -} diff --git a/avr/test/Makefile b/avr/test/Makefile deleted file mode 100644 index cb7d858..0000000 --- a/avr/test/Makefile +++ /dev/null @@ -1,37 +0,0 @@ -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/*) diff --git a/avr/test/arc.gc b/avr/test/arc.gc deleted file mode 100644 index 72b612f..0000000 --- a/avr/test/arc.gc +++ /dev/null @@ -1,10 +0,0 @@ -$resume - -$0vm=10000 -$1vm=10000 -$2vm=10000 -$0jm=50 -$1jm=50 -$2jm=50 - -G3 I2 J2 F10000 diff --git a/avr/test/hal.c b/avr/test/hal.c deleted file mode 100644 index 45ecfde..0000000 --- a/avr/test/hal.c +++ /dev/null @@ -1,232 +0,0 @@ -/******************************************************************************\ - - 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 . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -#include "status.h" -#include "spindle.h" -#include "i2c.h" -#include "cpp_magic.h" -#include "plan/buffer.h" - -#include -#include -#include -#include - - -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); -} diff --git a/avr/test/line.gc b/avr/test/line.gc deleted file mode 100644 index 20695cc..0000000 --- a/avr/test/line.gc +++ /dev/null @@ -1,16 +0,0 @@ -$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 diff --git a/avr/test/planner-test.c b/avr/test/planner-test.c deleted file mode 100644 index 78206ee..0000000 --- a/avr/test/planner-test.c +++ /dev/null @@ -1,82 +0,0 @@ -/******************************************************************************\ - - 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 . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -#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 -#include - - -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; -} diff --git a/avr/test/plot.py b/avr/test/plot.py deleted file mode 100755 index 28fc6c5..0000000 --- a/avr/test/plot.py +++ /dev/null @@ -1,90 +0,0 @@ -#!/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() diff --git a/avr/test/short.gc b/avr/test/short.gc deleted file mode 100644 index 1ba1685..0000000 --- a/avr/test/short.gc +++ /dev/null @@ -1,59 +0,0 @@ -$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 diff --git a/src/avr/.gitignore b/src/avr/.gitignore new file mode 100755 index 0000000..bc78328 --- /dev/null +++ b/src/avr/.gitignore @@ -0,0 +1,12 @@ +# Backup files +*~ +\#* + +build +.dep + +/*.eep +/*.hex +/*.elf +/*.lss +/*.map diff --git a/src/avr/BezierMath.md b/src/avr/BezierMath.md new file mode 100644 index 0000000..f9da75a --- /dev/null +++ b/src/avr/BezierMath.md @@ -0,0 +1,56 @@ +# 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 diff --git a/src/avr/CODE_TAG b/src/avr/CODE_TAG new file mode 100644 index 0000000..a0a7364 --- /dev/null +++ b/src/avr/CODE_TAG @@ -0,0 +1,22 @@ + 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 . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the software. If not, see + . + + For information regarding this software email: + "Joseph Coffland" diff --git a/src/avr/LICENSE b/src/avr/LICENSE new file mode 100644 index 0000000..b08b657 --- /dev/null +++ b/src/avr/LICENSE @@ -0,0 +1,380 @@ +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 +. + +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. + + + Copyright (C) + + 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. + + , 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. diff --git a/src/avr/Makefile b/src/avr/Makefile new file mode 100644 index 0000000..9c6c0d2 --- /dev/null +++ b/src/avr/Makefile @@ -0,0 +1,160 @@ +# 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/*) diff --git a/src/avr/MoveLifecycle.md b/src/avr/MoveLifecycle.md new file mode 100644 index 0000000..edb3101 --- /dev/null +++ b/src/avr/MoveLifecycle.md @@ -0,0 +1,99 @@ +# 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. diff --git a/src/avr/MoveLifecycleCalls.md b/src/avr/MoveLifecycleCalls.md new file mode 100644 index 0000000..b982100 --- /dev/null +++ b/src/avr/MoveLifecycleCalls.md @@ -0,0 +1,76 @@ +# 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 diff --git a/src/avr/README.md b/src/avr/README.md new file mode 100644 index 0000000..24ba38a --- /dev/null +++ b/src/avr/README.md @@ -0,0 +1,22 @@ +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 diff --git a/src/avr/data_usage.py b/src/avr/data_usage.py new file mode 100755 index 0000000..8ba92f0 --- /dev/null +++ b/src/avr/data_usage.py @@ -0,0 +1,36 @@ +#!/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) diff --git a/src/avr/src/axis.c b/src/avr/src/axis.c new file mode 100644 index 0000000..0697aaf --- /dev/null +++ b/src/avr/src/axis.c @@ -0,0 +1,197 @@ +/******************************************************************************\ + + 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 . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the software. If not, see + . + + For information regarding this software email: + "Joseph Coffland" + +\******************************************************************************/ + + +#include "axis.h" +#include "motor.h" +#include "plan/planner.h" + +#include +#include +#include + + +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)); +} diff --git a/src/avr/src/axis.h b/src/avr/src/axis.h new file mode 100644 index 0000000..af7f658 --- /dev/null +++ b/src/avr/src/axis.h @@ -0,0 +1,71 @@ +/******************************************************************************\ + + 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 . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the software. If not, see + . + + For information regarding this software email: + "Joseph Coffland" + +\******************************************************************************/ + +#pragma once + +#include "config.h" + +#include + + +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); diff --git a/src/avr/src/boot/boot.c b/src/avr/src/boot/boot.c new file mode 100644 index 0000000..a0ac310 --- /dev/null +++ b/src/avr/src/boot/boot.c @@ -0,0 +1,448 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2017 Buildbotics LLC + Copyright (c) 2010 Alex Forencich + All rights reserved. + + This file ("the software") is free software: you can redistribute it + and/or modify it under the terms of the GNU General Public License, + version 2 as published by the Free Software Foundation. You should + have received a copy of the GNU General Public License, version 2 + along with the software. If not, see . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the software. If not, see + . + + For information regarding this software email: + "Joseph Coffland" + +\******************************************************************************/ + +#include "boot.h" +#include "sp_driver.h" + +#include +#include + +#include +#include + +#include + + +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"); +} diff --git a/src/avr/src/boot/boot.h b/src/avr/src/boot/boot.h new file mode 100644 index 0000000..2af4be1 --- /dev/null +++ b/src/avr/src/boot/boot.h @@ -0,0 +1,111 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2017 Buildbotics LLC + Copyright (c) 2010 Alex Forencich + All rights reserved. + + This file ("the software") is free software: you can redistribute it + and/or modify it under the terms of the GNU General Public License, + version 2 as published by the Free Software Foundation. You should + have received a copy of the GNU General Public License, version 2 + along with the software. If not, see . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the software. If not, see + . + + For information regarding this software email: + "Joseph Coffland" + +\******************************************************************************/ + +#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 = '?', +}; diff --git a/src/avr/src/boot/sp_driver.S b/src/avr/src/boot/sp_driver.S new file mode 100644 index 0000000..bbb1ee7 --- /dev/null +++ b/src/avr/src/boot/sp_driver.S @@ -0,0 +1,403 @@ +;****************************************************************************** +;* $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 + +; 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 diff --git a/src/avr/src/boot/sp_driver.h b/src/avr/src/boot/sp_driver.h new file mode 100644 index 0000000..01aeaee --- /dev/null +++ b/src/avr/src/boot/sp_driver.h @@ -0,0 +1,173 @@ +/******************************************************************************* + * $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 + +#include + +#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(); diff --git a/src/avr/src/command.c b/src/avr/src/command.c new file mode 100644 index 0000000..3939751 --- /dev/null +++ b/src/avr/src/command.c @@ -0,0 +1,331 @@ +/******************************************************************************\ + + 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 . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the software. If not, see + . + + For information regarding this software email: + "Joseph Coffland" + +\******************************************************************************/ + +#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 +#endif + +#include +#include +#include +#include + + +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; +} diff --git a/src/avr/src/command.def b/src/avr/src/command.def new file mode 100644 index 0000000..b11766f --- /dev/null +++ b/src/avr/src/command.def @@ -0,0 +1,36 @@ +/******************************************************************************\ + + 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 . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the software. If not, see + . + + For information regarding this software email: + "Joseph Coffland" + +\******************************************************************************/ + +// Name Min, Max args, Help +CMD(help, 0, 1, "Print this help screen") +CMD(report, 1, 2, "[var] . 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") diff --git a/src/avr/src/command.h b/src/avr/src/command.h new file mode 100644 index 0000000..67397ac --- /dev/null +++ b/src/avr/src/command.h @@ -0,0 +1,50 @@ +/******************************************************************************\ + + 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 . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the software. If not, see + . + + For information regarding this software email: + "Joseph Coffland" + +\******************************************************************************/ + +#pragma once + + +#include + + +#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(); diff --git a/src/avr/src/config.h b/src/avr/src/config.h new file mode 100644 index 0000000..91fa972 --- /dev/null +++ b/src/avr/src/config.h @@ -0,0 +1,263 @@ +/******************************************************************************\ + + 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 . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the software. If not, see + . + + For information regarding this software email: + "Joseph Coffland" + +\******************************************************************************/ + +#pragma once + +#include "pins.h" + +#ifdef __AVR__ +#include +#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 diff --git a/src/avr/src/coolant.c b/src/avr/src/coolant.c new file mode 100644 index 0000000..46ec802 --- /dev/null +++ b/src/avr/src/coolant.c @@ -0,0 +1,45 @@ +/******************************************************************************\ + + 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 . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the software. If not, see + . + + For information regarding this software email: + "Joseph Coffland" + +\******************************************************************************/ + +#include "coolant.h" +#include "config.h" + +#include + + +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);} diff --git a/src/avr/src/coolant.h b/src/avr/src/coolant.h new file mode 100644 index 0000000..0d9b074 --- /dev/null +++ b/src/avr/src/coolant.h @@ -0,0 +1,37 @@ +/******************************************************************************\ + + 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 . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the software. If not, see + . + + For information regarding this software email: + "Joseph Coffland" + +\******************************************************************************/ + +#pragma once + +#include + + +void coolant_init(); +void coolant_set_mist(bool x); +void coolant_set_flood(bool x); +bool coolant_get_mist(); +bool coolant_get_flood(); diff --git a/src/avr/src/cpp_magic.h b/src/avr/src/cpp_magic.h new file mode 100644 index 0000000..c7abc59 --- /dev/null +++ b/src/avr/src/cpp_magic.h @@ -0,0 +1,510 @@ +/******************************************************************************\ + + 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 + diff --git a/src/avr/src/drv8711.c b/src/avr/src/drv8711.c new file mode 100644 index 0000000..18cd95d --- /dev/null +++ b/src/avr/src/drv8711.c @@ -0,0 +1,497 @@ +/******************************************************************************\ + + 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 . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the software. If not, see + . + + For information regarding this software email: + "Joseph Coffland" + +\******************************************************************************/ + +#include "drv8711.h" +#include "status.h" +#include "stepper.h" +#include "report.h" + +#include +#include + +#include +#include +#include + + +#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(); +} diff --git a/src/avr/src/drv8711.h b/src/avr/src/drv8711.h new file mode 100644 index 0000000..8356322 --- /dev/null +++ b/src/avr/src/drv8711.h @@ -0,0 +1,179 @@ +/******************************************************************************\ + + 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 . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the software. If not, see + . + + For information regarding this software email: + "Joseph Coffland" + +\******************************************************************************/ + +#pragma once + + +#include "config.h" +#include "status.h" +#include "motor.h" + +#include +#include + +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); diff --git a/src/avr/src/estop.c b/src/avr/src/estop.c new file mode 100644 index 0000000..99ed0f5 --- /dev/null +++ b/src/avr/src/estop.c @@ -0,0 +1,140 @@ +/******************************************************************************\ + + 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 . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the software. If not, see + . + + For information regarding this software email: + "Joseph Coffland" + +\******************************************************************************/ + +#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 + + +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()); +} diff --git a/src/avr/src/estop.h b/src/avr/src/estop.h new file mode 100644 index 0000000..757e03f --- /dev/null +++ b/src/avr/src/estop.h @@ -0,0 +1,38 @@ +/******************************************************************************\ + + 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 . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the software. If not, see + . + + For information regarding this software email: + "Joseph Coffland" + +\******************************************************************************/ + +#pragma once + +#include "status.h" + +#include + + +void estop_init(); +bool estop_triggered(); +void estop_trigger(stat_t reason); +void estop_clear(); diff --git a/src/avr/src/gcode_expr.c b/src/avr/src/gcode_expr.c new file mode 100644 index 0000000..8ac1a50 --- /dev/null +++ b/src/avr/src/gcode_expr.c @@ -0,0 +1,296 @@ +/******************************************************************************\ + + 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 . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the software. If not, see + . + + For information regarding this software email: + "Joseph Coffland" + +\******************************************************************************/ + +#include "gcode_expr.h" + +#include "gcode_parser.h" +#include "vars.h" + +#include +#include +#include + + +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(); +} diff --git a/src/avr/src/gcode_expr.h b/src/avr/src/gcode_expr.h new file mode 100644 index 0000000..d7a963a --- /dev/null +++ b/src/avr/src/gcode_expr.h @@ -0,0 +1,33 @@ +/******************************************************************************\ + + 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 . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the software. If not, see + . + + For information regarding this software email: + "Joseph Coffland" + +\******************************************************************************/ + +#pragma once + + +float parse_gcode_number(char **p); +float parse_gcode_expression(char **p); diff --git a/src/avr/src/gcode_parser.c b/src/avr/src/gcode_parser.c new file mode 100644 index 0000000..97fbe40 --- /dev/null +++ b/src/avr/src/gcode_parser.c @@ -0,0 +1,518 @@ +/******************************************************************************\ + + 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 . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the software. If not, see + . + + For information regarding this software email: + "Joseph Coffland" + +\******************************************************************************/ + +#include "gcode_parser.h" + +#include "gcode_expr.h" +#include "machine.h" +#include "plan/arc.h" +#include "axis.h" +#include "util.h" + +#include +#include +#include +#include +#include +#include + + +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(); +} diff --git a/src/avr/src/gcode_parser.h b/src/avr/src/gcode_parser.h new file mode 100644 index 0000000..dc880ef --- /dev/null +++ b/src/avr/src/gcode_parser.h @@ -0,0 +1,87 @@ +/******************************************************************************\ + + 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 . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the software. If not, see + . + + For information regarding this software email: + "Joseph Coffland" + +\******************************************************************************/ + +#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); diff --git a/src/avr/src/gcode_state.c b/src/avr/src/gcode_state.c new file mode 100644 index 0000000..7807e56 --- /dev/null +++ b/src/avr/src/gcode_state.c @@ -0,0 +1,179 @@ +/******************************************************************************\ + + 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 . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the software. If not, see + . + + For information regarding this software email: + "Joseph Coffland" + +\******************************************************************************/ + +#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; +} diff --git a/src/avr/src/gcode_state.def b/src/avr/src/gcode_state.def new file mode 100644 index 0000000..5d73cf4 --- /dev/null +++ b/src/avr/src/gcode_state.def @@ -0,0 +1,69 @@ +/******************************************************************************\ + + 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 . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the software. If not, see + . + + For information regarding this software email: + "Joseph Coffland" + +\******************************************************************************/ + +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 diff --git a/src/avr/src/gcode_state.h b/src/avr/src/gcode_state.h new file mode 100644 index 0000000..4dc2665 --- /dev/null +++ b/src/avr/src/gcode_state.h @@ -0,0 +1,206 @@ +/******************************************************************************\ + + 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 . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the software. If not, see + . + + For information regarding this software email: + "Joseph Coffland" + +\******************************************************************************/ + +#pragma once + +#include "config.h" +#include "pgmspace.h" + +#include +#include + + +/* 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); diff --git a/src/avr/src/hardware.c b/src/avr/src/hardware.c new file mode 100644 index 0000000..bd3de2b --- /dev/null +++ b/src/avr/src/hardware.c @@ -0,0 +1,179 @@ +/******************************************************************************\ + + 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 . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the software. If not, see + . + + For information regarding this software email: + "Joseph Coffland" + +\******************************************************************************/ + +#include "hardware.h" +#include "rtc.h" +#include "usart.h" +#include "huanyang.h" +#include "config.h" +#include "pgmspace.h" + +#include +#include +#include + +#include +#include + + +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; +} diff --git a/src/avr/src/hardware.h b/src/avr/src/hardware.h new file mode 100644 index 0000000..56ea39e --- /dev/null +++ b/src/avr/src/hardware.h @@ -0,0 +1,45 @@ +/******************************************************************************\ + + 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 . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the software. If not, see + . + + For information regarding this software email: + "Joseph Coffland" + +\******************************************************************************/ + +#pragma once + +#include "status.h" + +#include + + +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); diff --git a/src/avr/src/huanyang.c b/src/avr/src/huanyang.c new file mode 100644 index 0000000..f43bf11 --- /dev/null +++ b/src/avr/src/huanyang.c @@ -0,0 +1,523 @@ +/******************************************************************************\ + + 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 . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the software. If not, see + . + + For information regarding this software email: + "Joseph Coffland" + +\******************************************************************************/ + +#include "huanyang.h" +#include "config.h" +#include "rtc.h" +#include "report.h" +#include "pgmspace.h" + +#include +#include +#include + +#include +#include +#include + + +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;} diff --git a/src/avr/src/huanyang.h b/src/avr/src/huanyang.h new file mode 100644 index 0000000..1e52258 --- /dev/null +++ b/src/avr/src/huanyang.h @@ -0,0 +1,37 @@ +/******************************************************************************\ + + 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 . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the software. If not, see + . + + For information regarding this software email: + "Joseph Coffland" + +\******************************************************************************/ + +#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(); diff --git a/src/avr/src/i2c.c b/src/avr/src/i2c.c new file mode 100644 index 0000000..7ed4510 --- /dev/null +++ b/src/avr/src/i2c.c @@ -0,0 +1,128 @@ +/******************************************************************************\ + + 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 . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the software. If not, see + . + + For information regarding this software email: + "Joseph Coffland" + +\******************************************************************************/ + +#include "i2c.h" + +#include + +#include + + +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;} diff --git a/src/avr/src/i2c.h b/src/avr/src/i2c.h new file mode 100644 index 0000000..766396d --- /dev/null +++ b/src/avr/src/i2c.h @@ -0,0 +1,56 @@ +/******************************************************************************\ + + 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 . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the software. If not, see + . + + For information regarding this software email: + "Joseph Coffland" + +\******************************************************************************/ + +#pragma once + +#include "config.h" + +#include + + +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); diff --git a/src/avr/src/machine.c b/src/avr/src/machine.c new file mode 100644 index 0000000..5c5be2c --- /dev/null +++ b/src/avr/src/machine.c @@ -0,0 +1,934 @@ +/******************************************************************************\ + + 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 . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the software. If not, see + . + + For information regarding this software email: + "Joseph Coffland" + +\******************************************************************************/ + +/* 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); +} diff --git a/src/avr/src/machine.h b/src/avr/src/machine.h new file mode 100644 index 0000000..51e0480 --- /dev/null +++ b/src/avr/src/machine.h @@ -0,0 +1,142 @@ +/******************************************************************************\ + + 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 . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the software. If not, see + . + + For information regarding this software email: + "Joseph Coffland" + +\******************************************************************************/ + +#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(); diff --git a/src/avr/src/main.c b/src/avr/src/main.c new file mode 100644 index 0000000..6250827 --- /dev/null +++ b/src/avr/src/main.c @@ -0,0 +1,95 @@ +/******************************************************************************\ + + 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 . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the software. If not, see + . + + For information regarding this software email: + "Joseph Coffland" + +\******************************************************************************/ + +#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 + +#include +#include + + +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; +} diff --git a/src/avr/src/messages.def b/src/avr/src/messages.def new file mode 100644 index 0000000..c69933b --- /dev/null +++ b/src/avr/src/messages.def @@ -0,0 +1,129 @@ +/******************************************************************************\ + + 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 . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the software. If not, see + . + + For information regarding this software email: + "Joseph Coffland" + +\******************************************************************************/ + +// 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, "") diff --git a/src/avr/src/motor.c b/src/avr/src/motor.c new file mode 100644 index 0000000..4e421ff --- /dev/null +++ b/src/avr/src/motor.c @@ -0,0 +1,432 @@ +/******************************************************************************\ + + 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 . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the software. If not, see + . + + For information regarding this software email: + "Joseph Coffland" + +\******************************************************************************/ + +#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 + +#include +#include +#include +#include + + +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;} diff --git a/src/avr/src/motor.h b/src/avr/src/motor.h new file mode 100644 index 0000000..ce568b4 --- /dev/null +++ b/src/avr/src/motor.h @@ -0,0 +1,59 @@ +/******************************************************************************\ + + 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 . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the software. If not, see + . + + For information regarding this software email: + "Joseph Coffland" + +\******************************************************************************/ + +#pragma once + +#include "status.h" + +#include +#include + + +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); diff --git a/src/avr/src/outputs.c b/src/avr/src/outputs.c new file mode 100644 index 0000000..a01536f --- /dev/null +++ b/src/avr/src/outputs.c @@ -0,0 +1,129 @@ +/******************************************************************************\ + + 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 . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the software. If not, see + . + + For information regarding this software email: + "Joseph Coffland" + +\******************************************************************************/ + +#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]); +} diff --git a/src/avr/src/outputs.h b/src/avr/src/outputs.h new file mode 100644 index 0000000..a705069 --- /dev/null +++ b/src/avr/src/outputs.h @@ -0,0 +1,57 @@ +/******************************************************************************\ + + 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 . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the software. If not, see + . + + For information regarding this software email: + "Joseph Coffland" + +\******************************************************************************/ + +#pragma once + +#include +#include + + +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); diff --git a/src/avr/src/pgmspace.h b/src/avr/src/pgmspace.h new file mode 100644 index 0000000..f0b1cd0 --- /dev/null +++ b/src/avr/src/pgmspace.h @@ -0,0 +1,50 @@ +/******************************************************************************\ + + 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 . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the software. If not, see + . + + For information regarding this software email: + "Joseph Coffland" + +\******************************************************************************/ + +#pragma once + +#ifdef __AVR__ + +#include +#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__ diff --git a/src/avr/src/pins.c b/src/avr/src/pins.c new file mode 100644 index 0000000..65fb849 --- /dev/null +++ b/src/avr/src/pins.c @@ -0,0 +1,31 @@ +/******************************************************************************\ + + 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 . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the software. If not, see + . + + For information regarding this software email: + "Joseph Coffland" + +\******************************************************************************/ + +#include "pins.h" + + +PORT_t *pin_ports[] = {&PORTA, &PORTB, &PORTC, &PORTD, &PORTE, &PORTF}; diff --git a/src/avr/src/pins.h b/src/avr/src/pins.h new file mode 100644 index 0000000..d122e1b --- /dev/null +++ b/src/avr/src/pins.h @@ -0,0 +1,52 @@ +/******************************************************************************\ + + 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 . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the software. If not, see + . + + For information regarding this software email: + "Joseph Coffland" + +\******************************************************************************/ + +#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 + +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__ diff --git a/src/avr/src/plan/arc.c b/src/avr/src/plan/arc.c new file mode 100644 index 0000000..2cb1fcc --- /dev/null +++ b/src/avr/src/plan/arc.c @@ -0,0 +1,510 @@ +/******************************************************************************\ + + 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 . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the software. If not, see + . + + For information regarding this software email: + "Joseph Coffland" + +\******************************************************************************/ + +/* 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 + +#include +#include +#include + + +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); +} diff --git a/src/avr/src/plan/arc.h b/src/avr/src/plan/arc.h new file mode 100644 index 0000000..13a047f --- /dev/null +++ b/src/avr/src/plan/arc.h @@ -0,0 +1,47 @@ +/******************************************************************************\ + + 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 . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the software. If not, see + . + + For information regarding this software email: + "Joseph Coffland" + +\******************************************************************************/ + +#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(); diff --git a/src/avr/src/plan/buffer.c b/src/avr/src/plan/buffer.c new file mode 100644 index 0000000..6a1ad0a --- /dev/null +++ b/src/avr/src/plan/buffer.c @@ -0,0 +1,240 @@ +/******************************************************************************\ + + 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 . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the software. If not, see + . + + For information regarding this software email: + "Joseph Coffland" + +\******************************************************************************/ + +/* 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 +#include +#include + + +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)); +} diff --git a/src/avr/src/plan/buffer.h b/src/avr/src/plan/buffer.h new file mode 100644 index 0000000..de1d4d5 --- /dev/null +++ b/src/avr/src/plan/buffer.h @@ -0,0 +1,119 @@ +/******************************************************************************\ + + 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 . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the software. If not, see + . + + For information regarding this software email: + "Joseph Coffland" + +\******************************************************************************/ + +#pragma once + +#include "machine.h" +#include "config.h" + +#include + + +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;} diff --git a/src/avr/src/plan/calibrate.c b/src/avr/src/plan/calibrate.c new file mode 100644 index 0000000..1f7c6e8 --- /dev/null +++ b/src/avr/src/plan/calibrate.c @@ -0,0 +1,176 @@ +/******************************************************************************\ + + 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 . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the software. If not, see + . + + For information regarding this software email: + "Joseph Coffland" + +\******************************************************************************/ + + +#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 +#include +#include +#include +#include + + +#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; +} diff --git a/src/avr/src/plan/calibrate.h b/src/avr/src/plan/calibrate.h new file mode 100644 index 0000000..00a9190 --- /dev/null +++ b/src/avr/src/plan/calibrate.h @@ -0,0 +1,35 @@ +/******************************************************************************\ + + 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 . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the software. If not, see + . + + For information regarding this software email: + "Joseph Coffland" + +\******************************************************************************/ + +#pragma once + +#include +#include + + +bool calibrate_busy(); +void calibrate_set_stallguard(int motor, uint16_t sg); diff --git a/src/avr/src/plan/dwell.c b/src/avr/src/plan/dwell.c new file mode 100644 index 0000000..2d5af43 --- /dev/null +++ b/src/avr/src/plan/dwell.c @@ -0,0 +1,54 @@ +/******************************************************************************\ + + 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 . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the software. If not, see + . + + For information regarding this software email: + "Joseph Coffland" + +\******************************************************************************/ + +#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; +} diff --git a/src/avr/src/plan/dwell.h b/src/avr/src/plan/dwell.h new file mode 100644 index 0000000..0345084 --- /dev/null +++ b/src/avr/src/plan/dwell.h @@ -0,0 +1,35 @@ +/******************************************************************************\ + + 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 . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the software. If not, see + . + + For information regarding this software email: + "Joseph Coffland" + +\******************************************************************************/ + +#pragma once + +#include "status.h" + +#include + + +stat_t mp_dwell(float seconds, int32_t line); diff --git a/src/avr/src/plan/exec.c b/src/avr/src/plan/exec.c new file mode 100644 index 0000000..ec1cff3 --- /dev/null +++ b/src/avr/src/plan/exec.c @@ -0,0 +1,482 @@ +/******************************************************************************\ + + 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 . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the software. If not, see + . + + For information regarding this software email: + "Joseph Coffland" + +\******************************************************************************/ + +#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 +#include +#include +#include + + +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; + } +} diff --git a/src/avr/src/plan/exec.h b/src/avr/src/plan/exec.h new file mode 100644 index 0000000..409f62d --- /dev/null +++ b/src/avr/src/plan/exec.h @@ -0,0 +1,35 @@ +/******************************************************************************\ + + 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 . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the software. If not, see + . + + For information regarding this software email: + "Joseph Coffland" + +\******************************************************************************/ + +#pragma once + +#include "buffer.h" + + +stat_t mp_exec_move(); +void mp_exec_abort(); +stat_t mp_exec_aline(mp_buffer_t *bf); diff --git a/src/avr/src/plan/jog.c b/src/avr/src/plan/jog.c new file mode 100644 index 0000000..38315b6 --- /dev/null +++ b/src/avr/src/plan/jog.c @@ -0,0 +1,232 @@ +/******************************************************************************\ + + 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 . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the software. If not, see + . + + For information regarding this software email: + "Joseph Coffland" + +\******************************************************************************/ + +#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 +#include +#include +#include +#include + + +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; +} diff --git a/src/avr/src/plan/jog.h b/src/avr/src/plan/jog.h new file mode 100644 index 0000000..f2bfa69 --- /dev/null +++ b/src/avr/src/plan/jog.h @@ -0,0 +1,28 @@ +/******************************************************************************\ + + 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 . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the software. If not, see + . + + For information regarding this software email: + "Joseph Coffland" + +\******************************************************************************/ + +#pragma once diff --git a/src/avr/src/plan/line.c b/src/avr/src/plan/line.c new file mode 100644 index 0000000..1a0a34c --- /dev/null +++ b/src/avr/src/plan/line.c @@ -0,0 +1,437 @@ +/******************************************************************************\ + + 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 . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the software. If not, see + . + + For information regarding this software email: + "Joseph Coffland" + +\******************************************************************************/ + +#include "line.h" + +#include "axis.h" +#include "planner.h" +#include "exec.h" +#include "buffer.h" + +#include +#include + + +/* 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; +} diff --git a/src/avr/src/plan/line.h b/src/avr/src/plan/line.h new file mode 100644 index 0000000..e1f3b35 --- /dev/null +++ b/src/avr/src/plan/line.h @@ -0,0 +1,39 @@ +/******************************************************************************\ + + 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 . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the software. If not, see + . + + For information regarding this software email: + "Joseph Coffland" + +\******************************************************************************/ + +#pragma once + +#include "status.h" +#include "buffer.h" + +#include +#include + + +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[]); diff --git a/src/avr/src/plan/planner.c b/src/avr/src/plan/planner.c new file mode 100644 index 0000000..d8da7d9 --- /dev/null +++ b/src/avr/src/plan/planner.c @@ -0,0 +1,766 @@ +/******************************************************************************\ + + 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 . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the software. If not, see + . + + For information regarding this software email: + "Joseph Coffland" + +\******************************************************************************/ + +/* 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 +#include +#include + + +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 VeVx sufficient length exists for all parts (corner + * case: HBT') + * HB VeVx enter at full speed & decelerate (corner case: T') + * HT Ve & Vx perfect fit HT (very rare). May be symmetric or + * asymmetric + * H VeVx 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)Vx Ve is degraded (velocity step). Vx is met + * B" line is very short but drawable; is treated as a + * body only. + * F 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; +} diff --git a/src/avr/src/plan/planner.h b/src/avr/src/plan/planner.h new file mode 100644 index 0000000..bdc2c35 --- /dev/null +++ b/src/avr/src/plan/planner.h @@ -0,0 +1,86 @@ +/******************************************************************************\ + + 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 . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the software. If not, see + . + + For information regarding this software email: + "Joseph Coffland" + +\******************************************************************************/ + +#pragma once + + +#include "machine.h" // used for gcode_state_t +#include "buffer.h" +#include "util.h" +#include "config.h" + +#include + + +// 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); diff --git a/src/avr/src/plan/runtime.c b/src/avr/src/plan/runtime.c new file mode 100644 index 0000000..a9c1bf1 --- /dev/null +++ b/src/avr/src/plan/runtime.c @@ -0,0 +1,171 @@ +/******************************************************************************\ + + 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 . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the software. If not, see + . + + For information regarding this software email: + "Joseph Coffland" + +\******************************************************************************/ + +#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 +#include +#include +#include + + +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; +} diff --git a/src/avr/src/plan/runtime.h b/src/avr/src/plan/runtime.h new file mode 100644 index 0000000..a5c0fc2 --- /dev/null +++ b/src/avr/src/plan/runtime.h @@ -0,0 +1,58 @@ +/******************************************************************************\ + + 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 . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the software. If not, see + . + + For information regarding this software email: + "Joseph Coffland" + +\******************************************************************************/ + +#pragma once + +#include "status.h" + +#include + + +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[]); diff --git a/src/avr/src/plan/state.c b/src/avr/src/plan/state.c new file mode 100644 index 0000000..29c161b --- /dev/null +++ b/src/avr/src/plan/state.c @@ -0,0 +1,277 @@ +/******************************************************************************\ + + 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 . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the software. If not, see + . + + For information regarding this software email: + "Joseph Coffland" + +\******************************************************************************/ + +#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 + + +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); + } + } +} diff --git a/src/avr/src/plan/state.h b/src/avr/src/plan/state.h new file mode 100644 index 0000000..a287c42 --- /dev/null +++ b/src/avr/src/plan/state.h @@ -0,0 +1,95 @@ +/******************************************************************************\ + + 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 . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the software. If not, see + . + + For information regarding this software email: + "Joseph Coffland" + +\******************************************************************************/ + +#pragma once + +#include "pgmspace.h" + +#include + + +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(); diff --git a/src/avr/src/plan/velocity_curve.c b/src/avr/src/plan/velocity_curve.c new file mode 100644 index 0000000..7ef1009 --- /dev/null +++ b/src/avr/src/plan/velocity_curve.c @@ -0,0 +1,75 @@ +/******************************************************************************\ + + 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 . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the software. If not, see + . + + For information regarding this software email: + "Joseph Coffland" + +\******************************************************************************/ + +#include "velocity_curve.h" + +#include + + +/// 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; +} diff --git a/src/avr/src/plan/velocity_curve.h b/src/avr/src/plan/velocity_curve.h new file mode 100644 index 0000000..8d1e497 --- /dev/null +++ b/src/avr/src/plan/velocity_curve.h @@ -0,0 +1,32 @@ +/******************************************************************************\ + + 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 . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the software. If not, see + . + + For information regarding this software email: + "Joseph Coffland" + +\******************************************************************************/ + +#pragma once + +float velocity_curve(float Vi, float Vt, float t); diff --git a/src/avr/src/pwm_spindle.c b/src/avr/src/pwm_spindle.c new file mode 100644 index 0000000..305251a --- /dev/null +++ b/src/avr/src/pwm_spindle.c @@ -0,0 +1,151 @@ +/******************************************************************************\ + + 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 . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the software. If not, see + . + + For information regarding this software email: + "Joseph Coffland" + +\******************************************************************************/ + + +#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;} diff --git a/src/avr/src/pwm_spindle.h b/src/avr/src/pwm_spindle.h new file mode 100644 index 0000000..10006a6 --- /dev/null +++ b/src/avr/src/pwm_spindle.h @@ -0,0 +1,35 @@ +/******************************************************************************\ + + 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 . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the software. If not, see + . + + For information regarding this software email: + "Joseph Coffland" + +\******************************************************************************/ + +#pragma once + +#include "machine.h" + + +void pwm_spindle_init(); +void pwm_spindle_set(spindle_mode_t mode, float speed); +void pwm_spindle_stop(); diff --git a/src/avr/src/report.c b/src/avr/src/report.c new file mode 100644 index 0000000..bb04f63 --- /dev/null +++ b/src/avr/src/report.c @@ -0,0 +1,57 @@ +/******************************************************************************\ + + 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 . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the software. If not, see + . + + For information regarding this software email: + "Joseph Coffland" + +\******************************************************************************/ + +#include "report.h" +#include "config.h" +#include "usart.h" +#include "rtc.h" +#include "vars.h" +#include "pgmspace.h" + +#include +#include + + +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; + } +} diff --git a/src/avr/src/report.h b/src/avr/src/report.h new file mode 100644 index 0000000..dd0106c --- /dev/null +++ b/src/avr/src/report.h @@ -0,0 +1,35 @@ +/******************************************************************************\ + + 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 . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the software. If not, see + . + + For information regarding this software email: + "Joseph Coffland" + +\******************************************************************************/ + +#pragma once + + +#include "status.h" + +void report_request(); +void report_request_full(); +void report_callback(); diff --git a/src/avr/src/ringbuf.def b/src/avr/src/ringbuf.def new file mode 100644 index 0000000..5228fd0 --- /dev/null +++ b/src/avr/src/ringbuf.def @@ -0,0 +1,158 @@ +/******************************************************************************\ + + 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 . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the software. If not, see + . + + For information regarding this software email: + "Joseph Coffland" + +\******************************************************************************/ + +/* 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 _init(); + * int _empty(); + * int _full(); + * _peek(); + * void _pop(); + * void _push( data); + * + * Where is defined by RING_BUF_NAME and 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 + +#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 diff --git a/src/avr/src/rtc.c b/src/avr/src/rtc.c new file mode 100644 index 0000000..e09c584 --- /dev/null +++ b/src/avr/src/rtc.c @@ -0,0 +1,73 @@ +/******************************************************************************\ + + 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 . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the software. If not, see + . + + For information regarding this software email: + "Joseph Coffland" + +\******************************************************************************/ + +#include "rtc.h" + +#include "switch.h" +#include "huanyang.h" +#include "motor.h" + +#include +#include + +#include + + +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);} diff --git a/src/avr/src/rtc.h b/src/avr/src/rtc.h new file mode 100644 index 0000000..2c040bb --- /dev/null +++ b/src/avr/src/rtc.h @@ -0,0 +1,38 @@ +/******************************************************************************\ + + 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 . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the software. If not, see + . + + For information regarding this software email: + "Joseph Coffland" + +\******************************************************************************/ + +#pragma once + + +#include +#include + +void rtc_init(); +uint32_t rtc_get_time(); +int32_t rtc_diff(uint32_t t); +bool rtc_expired(uint32_t t); diff --git a/src/avr/src/spindle.c b/src/avr/src/spindle.c new file mode 100644 index 0000000..d6ae802 --- /dev/null +++ b/src/avr/src/spindle.c @@ -0,0 +1,124 @@ +/******************************************************************************\ + + 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 . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the software. If not, see + . + + For information regarding this software email: + "Joseph Coffland" + +\******************************************************************************/ + +#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"); +} diff --git a/src/avr/src/spindle.h b/src/avr/src/spindle.h new file mode 100644 index 0000000..9dbe5a5 --- /dev/null +++ b/src/avr/src/spindle.h @@ -0,0 +1,40 @@ +/******************************************************************************\ + + 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 . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the software. If not, see + . + + For information regarding this software email: + "Joseph Coffland" + +\******************************************************************************/ + +#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(); diff --git a/src/avr/src/status.c b/src/avr/src/status.c new file mode 100644 index 0000000..8d1a0a8 --- /dev/null +++ b/src/avr/src/status.c @@ -0,0 +1,123 @@ +/******************************************************************************\ + + 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 . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the software. If not, see + . + + For information regarding this software email: + "Joseph Coffland" + +\******************************************************************************/ + +#include "status.h" +#include "estop.h" +#include "usart.h" + +#include +#include + + +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; +} diff --git a/src/avr/src/status.h b/src/avr/src/status.h new file mode 100644 index 0000000..5e2b91d --- /dev/null +++ b/src/avr/src/status.h @@ -0,0 +1,99 @@ +/******************************************************************************\ + + 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 . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the software. If not, see + . + + For information regarding this software email: + "Joseph Coffland" + +\******************************************************************************/ + +#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 diff --git a/src/avr/src/stepper.c b/src/avr/src/stepper.c new file mode 100644 index 0000000..2396f07 --- /dev/null +++ b/src/avr/src/stepper.c @@ -0,0 +1,242 @@ +/******************************************************************************\ + + 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 . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the software. If not, see + . + + For information regarding this software email: + "Joseph Coffland" + +\******************************************************************************/ + +#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 +#include + + +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 +} diff --git a/src/avr/src/stepper.h b/src/avr/src/stepper.h new file mode 100644 index 0000000..44f2d45 --- /dev/null +++ b/src/avr/src/stepper.h @@ -0,0 +1,43 @@ +/******************************************************************************\ + + 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 . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the software. If not, see + . + + For information regarding this software email: + "Joseph Coffland" + +\******************************************************************************/ + +#pragma once + +#include "status.h" + +#include +#include + + +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); diff --git a/src/avr/src/switch.c b/src/avr/src/switch.c new file mode 100644 index 0000000..489c554 --- /dev/null +++ b/src/avr/src/switch.c @@ -0,0 +1,195 @@ +/******************************************************************************\ + + 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 . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the software. If not, see + . + + For information regarding this software email: + "Joseph Coffland" + +\******************************************************************************/ + +#include "switch.h" +#include "config.h" + +#include + +#include + + +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);} diff --git a/src/avr/src/switch.h b/src/avr/src/switch.h new file mode 100644 index 0000000..aad8ad1 --- /dev/null +++ b/src/avr/src/switch.h @@ -0,0 +1,69 @@ +/******************************************************************************\ + + 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 . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the software. If not, see + . + + For information regarding this software email: + "Joseph Coffland" + +\******************************************************************************/ + +#pragma once + + +#include "config.h" + +#include +#include + + +// 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); diff --git a/src/avr/src/usart.c b/src/avr/src/usart.c new file mode 100644 index 0000000..be88d8b --- /dev/null +++ b/src/avr/src/usart.c @@ -0,0 +1,282 @@ +/******************************************************************************\ + + 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 . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the software. If not, see + . + + For information regarding this software email: + "Joseph Coffland" + +\******************************************************************************/ + +#include "usart.h" +#include "cpp_magic.h" +#include "config.h" + +#include +#include + +#include +#include + +// 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(); +} diff --git a/src/avr/src/usart.h b/src/avr/src/usart.h new file mode 100644 index 0000000..915d362 --- /dev/null +++ b/src/avr/src/usart.h @@ -0,0 +1,77 @@ +/******************************************************************************\ + + 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 . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the software. If not, see + . + + For information regarding this software email: + "Joseph Coffland" + +\******************************************************************************/ + +#pragma once + + +#include +#include + +#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();} diff --git a/src/avr/src/util.c b/src/avr/src/util.c new file mode 100644 index 0000000..1a76542 --- /dev/null +++ b/src/avr/src/util.c @@ -0,0 +1,51 @@ +/******************************************************************************\ + + 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 . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the software. If not, see + . + + For information regarding this software email: + "Joseph Coffland" + +\******************************************************************************/ + +#include "util.h" + +#include + + +/// 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; +} diff --git a/src/avr/src/util.h b/src/avr/src/util.h new file mode 100644 index 0000000..3ec44b8 --- /dev/null +++ b/src/avr/src/util.h @@ -0,0 +1,80 @@ +/******************************************************************************\ + + 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 . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the software. If not, see + . + + For information regarding this software email: + "Joseph Coffland" + +\******************************************************************************/ + +#pragma once + + +#include "config.h" + +#include +#include +#include +#include + + +// 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 diff --git a/src/avr/src/varcb.c b/src/avr/src/varcb.c new file mode 100644 index 0000000..1a59471 --- /dev/null +++ b/src/avr/src/varcb.c @@ -0,0 +1,141 @@ +/******************************************************************************\ + + 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 . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the software. If not, see + . + + For information regarding this software email: + "Joseph Coffland" + +\******************************************************************************/ + +#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()); +} diff --git a/src/avr/src/vars.c b/src/avr/src/vars.c new file mode 100644 index 0000000..815bcc1 --- /dev/null +++ b/src/avr/src/vars.c @@ -0,0 +1,456 @@ +/******************************************************************************\ + + 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 . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the software. If not, see + . + + For information regarding this software email: + "Joseph Coffland" + +\******************************************************************************/ + +#include "vars.h" + +#include "cpp_magic.h" +#include "status.h" +#include "hardware.h" +#include "config.h" +#include "axis.h" +#include "pgmspace.h" + +#include +#include +#include +#include +#include +#include +#include +#include + + +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 = ""; +#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); +} diff --git a/src/avr/src/vars.def b/src/avr/src/vars.def new file mode 100644 index 0000000..8346d7c --- /dev/null +++ b/src/avr/src/vars.def @@ -0,0 +1,141 @@ +/******************************************************************************\ + + 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 . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the software. If not, see + . + + For information regarding this software email: + "Joseph Coffland" + +\******************************************************************************/ + +#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") diff --git a/src/avr/src/vars.h b/src/avr/src/vars.h new file mode 100644 index 0000000..4ad37b7 --- /dev/null +++ b/src/avr/src/vars.h @@ -0,0 +1,45 @@ +/******************************************************************************\ + + 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 . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the software. If not, see + . + + For information regarding this software email: + "Joseph Coffland" + +\******************************************************************************/ + +#pragma once + +#include "status.h" + +#include + +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(); diff --git a/src/avr/src/vars.json.in b/src/avr/src/vars.json.in new file mode 100644 index 0000000..03195c7 --- /dev/null +++ b/src/avr/src/vars.json.in @@ -0,0 +1,15 @@ +#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 + "_": {} +} diff --git a/src/avr/test/Makefile b/src/avr/test/Makefile new file mode 100644 index 0000000..cb7d858 --- /dev/null +++ b/src/avr/test/Makefile @@ -0,0 +1,37 @@ +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/*) diff --git a/src/avr/test/arc.gc b/src/avr/test/arc.gc new file mode 100644 index 0000000..72b612f --- /dev/null +++ b/src/avr/test/arc.gc @@ -0,0 +1,10 @@ +$resume + +$0vm=10000 +$1vm=10000 +$2vm=10000 +$0jm=50 +$1jm=50 +$2jm=50 + +G3 I2 J2 F10000 diff --git a/src/avr/test/hal.c b/src/avr/test/hal.c new file mode 100644 index 0000000..45ecfde --- /dev/null +++ b/src/avr/test/hal.c @@ -0,0 +1,232 @@ +/******************************************************************************\ + + 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 . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the software. If not, see + . + + For information regarding this software email: + "Joseph Coffland" + +\******************************************************************************/ + +#include "status.h" +#include "spindle.h" +#include "i2c.h" +#include "cpp_magic.h" +#include "plan/buffer.h" + +#include +#include +#include +#include + + +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); +} diff --git a/src/avr/test/line.gc b/src/avr/test/line.gc new file mode 100644 index 0000000..20695cc --- /dev/null +++ b/src/avr/test/line.gc @@ -0,0 +1,16 @@ +$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 diff --git a/src/avr/test/planner-test.c b/src/avr/test/planner-test.c new file mode 100644 index 0000000..78206ee --- /dev/null +++ b/src/avr/test/planner-test.c @@ -0,0 +1,82 @@ +/******************************************************************************\ + + 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 . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the software. If not, see + . + + For information regarding this software email: + "Joseph Coffland" + +\******************************************************************************/ + +#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 +#include + + +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; +} diff --git a/src/avr/test/plot.py b/src/avr/test/plot.py new file mode 100755 index 0000000..28fc6c5 --- /dev/null +++ b/src/avr/test/plot.py @@ -0,0 +1,90 @@ +#!/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() diff --git a/src/avr/test/short.gc b/src/avr/test/short.gc new file mode 100644 index 0000000..1ba1685 --- /dev/null +++ b/src/avr/test/short.gc @@ -0,0 +1,59 @@ +$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